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 #include <bsoncxx/builder/basic/document.hpp>
26 #include <bsoncxx/json.hpp>
27 
28 using namespace fawkes;
29 using namespace bsoncxx;
30 
31 /** @class OpenraveRobotMemoryThread 'openrave-robot-memory_thread.h'
32  * Creates an OpenRave Scene for motion planning from data in the robot memory
33  * @author Frederik Zwilling
34  */
35 
36 /** Constructor. */
38 : Thread("OpenraveRobotMemoryThread", Thread::OPMODE_WAITFORWAKEUP),
40 {
41 }
42 
43 void
45 {
47  config->get_string("plugins/openrave-robot-memory/openrave-if-name").c_str());
49  config->get_string("plugins/openrave-robot-memory/if-name").c_str());
50 }
51 
52 void
54 {
55  // process interface messages
56  while (!or_rm_if_->msgq_empty()) {
58  construct_scene();
59  } else {
60  logger->log_warn(name(), "Unknown message received");
61  }
62  or_rm_if_->msgq_pop();
63  }
64 }
65 
66 void
68 {
69 }
70 
71 void
72 OpenraveRobotMemoryThread::construct_scene()
73 {
74  logger->log_info(name(), "Constructing Scene");
75 
76  // add all object types by iterating over config paths
77  std::string prefix = "plugins/openrave-robot-memory/object-types/";
78  std::unique_ptr<Configuration::ValueIterator> object_types(config->search(prefix.c_str()));
79  while (object_types->next()) {
80  //object_types->next() yields the whole path, so we have to get the right prefix
81  std::string cfg_name = std::string(object_types->path()).substr(prefix.length());
82  cfg_name = cfg_name.substr(0, cfg_name.find("/"));
83  //don't use the same prefix again
84  if (std::find(added_object_types_.begin(), added_object_types_.end(), cfg_name)
85  != added_object_types_.end())
86  continue;
87  added_object_types_.push_back(cfg_name);
88  logger->log_info(name(), "Adding object type: %s", cfg_name.c_str());
89  std::string cfg_prefix = prefix + cfg_name + "/";
90  std::string collection = config->get_string(cfg_prefix + "collection");
91 
92  //construct query
93  using namespace bsoncxx::builder;
94  basic::document query;
95  query.append(builder::concatenate(from_json(config->get_string(cfg_prefix + "query"))));
96  query.append(basic::kvp("frame", "base_link"));
97  query.append(basic::kvp("allow_tf", true));
98  logger->log_info(name(), "Querying: %s", to_json(query).c_str());
99  auto cursor = robot_memory->query(query, collection);
100  //while (cur->more()) {
101  for (auto block : cursor) {
102  //logger->log_info(name(), "Adding: %s", cfg_prefix.c_str(), to_json(block).c_str());
103  std::string block_name =
104  block[config->get_string(cfg_prefix + "name-key")].get_utf8().value.to_string();
105  if (std::find(added_objects_.begin(), added_objects_.end(), block_name)
106  == added_objects_.end()) {
107  //add new object
108  logger->log_info(name(), "adding %s", block_name.c_str());
110  add_msg.set_name(block_name.c_str());
111  add_msg.set_path(config->get_string(cfg_prefix + "model-path").c_str());
112  openrave_if_->msgq_enqueue_copy(&add_msg);
113  added_objects_.push_back(block_name);
114  }
115  //move object to right position
117  move_msg.set_name(block_name.c_str());
118  array::view translation = block["translation"].get_array();
119  move_msg.set_x(translation[0].get_double());
120  move_msg.set_y(translation[1].get_double());
121  move_msg.set_z(translation[2].get_double());
122  openrave_if_->msgq_enqueue_copy(&move_msg);
123  //rotate object
125  rotate_msg.set_name(block_name.c_str());
126  array::view rotation = block["rotation"].get_array();
127  rotate_msg.set_x(rotation[0].get_double());
128  rotate_msg.set_y(rotation[1].get_double());
129  rotate_msg.set_z(rotation[2].get_double());
130  rotate_msg.set_w(rotation[3].get_double());
131  openrave_if_->msgq_enqueue_copy(&rotate_msg);
132  }
133  }
134  added_object_types_.clear();
135  logger->log_info(name(), "Finished Constructing Scene");
136 }
fawkes::Interface::msgq_first_is
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:313
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
fawkes::OpenRaveInterface::RotateObjectQuatMessage::set_x
void set_x(const float new_x)
Set x value.
Definition: OpenRaveInterface.cpp:1164
OpenraveRobotMemoryThread::init
virtual void init()
Initialize the thread.
Definition: openrave-robot-memory_thread.cpp:43
fawkes::OpenRaveInterface::MoveObjectMessage::set_y
void set_y(const float new_y)
Set y value.
Definition: OpenRaveInterface.cpp:996
fawkes::OpenraveRobotMemoryInterface::ConstructSceneMessage
Definition: OpenraveRobotMemoryInterface.h:64
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
OpenraveRobotMemoryThread::finalize
virtual void finalize()
Finalize the thread.
Definition: openrave-robot-memory_thread.cpp:66
OpenraveRobotMemoryThread::OpenraveRobotMemoryThread
OpenraveRobotMemoryThread()
Constructor.
Definition: openrave-robot-memory_thread.cpp:36
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
fawkes::OpenRaveInterface::MoveObjectMessage
Definition: OpenRaveInterface.h:237
fawkes::Thread::name
const char * name() const
Definition: thread.h:99
fawkes::OpenRaveInterface::AddObjectMessage::set_path
void set_path(const char *new_path)
Set path value.
Definition: OpenRaveInterface.cpp:427
fawkes::Configuration::search
virtual ValueIterator * search(const char *path)=0
fawkes::OpenRaveInterface::MoveObjectMessage::set_name
void set_name(const char *new_name)
Set name value.
Definition: OpenRaveInterface.cpp:935
fawkes::OpenRaveInterface::AddObjectMessage::set_name
void set_name(const char *new_name)
Set name value.
Definition: OpenRaveInterface.cpp:396
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:50
fawkes::OpenRaveInterface::RotateObjectQuatMessage::set_name
void set_name(const char *new_name)
Set name value.
Definition: OpenRaveInterface.cpp:1133
fawkes
OpenraveRobotMemoryThread::loop
virtual void loop()
Code to execute in the thread.
Definition: openrave-robot-memory_thread.cpp:52
fawkes::RobotMemoryAspect::robot_memory
RobotMemory * robot_memory
RobotMemory object for storing and querying information.
Definition: robot_memory_aspect.h:55
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::OpenRaveInterface::MoveObjectMessage::set_z
void set_z(const float new_z)
Set z value.
Definition: OpenRaveInterface.cpp:1026
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:50
fawkes::OpenRaveInterface
Definition: OpenRaveInterface.h:37
fawkes::OpenRaveInterface::RotateObjectQuatMessage
Definition: OpenRaveInterface.h:274
fawkes::Thread
Definition: thread.h:44
RobotMemory::query
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.
Definition: robot_memory.cpp:201
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:47
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::OpenRaveInterface::RotateObjectQuatMessage::set_z
void set_z(const float new_z)
Set z value.
Definition: OpenRaveInterface.cpp:1224
fawkes::OpenRaveInterface::RotateObjectQuatMessage::set_y
void set_y(const float new_y)
Set y value.
Definition: OpenRaveInterface.cpp:1194
fawkes::OpenRaveInterface::AddObjectMessage
Definition: OpenRaveInterface.h:89
fawkes::OpenRaveInterface::MoveObjectMessage::set_x
void set_x(const float new_x)
Set x value.
Definition: OpenRaveInterface.cpp:966
fawkes::OpenRaveInterface::RotateObjectQuatMessage::set_w
void set_w(const float new_w)
Set w value.
Definition: OpenRaveInterface.cpp:1254
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::OpenraveRobotMemoryInterface
Definition: OpenraveRobotMemoryInterface.h:37