Fawkes API Fawkes Development Version
robot_memory_thread.cpp
1
2/***************************************************************************
3 * robot_memory_thread.cpp - Robot Memory thread
4 *
5 * Created: Sun May 01 13:41:45 2016
6 * Copyright 2016 Frederik Zwilling
7 * 2017 Tim Niemueller [www.niemueller.de]
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "robot_memory_thread.h"
24
25#include "interfaces/RobotMemoryInterface.h"
26
27#include <core/threading/mutex.h>
28#include <core/threading/mutex_locker.h>
29#ifdef USE_TIMETRACKER
30# include <utils/time/tracker.h>
31#endif
32#include <utils/time/tracker_macros.h>
33#include <utils/time/wait.h>
34
35#include <bsoncxx/json.hpp>
36#include <chrono>
37#include <memory>
38#include <string>
39
40using namespace fawkes;
41
42/** @class RobotMemoryThread "robot_memory_thread.h"
43 * Thread that provides a robot memory with MongoDB
44 * @author Frederik Zwilling
45 */
46
47/** Constructor for thread */
49: Thread("RobotMemoryThread", Thread::OPMODE_CONTINUOUS),
50 AspectProviderAspect(&robot_memory_inifin_)
51{
52}
53
54/** Destructor. */
56{
57}
58
59void
61{
62 //init RobotMemory itself
64 robot_memory->init();
65 //prepare aspect initializer
66 robot_memory_inifin_.set_robot_memory(robot_memory);
67
68 //register computables
69 blackboard_computable = new BlackboardComputable(robot_memory, blackboard, logger, config);
70 transform_computable = new TransformComputable(robot_memory, tf_listener, logger, config);
71
72 int loop_time_microsec;
73 try {
74 float loop_interval = config->get_float("/plugins/robot-memory/loop-interval");
75 loop_time_microsec = (int)loop_interval * 1e6;
76
77 } catch (Exception &e) {
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;
81 } else {
82 // use default of 0.1s
83 loop_time_microsec = 1e5;
84 }
85 }
86 timewait_ = new TimeWait(clock, loop_time_microsec);
87
88#ifdef USE_TIMETRACKER
89 tt_ = new TimeTracker();
90 tt_loopcount_ = 0;
91 ttc_msgproc_ = tt_->add_class("Message Processing");
92 ttc_rmloop_ = tt_->add_class("Robot Memory Processing Loop");
93#endif
94}
95
96void
98{
99 delete blackboard_computable;
100 delete transform_computable;
101 robot_memory_inifin_.set_robot_memory(NULL);
102 delete robot_memory;
103 delete timewait_;
104#ifdef USE_TIMETRACKER
105 delete tt_;
106#endif
107}
108
109void
111{
112 TIMETRACK_START(ttc_msgproc_);
113 timewait_->mark_start();
114 // process interface messages
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());
121 //output result
122 std::string result = "Result of query " + query + ":\n";
123 while (true) {
124 auto doc = res.begin();
125 if (doc == res.end()) {
126 break;
127 }
128 result += bsoncxx::to_json(*doc) + "\n";
129 }
130 logger->log_info(name(), "%s", result.c_str());
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()),
141 msg->collection());
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());
146 } else {
147 logger->log_warn(name(), "Unknown message received");
148 }
149
150 robot_memory->rm_if_->msgq_pop();
151 }
152 TIMETRACK_END(ttc_msgproc_);
153
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();
160 }
161#endif
162
163 timewait_->wait_systime();
164}
Computable providing access to blackboard interfaces.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual ~RobotMemoryThread()
Destructor.
RobotMemoryThread()
Constructor for thread.
virtual void finalize()
Finalize the thread.
Access to the robot memory based on mongodb.
Definition: robot_memory.h:47
mongocxx::cursor query(bsoncxx::document::view query, const std::string &collection_name="", mongocxx::options::find query_options=mongocxx::options::find())
Query information from the robot memory.
int remove(const bsoncxx::document::view &query, const std::string &collection="")
Remove documents from the robot memory.
int insert(bsoncxx::document::view, const std::string &collection="")
Inserts a document into the robot memory.
int update(const bsoncxx::document::view &query, const bsoncxx::document::view &update, const std::string &collection="", bool upsert=false)
Updates documents in the robot memory.
Computable proving positions in other frames by using transforms.
Thread aspect provide a new aspect.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
MongoDBConnCreator * mongodb_connmgr
Connection manager to retrieve more client connections from if necessary.
Definition: mongodb.h:55
void set_robot_memory(RobotMemory *robot_memory)
Set the reference to the robot memory for the aspect.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Time tracking utility.
Definition: tracker.h:37
Time wait utility.
Definition: wait.h:33
void mark_start()
Mark start of loop.
Definition: wait.cpp:68
void wait_systime()
Wait until minimum loop time has been reached in real time.
Definition: wait.cpp:96
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Fawkes library namespace.