[Fawkes Git] branch/fzwilling/robot-memory: 4 revs updated. (0.5.0-3456-g843d44e)

Frederik Zwilling zwilling at kbsg.rwth-aachen.de
Tue Dec 6 22:15:12 CET 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, fzwilling/robot-memory has been updated
        to  843d44ef8d1ecea204f491ec677f5ff32d0e1e0d (commit)
       via  ec8af3bbe772ad5860b3d3fc72fde6155f92a8f4 (commit)
       via  9ff4c39fe4d293c64d854b398bb95abf72e3778b (commit)
       via  1dc891c50987bc30e0c3a2af7027142bc3b92846 (commit)
      from  ac7cf879b77533cdc706afe79a17496a169c9694 (commit)

http://git.fawkesrobotics.org/fawkes.git/fzwilling/robot-memory

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- *Log* ---------------------------------------------------------------
commit 1dc891c50987bc30e0c3a2af7027142bc3b92846
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Tue Dec 6 12:15:19 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Tue Dec 6 22:14:54 2016 +0100

    pddl-robot-memory: add goal from config or interface message

http://git.fawkesrobotics.org/fawkes.git/commit/1dc891c
http://trac.fawkesrobotics.org/changeset/1dc891c

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 9ff4c39fe4d293c64d854b398bb95abf72e3778b
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Tue Dec 6 14:09:12 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Tue Dec 6 22:14:59 2016 +0100

    robot-memory: fixed using default db

http://git.fawkesrobotics.org/fawkes.git/commit/9ff4c39
http://trac.fawkesrobotics.org/changeset/9ff4c39

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit ec8af3bbe772ad5860b3d3fc72fde6155f92a8f4
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Tue Dec 6 14:19:59 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Tue Dec 6 22:15:03 2016 +0100

    robot-memory: properly drop collection instead of emptying it

http://git.fawkesrobotics.org/fawkes.git/commit/ec8af3b
http://trac.fawkesrobotics.org/changeset/ec8af3b

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 843d44ef8d1ecea204f491ec677f5ff32d0e1e0d
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Tue Dec 6 16:21:56 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Tue Dec 6 22:15:07 2016 +0100

    openrave-robot-memory: genericly add objects from the RM as configured
    
    Now you can specify in the config which types of objects you want to add
    to the motion planner scene. Here an example:
    
      object-types:
        blocks:
          collection: "robmem.blocks_world"
          query: "{block:{$exists:true}}"
          name-key: "block"
          model-path: "../res/openrave/block.kinbody.xml"

http://git.fawkesrobotics.org/fawkes.git/commit/843d44e
http://trac.fawkesrobotics.org/changeset/843d44e

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 src/libs/interfaces/PddlGenInterface.h             |    7 ++
 .../openrave-robot-memory_thread.cpp               |   67 +++++++++++++-------
 .../openrave-robot-memory_thread.h                 |    2 +-
 .../interfaces/PddlGenInterface.cpp                |   50 ++++++++++++++-
 .../interfaces/PddlGenInterface.h_ext              |    7 ++
 .../interfaces/PddlGenInterface.tolua              |    4 +
 .../interfaces/PddlGenInterface.xml                |    5 +-
 .../pddl-robot-memory/pddl_robot_memory_thread.cpp |   23 +++++---
 .../pddl-robot-memory/pddl_robot_memory_thread.h   |    1 +
 src/plugins/robot-memory/robot_memory.cpp          |   18 +++--
 src/tools/gtest/gtest_fawkes.cpp                   |    2 +-
 11 files changed, 145 insertions(+), 41 deletions(-)


- *Diffs* -------------------------------------------------------------

- *commit* 1dc891c50987bc30e0c3a2af7027142bc3b92846 - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Tue Dec 6 12:15:19 2016 +0100
Subject: pddl-robot-memory: add goal from config or interface message

 src/libs/interfaces/PddlGenInterface.h             |    7 +++
 .../interfaces/PddlGenInterface.cpp                |   50 +++++++++++++++++++-
 .../interfaces/PddlGenInterface.h_ext              |    7 +++
 .../interfaces/PddlGenInterface.tolua              |    4 ++
 .../interfaces/PddlGenInterface.xml                |    5 ++-
 .../pddl-robot-memory/pddl_robot_memory_thread.cpp |   23 ++++++---
 .../pddl-robot-memory/pddl_robot_memory_thread.h   |    1 +
 7 files changed, 87 insertions(+), 10 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/interfaces/PddlGenInterface.h b/src/libs/interfaces/PddlGenInterface.h
