23 #include "robot_memory_thread.h"
25 #include "interfaces/RobotMemoryInterface.h"
27 #include <core/threading/mutex.h>
28 #include <core/threading/mutex_locker.h>
29 #ifdef USE_TIMETRACKER
30 # include <utils/time/tracker.h>
32 #include <utils/time/tracker_macros.h>
33 #include <utils/time/wait.h>
35 #include <bsoncxx/json.hpp>
49 :
Thread(
"RobotMemoryThread",
Thread::OPMODE_CONTINUOUS),
72 int loop_time_microsec;
74 float loop_interval =
config->
get_float(
"/plugins/robot-memory/loop-interval");
75 loop_time_microsec = (int)loop_interval * 1e6;
78 int main_loop_time =
config->
get_int(
"/fawkes/mainapp/desired_loop_time");
79 if (main_loop_time > 0) {
80 loop_time_microsec = main_loop_time;
83 loop_time_microsec = 1e5;
88 #ifdef USE_TIMETRACKER
91 ttc_msgproc_ = tt_->add_class(
"Message Processing");
92 ttc_rmloop_ = tt_->add_class(
"Robot Memory Processing Loop");
99 delete blackboard_computable;
100 delete transform_computable;
104 #ifdef USE_TIMETRACKER
112 TIMETRACK_START(ttc_msgproc_);
115 while (!robot_memory->rm_if_->msgq_empty()) {
116 if (robot_memory->rm_if_->msgq_first_is<RobotMemoryInterface::QueryMessage>()) {
117 RobotMemoryInterface::QueryMessage *msg =
118 (RobotMemoryInterface::QueryMessage *)robot_memory->rm_if_->msgq_first();
119 std::string query = msg->query();
120 mongocxx::cursor res = robot_memory->
query(bsoncxx::from_json(query), msg->collection());
122 std::string result =
"Result of query " + query +
":\n";
124 auto doc = res.begin();
125 if (doc == res.end()) {
128 result += bsoncxx::to_json(*doc) +
"\n";
131 robot_memory->rm_if_->set_result(result.c_str());
132 }
else if (robot_memory->rm_if_->msgq_first_is<RobotMemoryInterface::InsertMessage>()) {
133 RobotMemoryInterface::InsertMessage *msg =
134 (RobotMemoryInterface::InsertMessage *)robot_memory->rm_if_->msgq_first();
135 robot_memory->
insert(msg->insert()), msg->collection();
136 }
else if (robot_memory->rm_if_->msgq_first_is<RobotMemoryInterface::UpdateMessage>()) {
137 RobotMemoryInterface::UpdateMessage *msg =
138 (RobotMemoryInterface::UpdateMessage *)robot_memory->rm_if_->msgq_first();
139 robot_memory->
update(bsoncxx::from_json(msg->query()),
140 bsoncxx::from_json(msg->update()),
142 }
else if (robot_memory->rm_if_->msgq_first_is<RobotMemoryInterface::RemoveMessage>()) {
143 RobotMemoryInterface::RemoveMessage *msg =
144 (RobotMemoryInterface::RemoveMessage *)robot_memory->rm_if_->msgq_first();
145 robot_memory->
remove(bsoncxx::from_json(msg->query()), msg->collection());
150 robot_memory->rm_if_->msgq_pop();
152 TIMETRACK_END(ttc_msgproc_);
154 TIMETRACK_START(ttc_rmloop_);
155 robot_memory->loop();
156 TIMETRACK_END(ttc_rmloop_);
157 #ifdef USE_TIMETRACKER
158 if (++tt_loopcount_ % 5 == 0) {
159 tt_->print_to_stdout();