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 
40 using 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 
59 void
61 {
62  //init RobotMemory itself
63  robot_memory = new RobotMemory(config, logger, clock, mongodb_connmgr, blackboard);
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 
96 void
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 
109 void
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.