index bcafa03..8845b24 100644
--- a/src/libs/interfaces/PddlGenInterface.h
+++ b/src/libs/interfaces/PddlGenInterface.h
@@ -65,17 +65,24 @@ class PddlGenInterface : public Interface
     typedef struct {
       int64_t timestamp_sec;  /**< Interface Unix timestamp, seconds */
       int64_t timestamp_usec; /**< Interface Unix timestamp, micro-seconds */
+      char goal[1024]; /**< 
+    	Optional goal to insert into the template dictionary.
+     */
     } GenerateMessage_data_t;
 #pragma pack(pop)
 
     GenerateMessage_data_t *data;
 
    public:
+    GenerateMessage(const char * ini_goal);
     GenerateMessage();
     ~GenerateMessage();
 
     GenerateMessage(const GenerateMessage *m);
     /* Methods */
+    char * goal() const;
+    void set_goal(const char * new_goal);
+    size_t maxlenof_goal() const;
     virtual Message * clone() const;
   };
 
diff --git a/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.cpp b/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.cpp
index bf0a9e8..97d5fe9 100644
--- a/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.cpp
+++ b/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.cpp
@@ -53,7 +53,7 @@ PddlGenInterface::PddlGenInterface() : Interface()
   add_fieldinfo(IFT_UINT32, "msg_id", 1, &data->msg_id);
   add_fieldinfo(IFT_BOOL, "final", 1, &data->final);
   add_messageinfo("GenerateMessage");
-  unsigned char tmp_hash[] = {0x5d, 0xea, 0x4d, 0x50, 0x65, 0x68, 0x7, 0x93, 0xe0, 0x52, 0x46, 0x2, 0xe7, 0x3c, 0x3b, 0xa1};
+  unsigned char tmp_hash[] = {0x24, 0x2e, 0xeb, 0xd7, 0x1d, 0x5d, 0x15, 0x7f, 0x73, 0xca, 0xa3, 0xf3, 0x74, 0x1, 0x55, 0xc5};
   set_hash(tmp_hash);
 }
 
@@ -174,6 +174,19 @@ PddlGenInterface::enum_tostring(const char *enumtype, int val) const
  */
 
 
+/** Constructor with initial values.
+ * @param ini_goal initial value for goal
+ */
+PddlGenInterface::GenerateMessage::GenerateMessage(const char * ini_goal) : Message("GenerateMessage")
+{
+  data_size = sizeof(GenerateMessage_data_t);
+  data_ptr  = malloc(data_size);
+  memset(data_ptr, 0, data_size);
+  data      = (GenerateMessage_data_t *)data_ptr;
+  data_ts   = (message_data_ts_t *)data_ptr;
+  strncpy(data->goal, ini_goal, 1024);
+  add_fieldinfo(IFT_STRING, "goal", 1024, data->goal);
+}
 /** Constructor */
 PddlGenInterface::GenerateMessage::GenerateMessage() : Message("GenerateMessage")
 {
@@ -182,6 +195,7 @@ PddlGenInterface::GenerateMessage::GenerateMessage() : Message("GenerateMessage"
   memset(data_ptr, 0, data_size);
   data      = (GenerateMessage_data_t *)data_ptr;
   data_ts   = (message_data_ts_t *)data_ptr;
+  add_fieldinfo(IFT_STRING, "goal", 1024, data->goal);
 }
 
 /** Destructor */
@@ -203,6 +217,40 @@ PddlGenInterface::GenerateMessage::GenerateMessage(const GenerateMessage *m) : M
 }
 
 /* Methods */
