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 
30 #include <chrono>
31 #include <memory>
32 
33 using namespace fawkes;
34 
35 /** @class RobotMemoryThread "robot_memory_thread.h"
36  * Thread that provides a robot memory with MongoDB
37  * @author Frederik Zwilling
38  */
39 
40 /** Constructor for thread */
42 : Thread("RobotMemoryThread", Thread::OPMODE_WAITFORWAKEUP),
43  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
44  AspectProviderAspect(&robot_memory_inifin_)
45 {
46 }
47 
48 /** Destructor. */
50 {
51 }
52 
53 void
55 {
56  //init RobotMemory itself
57  robot_memory = new RobotMemory(config, logger, clock, mongodb_connmgr, blackboard);
58  robot_memory->init();
59  //prepare aspect initializer
60  robot_memory_inifin_.set_robot_memory(robot_memory);
61 
62  //register computables
63  blackboard_computable = new BlackboardComputable(robot_memory, blackboard, logger, config);
64  transform_computable = new TransformComputable(robot_memory, tf_listener, logger, config);
65 }
66 
67 void
69 {
70  delete blackboard_computable;
71  delete transform_computable;
72  robot_memory_inifin_.set_robot_memory(NULL);
73  delete robot_memory;
74 }
75 
76 void
78 {
79  // process interface messages
80  while (!robot_memory->rm_if_->msgq_empty()) {
81  if (robot_memory->rm_if_->msgq_first_is<RobotMemoryInterface::QueryMessage>()) {
82  RobotMemoryInterface::QueryMessage *msg =
83  (RobotMemoryInterface::QueryMessage *)robot_memory->rm_if_->msgq_first();
84  QResCursor res = robot_memory->query(msg->query(), msg->collection());
85  //output result
86  std::string query = msg->query();
87  std::string result = "Result of query " + query + ":\n";
88  while (res->more()) {
89  mongo::BSONObj doc = res->next();
90  result += doc.toString() + "\n";
91  }
92  logger->log_info(name(), "%s", result.c_str());
93  robot_memory->rm_if_->set_result(result.c_str());
94  } else if (robot_memory->rm_if_->msgq_first_is<RobotMemoryInterface::InsertMessage>()) {
95  RobotMemoryInterface::InsertMessage *msg =
96  (RobotMemoryInterface::InsertMessage *)robot_memory->rm_if_->msgq_first();
97  robot_memory->insert(msg->insert()), msg->collection();
98  } else if (robot_memory->rm_if_->msgq_first_is<RobotMemoryInterface::UpdateMessage>()) {
99  RobotMemoryInterface::UpdateMessage *msg =
100  (RobotMemoryInterface::UpdateMessage *)robot_memory->rm_if_->msgq_first();
101  robot_memory->update(msg->query(), msg->update(), msg->collection());
102  } else if (robot_memory->rm_if_->msgq_first_is<RobotMemoryInterface::RemoveMessage>()) {
103  RobotMemoryInterface::RemoveMessage *msg =
104  (RobotMemoryInterface::RemoveMessage *)robot_memory->rm_if_->msgq_first();
105  robot_memory->remove(msg->query(), msg->collection());
106  } else {
107  logger->log_warn(name(), "Unknown message received");
108  }
109 
110  robot_memory->rm_if_->msgq_pop();
111  }
112 
113  robot_memory->loop();
114 }
QResCursor query(mongo::Query query, const std::string &collection="")
Query information from the robot memory.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
RobotMemoryThread()
Constructor for thread.
int remove(mongo::Query query, const std::string &collection="")
Remove documents from the robot memory.
Fawkes library namespace.
int update(mongo::Query query, mongo::BSONObj update, const std::string &collection="", bool upsert=false)
Updates documents in the robot memory.
Computable providing access to blackboard interfaces.
virtual void finalize()
Finalize the thread.
MongoDBConnCreator * mongodb_connmgr
Connection manager to retrieve more client connections from if necessary.
Definition: mongodb.h:56
Thread class encapsulation of pthreads.
Definition: thread.h:45
Computable proving positions in other frames by using transforms.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
virtual void init()
Initialize the thread.
int insert(mongo::BSONObj obj, const std::string &collection="")
Inserts a document into the robot memory.
Thread aspect to use blocked timing.
void set_robot_memory(RobotMemory *robot_memory)
Set the reference to the robot memory for the aspect.
virtual void loop()
Code to execute in the thread.
const char * name() const
Get name of thread.
Definition: thread.h:100
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Thread aspect provide a new aspect.
Access to the robot memory based on mongodb.
Definition: robot_memory.h:48
virtual ~RobotMemoryThread()
Destructor.
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44