Fawkes API  Fawkes Development Version
openrave-robot-memory_thread.cpp
1 
2 /***************************************************************************
3  * openrave-robot-memory_thread.cpp - openrave-robot-memory
4  *
5  * Created: Thu Nov 24 13:14:33 2016
6  * Copyright 2016 Frederik Zwilling
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "openrave-robot-memory_thread.h"
23 
24 #include <algorithm>
25 
26 using namespace fawkes;
27 using namespace mongo;
28 
29 /** @class OpenraveRobotMemoryThread 'openrave-robot-memory_thread.h'
30  * Creates an OpenRave Scene for motion planning from data in the robot memory
31  * @author Frederik Zwilling
32  */
33 
34 /** Constructor. */
36 : Thread("OpenraveRobotMemoryThread", Thread::OPMODE_WAITFORWAKEUP),
38 {
39 }
40 
41 void
43 {
45  config->get_string("plugins/openrave-robot-memory/openrave-if-name").c_str());
47  config->get_string("plugins/openrave-robot-memory/if-name").c_str());
48 }
49 
50 void
52 {
53  // process interface messages
54  while (!or_rm_if_->msgq_empty()) {
56  construct_scene();
57  } else {
58  logger->log_warn(name(), "Unknown message received");
59  }
60  or_rm_if_->msgq_pop();
61  }
62 }
63 
64 void
66 {
67 }
68 
69 void
70 OpenraveRobotMemoryThread::construct_scene()
71 {
72  logger->log_info(name(), "Constructing Scene");
73 
74  // add all object types by iterating over config paths
75  std::string prefix = "plugins/openrave-robot-memory/object-types/";
76  std::unique_ptr<Configuration::ValueIterator> object_types(config->search(prefix.c_str()));
77  while (object_types->next()) {
78  //object_types->next() yields the whole path, so we have to get the right prefix
79  std::string cfg_name = std::string(object_types->path()).substr(prefix.length());
80  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
81  //don't use the same prefix again
82  if (std::find(added_object_types_.begin(), added_object_types_.end(), cfg_name)
83  != added_object_types_.end())
84  continue;
85  added_object_types_.push_back(cfg_name);
86  logger->log_info(name(), "Adding object type: %s", cfg_name.c_str());
87  std::string cfg_prefix = prefix + cfg_name + "/";
88  std::string collection = config->get_string(cfg_prefix + "collection");
89 
90  //construct query
91  BSONObjBuilder query_builder;
92  query_builder.appendElements(fromjson(config->get_string(cfg_prefix + "query")));
93  query_builder << "frame"
94  << "base_link"
95  << "allow_tf" << true;
96  BSONObj query = query_builder.obj();
97  logger->log_info(name(), "Querying: %s", query.toString().c_str());
98  QResCursor cur = robot_memory->query(query, collection);
99  while (cur->more()) {
100  BSONObj block = cur->next();
101  //logger->log_info(name(), "Adding: %s", cfg_prefix.c_str(), block.toString().c_str());
102  std::string block_name = block.getStringField(config->get_string(cfg_prefix + "name-key"));
103  if (std::find(added_objects_.begin(), added_objects_.end(), block_name)
104  == added_objects_.end()) {
105  //add new object
106  logger->log_info(name(), "adding %s", block_name.c_str());
108  add_msg.set_name(block_name.c_str());
109  add_msg.set_path(config->get_string(cfg_prefix + "model-path").c_str());
110  openrave_if_->msgq_enqueue_copy(&add_msg);
111  added_objects_.push_back(block_name);
112  }
113  //move object to right position
115  move_msg.set_name(block_name.c_str());
116  move_msg.set_x(block.getField("translation").Array()[0].Double());
117  move_msg.set_y(block.getField("translation").Array()[1].Double());
118  move_msg.set_z(block.getField("translation").Array()[2].Double());
119  openrave_if_->msgq_enqueue_copy(&move_msg);
120  //rotate object
122  rotate_msg.set_name(block_name.c_str());
123  rotate_msg.set_x(block.getField("rotation").Array()[0].Double());
124  rotate_msg.set_y(block.getField("rotation").Array()[1].Double());
125  rotate_msg.set_z(block.getField("rotation").Array()[2].Double());
126  rotate_msg.set_w(block.getField("rotation").Array()[3].Double());
127  openrave_if_->msgq_enqueue_copy(&rotate_msg);
128  }
129  }
130  added_object_types_.clear();
131  logger->log_info(name(), "Finished Constructing Scene");
132 }
void set_path(const char *new_path)
Set path value.
QResCursor query(mongo::Query query, const std::string &collection="")
Query information from the robot memory.
RotateObjectQuatMessage Fawkes BlackBoard Interface Message.
void set_name(const char *new_name)
Set name value.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1026
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void init()
Initialize the thread.
unsigned int msgq_enqueue_copy(Message *message)
Enqueue copy of message at end of queue.
Definition: interface.cpp:917
Fawkes library namespace.
ConstructSceneMessage Fawkes BlackBoard Interface Message.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
Thread class encapsulation of pthreads.
Definition: thread.h:45
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
virtual void finalize()
Finalize the thread.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1179
void set_y(const float new_y)
Set y value.
virtual void loop()
Code to execute in the thread.
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_z(const float new_z)
Set z value.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
void set_x(const float new_x)
Set x value.
void set_name(const char *new_name)
Set name value.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
OpenraveRobotMemoryInterface Fawkes BlackBoard Interface.
void set_name(const char *new_name)
Set name value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
MoveObjectMessage Fawkes BlackBoard Interface Message.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
OpenRaveInterface Fawkes BlackBoard Interface.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
AddObjectMessage Fawkes BlackBoard Interface Message.