+/** Get goal value.
+ * 
+    	Optional goal to insert into the template dictionary.
+    
+ * @return goal value
+ */
+char *
+PddlGenInterface::GenerateMessage::goal() const
+{
+  return data->goal;
+}
+
+/** Get maximum length of goal value.
+ * @return length of goal value, can be length of the array or number of 
+ * maximum number of characters for a string
+ */
+size_t
+PddlGenInterface::GenerateMessage::maxlenof_goal() const
+{
+  return 1024;
+}
+
+/** Set goal value.
+ * 
+    	Optional goal to insert into the template dictionary.
+    
+ * @param new_goal new goal value
+ */
+void
+PddlGenInterface::GenerateMessage::set_goal(const char * new_goal)
+{
+  strncpy(data->goal, new_goal, sizeof(data->goal));
+}
+
 /** Clone this message.
  * Produces a message of the same type as this message and copies the
  * data to the new message.
diff --git a/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.h_ext b/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.h_ext
index bcafa03..8845b24 100644
--- a/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.h_ext
+++ b/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.h_ext
@@ -65,17 +65,24 @@ class PddlGenInterface : public Interface
     typedef struct {
       int64_t timestamp_sec;  /**< Interface Unix timestamp, seconds */
       int64_t timestamp_usec; /**< Interface Unix timestamp, micro-seconds */
+      char goal[1024]; /**< 
+    	Optional goal to insert into the template dictionary.
+     */
     } GenerateMessage_data_t;
 #pragma pack(pop)
 
     GenerateMessage_data_t *data;
 
    public:
+    GenerateMessage(const char * ini_goal);
     GenerateMessage();
     ~GenerateMessage();
 
     GenerateMessage(const GenerateMessage *m);
     /* Methods */
+    char * goal() const;
+    void set_goal(const char * new_goal);
+    size_t maxlenof_goal() const;
     virtual Message * clone() const;
   };
 
diff --git a/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.tolua b/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.tolua
index 8de9555..3455188 100644
--- a/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.tolua
+++ b/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.tolua
@@ -8,6 +8,7 @@ class PddlGenInterface : public Interface
 
   class GenerateMessage : public Message
   {
+    GenerateMessage(char * ini_goal);
     GenerateMessage();
     ~GenerateMessage();
 
@@ -28,6 +29,9 @@ class PddlGenInterface : public Interface
     void              unref();
     unsigned int      refcount();
 
+    char * goal();
+    void set_goal(const char * new_goal);
+    int maxlenof_goal() const;
   };
 
   unsigned int msg_id();
diff --git a/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.xml b/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.xml
index 263aa64..1fd37f2 100644
--- a/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.xml
+++ b/src/plugins/pddl-robot-memory/interfaces/PddlGenInterface.xml
@@ -15,7 +15,10 @@
   
   <message name="Generate">
     <comment>
-      Query to execute.
+      Call to generate PDDL Problem
     </comment>
+    <field type="string" length="1024" name="goal">
+    	Optional goal to insert into the template dictionary.
+    </field>
   </message>
 </interface>
diff --git a/src/plugins/pddl-robot-memory/pddl_robot_memory_thread.cpp b/src/plugins/pddl-robot-memory/pddl_robot_memory_thread.cpp
index d95248d..86bb38f 100644
--- a/src/plugins/pddl-robot-memory/pddl_robot_memory_thread.cpp
+++ b/src/plugins/pddl-robot-memory/pddl_robot_memory_thread.cpp
@@ -68,6 +68,8 @@ PddlRobotMemoryThread::init()
     config->get_string("plugins/pddl-robot-memory/input-problem-description"));
   output_path = StringConversions::resolve_path("@BASEDIR@/src/agents/" +
     config->get_string("plugins/pddl-robot-memory/output-problem-description"));
+  if(config->exists("plugins/pddl-robot-memory/goal"))
+    goal = config->get_string("plugins/pddl-robot-memory/goal");
 
   //setup interface
   gen_if = blackboard->open_for_writing<PddlGenInterface>(config->get_string("plugins/pddl-robot-memory/interface-name").c_str());
@@ -135,6 +137,8 @@ PddlRobotMemoryThread::loop()
       fill_dict_from_document(entry_dict, obj);
     }
   }
+  //Add goal to dictionary
+  dict.SetValue("GOAL", goal);
 
   //prepare template expanding
   ctemplate::StringToTemplateCache("tpl-cache", input, ctemplate::DO_NOT_STRIP);
@@ -179,14 +183,17 @@ bool
 PddlRobotMemoryThread::bb_interface_message_received(Interface *interface, fawkes::Message *message) throw()
 {
   if (message->is_of_type<PddlGenInterface::GenerateMessage>()) {
-      gen_if->set_msg_id(message->id());
-      gen_if->set_final(false);
-      gen_if->write();
-      wakeup(); //activates loop where the generation is done
-    } else {
-      logger->log_error(name(), "Received unknown message of type %s, ignoring",
-            message->type());
-    }
+    PddlGenInterface::GenerateMessage* msg = (PddlGenInterface::GenerateMessage*) message;
+    gen_if->set_msg_id(msg->id());
+    gen_if->set_final(false);
+    gen_if->write();
+    if(std::string(msg->goal()) != "")
+      goal = msg->goal();
+    wakeup(); //activates loop where the generation is done
+  } else {
+    logger->log_error(name(), "Received unknown message of type %s, ignoring",
+        message->type());
+  }
   return false;
 }
 
diff --git a/src/plugins/pddl-robot-memory/pddl_robot_memory_thread.h b/src/plugins/pddl-robot-memory/pddl_robot_memory_thread.h
index 6c17ee1..aef67b2 100644
--- a/src/plugins/pddl-robot-memory/pddl_robot_memory_thread.h
+++ b/src/plugins/pddl-robot-memory/pddl_robot_memory_thread.h
@@ -64,6 +64,7 @@ class PddlRobotMemoryThread
   std::string collection;
   std::string input_path;
   std::string output_path;
+  std::string goal;
 
   void fill_dict_from_document(ctemplate::TemplateDictionary *dict, mongo::BSONObj obj, std::string prefix = "");
   void generate();

- *commit* 9ff4c39fe4d293c64d854b398bb95abf72e3778b - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Tue Dec 6 14:09:12 2016 +0100
Subject: robot-memory: fixed using default db

 src/plugins/robot-memory/robot_memory.cpp |   15 +++++++++------
 src/tools/gtest/gtest_fawkes.cpp          |    2 +-
 2 files changed, 10 insertions(+), 7 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robot-memory/robot_memory.cpp b/src/plugins/robot-memory/robot_memory.cpp
index d010b3e..5de6668 100644
--- a/src/plugins/robot-memory/robot_memory.cpp
+++ b/src/plugins/robot-memory/robot_memory.cpp
@@ -286,7 +286,8 @@ int RobotMemory::remove(Query query, std::string collection)
  */
 int RobotMemory::drop_collection(std::string collection)
 {
-  log_deb("Clearing whole robot memory");
+  check_collection_name(collection);
+  log_deb("Dropping collection " + collection);
   return remove("{}", collection);
 }
 
@@ -312,6 +313,7 @@ int RobotMemory::clear_memory()
  */
 int RobotMemory::restore_collection(std::string collection, std::string directory)
 {
+  check_collection_name(collection);
   drop_collection(collection);
 
   //resolve path to restore
@@ -326,7 +328,8 @@ int RobotMemory::restore_collection(std::string collection, std::string director
   log_deb(std::string("Restore collection " + collection + " from " + path), "warn");
 
   //call mongorestore from folder with initial restores
-  std::string command = "/usr/bin/mongorestore --dir " + path + " --quiet";
+  std::string command = "/usr/bin/mongorestore --dir " + path
+      + " --host=127.0.0.1 --quiet";
   log_deb(std::string("Restore command: " + command), "warn");
   FILE *bash_output = popen(command.c_str(), "r");
 
@@ -364,6 +367,7 @@ int RobotMemory::restore_collection(std::string collection, std::string director
  */
 int RobotMemory::dump_collection(std::string collection, std::string directory)
 {
+  check_collection_name(collection);
   //resolve path to dump to
   if(collection.find(".") == std::string::npos)
   {
@@ -377,10 +381,9 @@ int RobotMemory::dump_collection(std::string collection, std::string directory)
   //call mongorestore from folder with initial restores
   std::vector<std::string> split = str_split(collection, '.');
   std::string command = "/usr/bin/mongodump --out=" + path + " --db=" + split[0]
-    + " --collection=" + split[1] + " --quiet";
+    + " --collection=" + split[1] + " --host=127.0.0.1 --quiet";
   log_deb(std::string("Dump command: " + command), "warn");
   FILE *bash_output = popen(command.c_str(), "r");
-
   //check if output is ok
   if(!bash_output)
   {
@@ -515,10 +518,10 @@ RobotMemory::check_collection_name(std::string &collection)
   {
       collection = default_collection_;
   }
-  else if(default_collection_ != "robmem" && collection.find("robmem.") == 1)
+  if(database_name_ != "robmem" && collection.find("robmem.") == 0)
   {
     //change used database name (e.g. for the case of multiple simulated dababases)
-    collection.replace(0, 6, default_collection_);
+    collection.replace(0, 6, database_name_);
   }
 }
 
diff --git a/src/tools/gtest/gtest_fawkes.cpp b/src/tools/gtest/gtest_fawkes.cpp
index e601ad5..9f224a1 100644
--- a/src/tools/gtest/gtest_fawkes.cpp
+++ b/src/tools/gtest/gtest_fawkes.cpp
@@ -51,7 +51,7 @@ int main(int argc, char **argv) {
     fawkes_argv[1] = new char[2];
     strcpy(fawkes_argv[1], "-p");
     fawkes_argv[2] = new char[128];
-    strcpy(fawkes_argv[2], "static-transforms,m-robot-memory,robot_memory_test");
+    strcpy(fawkes_argv[2], "static-transforms,mongodb,robot-memory,robot_memory_test");
     fawkes_argv[3] = new char[2];
     strcpy(fawkes_argv[3], "-c");
     fawkes_argv[4] = new char[128];

- *commit* ec8af3bbe772ad5860b3d3fc72fde6155f92a8f4 - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Tue Dec 6 14:19:59 2016 +0100
Subject: robot-memory: properly drop collection instead of emptying it

 src/plugins/robot-memory/robot_memory.cpp |    3 ++-
 1 files changed, 2 insertions(+), 1 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robot-memory/robot_memory.cpp b/src/plugins/robot-memory/robot_memory.cpp
index 5de6668..027add9 100644
--- a/src/plugins/robot-memory/robot_memory.cpp
+++ b/src/plugins/robot-memory/robot_memory.cpp
@@ -287,8 +287,9 @@ int RobotMemory::remove(Query query, std::string collection)
 int RobotMemory::drop_collection(std::string collection)
 {
   check_collection_name(collection);
+  MutexLocker lock(mutex_);
   log_deb("Dropping collection " + collection);
-  return remove("{}", collection);
+  return mongodb_client_->dropCollection(collection);
 }
 
 /**

- *commit* 843d44ef8d1ecea204f491ec677f5ff32d0e1e0d - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Tue Dec 6 16:21:56 2016 +0100
Subject: openrave-robot-memory: genericly add objects from the RM as configured

 .../openrave-robot-memory_thread.cpp               |   67 +++++++++++++-------
 .../openrave-robot-memory_thread.h                 |    2 +-
 2 files changed, 46 insertions(+), 23 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.cpp b/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.cpp
index 3e34936..7405558 100644
--- a/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.cpp
+++ b/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "openrave-robot-memory_thread.h"
+#include <algorithm>
 
 using namespace fawkes;
 using namespace mongo;
@@ -39,7 +40,6 @@ OpenraveRobotMemoryThread::OpenraveRobotMemoryThread()
 void
 OpenraveRobotMemoryThread::init()
 {
-  collection_ = config->get_string("plugins/openrave-robot-memory/input-collection");
   openrave_if_ = blackboard->open_for_reading<OpenRaveInterface>(config->get_string("plugins/openrave-robot-memory/openrave-if-name").c_str());
   or_rm_if_ = blackboard->open_for_writing<OpenraveRobotMemoryInterface>(config->get_string("plugins/openrave-robot-memory/if-name").c_str());
 }
@@ -68,30 +68,53 @@ OpenraveRobotMemoryThread::construct_scene()
 {
   logger->log_info(name(), "Constructing Scene");
 
-  //add or move already added objects:
-  QResCursor cur = robot_memory->query(fromjson("{block:{$exists:true},frame:'base_link',allow_tf:true}"), collection_);
-  while(cur->more())
+  // add all object types by iterating over config paths
+  std::string prefix = "plugins/openrave-robot-memory/object-types/";
+  std::unique_ptr<Configuration::ValueIterator> object_types(config->search(prefix.c_str()));
+  while(object_types->next())
   {
-    BSONObj block = cur->next();
-    logger->log_info(name(), "Block: %s", block.toString().c_str());
-    std::string block_name = block.getStringField("block");
-    if(std::find(added_objects_.begin(), added_objects_.end(), block_name) == added_objects_.end())
+    //object_types->next() yields the whole path, so we have to get the right prefix
+    std::string cfg_name = std::string(object_types->path()).substr(prefix.length());
+    cfg_name = cfg_name.substr(0, cfg_name.find("/"));
+    //don't use the same prefix again
+    if(std::find(added_object_types_.begin(), added_object_types_.end(), cfg_name) != added_object_types_.end())
+      continue;
+    added_object_types_.push_back(cfg_name);
+    logger->log_info(name(), "Adding object type: %s", cfg_name.c_str());
+    std::string cfg_prefix = prefix + cfg_name + "/";
+    std::string collection = config->get_string(cfg_prefix + "collection");
+
+    //construct query
+    BSONObjBuilder query_builder;
+    query_builder.appendElements(fromjson(config->get_string(cfg_prefix + "query")));
+    query_builder << "frame" << "base_link" << "allow_tf" << true;
+    BSONObj query = query_builder.obj();
+    logger->log_info(name(), "Querying: %s", query.toString().c_str());
+    QResCursor cur = robot_memory->query(query, collection);
+    while(cur->more())
     {
-      //add new object
-      logger->log_info(name(), "adding %s", block_name.c_str());
-      OpenRaveInterface::AddObjectMessage add_msg;
-      add_msg.set_name(block_name.c_str());
-      add_msg.set_path("../fawkes/res/openrave/cylinder.kinbody.xml");
-      openrave_if_->msgq_enqueue_copy(&add_msg);
-      added_objects_.push_back(block_name);
+      BSONObj block = cur->next();
+      //logger->log_info(name(), "Adding: %s", cfg_prefix.c_str(), block.toString().c_str());
+      std::string block_name = block.getStringField(config->get_string(cfg_prefix + "name-key"));
+      if(std::find(added_objects_.begin(), added_objects_.end(), block_name) == added_objects_.end())
+      {
+        //add new object
+        logger->log_info(name(), "adding %s", block_name.c_str());
+        OpenRaveInterface::AddObjectMessage add_msg;
+        add_msg.set_name(block_name.c_str());
+        add_msg.set_path(config->get_string(cfg_prefix + "model-path").c_str());
+        openrave_if_->msgq_enqueue_copy(&add_msg);
+        added_objects_.push_back(block_name);
+      }
+      //move object to right position
+      OpenRaveInterface::MoveObjectMessage move_msg;
+      move_msg.set_name(block_name.c_str());
+      move_msg.set_x(block.getField("translation").Array()[0].Double());
+      move_msg.set_y(block.getField("translation").Array()[1].Double());
+      move_msg.set_z(block.getField("translation").Array()[2].Double());
+      openrave_if_->msgq_enqueue_copy(&move_msg);
     }
-    //move object to right position
-    OpenRaveInterface::MoveObjectMessage move_msg;
-    move_msg.set_name(block_name.c_str());
-    move_msg.set_x(block.getField("translation").Array()[0].Double());
-    move_msg.set_y(block.getField("translation").Array()[1].Double());
-    move_msg.set_z(block.getField("translation").Array()[2].Double());
-    openrave_if_->msgq_enqueue_copy(&move_msg);
   }
+  added_object_types_.clear();
   logger->log_info(name(), "Finished Constructing Scene");
 }
diff --git a/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.h b/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.h
index 7aba40b..cd6f0b1 100644
--- a/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.h
+++ b/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.h
@@ -57,10 +57,10 @@ class OpenraveRobotMemoryThread
   protected: virtual void run() { Thread::run(); }
 
  private:
-  std::string collection_;
   fawkes::OpenRaveInterface* openrave_if_;
   fawkes::OpenraveRobotMemoryInterface* or_rm_if_;
   std::list<std::string> added_objects_;
+  std::list<std::string> added_object_types_;
 
   void construct_scene();
 };




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list