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

Frederik Zwilling zwilling at kbsg.rwth-aachen.de
Thu Nov 24 18:14:59 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  0bd5b96d1753dbf5b8414ceac7422ed4eda6ba2b (commit)
       via  7041c4e002a9b2adc3c97e813124e3edfc4f980f (commit)
       via  273fb8a22e4405802e7eebb1e34d140e40260e1c (commit)
       via  2d9deb88f21c8aa27c2b8d9f10b74d6617e46816 (commit)
      from  903a772eb9e57c66896d10b79d77348702884a71 (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 2d9deb88f21c8aa27c2b8d9f10b74d6617e46816
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Thu Nov 24 14:38:14 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Thu Nov 24 18:14:40 2016 +0100

    openrave-robot-memory: constructing motion planner scenes from robot memory
    
    This new plugin connects the robot memory to the motion planner
    openrave. It should construct the scene in which opnrave plans robot arm
    motion. To do this it querries a collection of the robot memory for
    objects in the scene. This is only the plugin skeleton.

http://git.fawkesrobotics.org/fawkes.git/commit/2d9deb8
http://trac.fawkesrobotics.org/changeset/2d9deb8

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 273fb8a22e4405802e7eebb1e34d140e40260e1c
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Thu Nov 24 18:11:13 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Thu Nov 24 18:14:44 2016 +0100

    robot-memory: fixed transform computable not keeping fields

http://git.fawkesrobotics.org/fawkes.git/commit/273fb8a
http://trac.fawkesrobotics.org/changeset/273fb8a

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 7041c4e002a9b2adc3c97e813124e3edfc4f980f
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Thu Nov 24 18:12:04 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Thu Nov 24 18:14:50 2016 +0100

    robot-memory: multithreading robust checking if query mathes a computable

http://git.fawkesrobotics.org/fawkes.git/commit/7041c4e
http://trac.fawkesrobotics.org/changeset/7041c4e

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 0bd5b96d1753dbf5b8414ceac7422ed4eda6ba2b
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Thu Nov 24 18:13:16 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Thu Nov 24 18:14:55 2016 +0100

    openrave-robot-memory: add and place block objects in openrave

http://git.fawkesrobotics.org/fawkes.git/commit/0bd5b96
http://trac.fawkesrobotics.org/changeset/0bd5b96

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


- *Summary* -----------------------------------------------------------
 .../interfaces/OpenraveRobotMemoryInterface.cpp    |  199 ++++++++++++++++++++
 ...rInterface.h => OpenraveRobotMemoryInterface.h} |   46 +++---
 ...ce.tolua => OpenraveRobotMemoryInterface.tolua} |   25 +--
 .../interfaces/OpenraveRobotMemoryInterface.xml    |   18 ++
 src/plugins/Makefile                               |    2 +-
 src/plugins/openrave-robot-memory/Makefile         |   53 +++++
 .../openrave-robot-memory_plugin.cpp}              |   24 +--
 .../openrave-robot-memory_thread.cpp               |   97 ++++++++++
 .../openrave-robot-memory_thread.h}                |   38 ++--
 .../computables/computables_manager.cpp            |   11 +-
 .../computables/transform_computable.cpp           |    5 +-
 src/plugins/robot-memory/robot_memory.cpp          |    2 +-
 12 files changed, 441 insertions(+), 79 deletions(-)
 create mode 100644 src/libs/interfaces/OpenraveRobotMemoryInterface.cpp
 copy src/libs/interfaces/{EclipseDebuggerInterface.h => OpenraveRobotMemoryInterface.h} (68%)
 copy src/libs/interfaces/{EclipseDebuggerInterface.tolua => OpenraveRobotMemoryInterface.tolua} (78%)
 create mode 100644 src/libs/interfaces/OpenraveRobotMemoryInterface.xml
 create mode 100644 src/plugins/openrave-robot-memory/Makefile
 copy src/plugins/{clips-robot-memory/clips_robot_memory_plugin.cpp => openrave-robot-memory/openrave-robot-memory_plugin.cpp} (61%)
 create mode 100644 src/plugins/openrave-robot-memory/openrave-robot-memory_thread.cpp
 copy src/plugins/{robot-memory/test-plugin/robot_memory_test_thread.h => openrave-robot-memory/openrave-robot-memory_thread.h} (62%)


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

- *commit* 2d9deb88f21c8aa27c2b8d9f10b74d6617e46816 - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Thu Nov 24 14:38:14 2016 +0100
Subject: openrave-robot-memory: constructing motion planner scenes from robot memory

 .../interfaces/OpenraveRobotMemoryInterface.cpp    |  199 ++++++++++++++++++++
 ...rInterface.h => OpenraveRobotMemoryInterface.h} |   46 +++---
 ...ce.tolua => OpenraveRobotMemoryInterface.tolua} |   25 +--
 .../interfaces/OpenraveRobotMemoryInterface.xml    |   18 ++
 src/plugins/Makefile                               |    2 +-
 .../aspect => openrave-robot-memory}/Makefile      |   28 ++--
 .../openrave-robot-memory_plugin.cpp}              |   24 +--
 .../openrave-robot-memory_thread.cpp               |   69 +++++++
 .../openrave-robot-memory_thread.h}                |   34 ++--
 9 files changed, 361 insertions(+), 84 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/Makefile b/src/plugins/Makefile
index b0e0275..55433d9 100644
--- a/src/plugins/Makefile
+++ b/src/plugins/Makefile
@@ -18,7 +18,7 @@ include $(BASEDIR)/etc/buildsys/config.mk
 
 # base + hardware drivers + perception + functional + integration
 SUBDIRS	= bbsync bblogger webview ttmainloop rrd rrdweb \
-	  laser imu flite festival joystick openrave \
+	  laser imu flite festival joystick openrave openrave-robot-memory\
 	  katana jaco pantilt roomba nao robotino \
 	  bumblebee2 perception amcl \
 	  skiller luaagent worldmodel \

- *commit* 273fb8a22e4405802e7eebb1e34d140e40260e1c - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Thu Nov 24 18:11:13 2016 +0100
Subject: robot-memory: fixed transform computable not keeping fields

 .../computables/transform_computable.cpp           |    5 ++---
 1 files changed, 2 insertions(+), 3 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robot-memory/computables/transform_computable.cpp b/src/plugins/robot-memory/computables/transform_computable.cpp
index cb082e1..6e87f18 100644
--- a/src/plugins/robot-memory/computables/transform_computable.cpp
+++ b/src/plugins/robot-memory/computables/transform_computable.cpp
@@ -82,8 +82,9 @@ std::list<mongo::BSONObj> TransformComputable::compute_transform(mongo::BSONObj
         fawkes::tf::Stamped<fawkes::tf::Pose> res_stamped_pose;
         tf_->transform_pose(target_frame.c_str(), src_stamped_pose, res_stamped_pose);
 
-        res_pos.appendElements(query.removeField("frame").removeField("translation").removeField("rotation"));
+        res_pos.appendElements(pos.removeField("frame").removeField("translation").removeField("rotation").removeField("_id"));
         res_pos.append("frame", target_frame);
+        res_pos.append("allow_tf", true);
         BSONArrayBuilder arrb_trans;
         arrb_trans.append(res_stamped_pose.getOrigin().x());
         arrb_trans.append(res_stamped_pose.getOrigin().y());
@@ -95,8 +96,6 @@ std::list<mongo::BSONObj> TransformComputable::compute_transform(mongo::BSONObj
         arrb_rot.append(res_stamped_pose.getRotation().z());
         arrb_rot.append(res_stamped_pose.getRotation().w());
         res_pos.append("rotation", arrb_rot.arr());
-        //logger_->log_info(name_, "Tf res: %s", res_pos.obj().toString().c_str());
-
         res.push_back(res_pos.obj());
       }
 //      else

- *commit* 7041c4e002a9b2adc3c97e813124e3edfc4f980f - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Thu Nov 24 18:12:04 2016 +0100
Subject: robot-memory: multithreading robust checking if query mathes a computable

 .../computables/computables_manager.cpp            |   11 +++++------
 src/plugins/robot-memory/robot_memory.cpp          |    2 +-
 2 files changed, 6 insertions(+), 7 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robot-memory/computables/computables_manager.cpp b/src/plugins/robot-memory/computables/computables_manager.cpp
index 4e2f21e..655b20f 100644
--- a/src/plugins/robot-memory/computables/computables_manager.cpp
+++ b/src/plugins/robot-memory/computables/computables_manager.cpp
@@ -85,20 +85,19 @@ bool ComputablesManager::check_and_compute(mongo::Query query, std::string colle
   {
     if(collection == std::get<0>(it->first) && query.toString() == std::get<1>(it->first))
     {
-      logger_->log_info(name.c_str(), "Already computed");
       return false;
     }
   }
-  if(collection == matching_test_collection_)
+  if(collection.find(matching_test_collection_) != std::string::npos)
     return false; //not necessary for matching test itself
   bool added_computed_docs = false;
   //check if the query is matched by the computable identifyer
   //to do that we just insert the query as if it would be a document and query for it with the computable identifiers
-  robot_memory_->remove(fromjson("{}"), matching_test_collection_);
-  robot_memory_->insert(query.obj, matching_test_collection_);
+  std::string current_test_collection = matching_test_collection_ + std::to_string(rand());
+  robot_memory_->insert(query.obj, current_test_collection);
   for(std::list<Computable*>::iterator it = computables.begin(); it != computables.end(); it++)
   {
-    if(collection == (*it)->get_collection() &&  robot_memory_->query((*it)->get_query(), matching_test_collection_)->more())
+    if(collection == (*it)->get_collection() &&  robot_memory_->query((*it)->get_query(), current_test_collection)->more())
     {
       std::list<BSONObj> computed_docs_list = (*it)->compute(query.obj);
       if(computed_docs_list.size() > 0)
@@ -115,6 +114,7 @@ bool ComputablesManager::check_and_compute(mongo::Query query, std::string colle
       }
     }
   }
+  robot_memory_->drop_collection(current_test_collection);
   return added_computed_docs;
 }
 
@@ -131,7 +131,6 @@ void ComputablesManager::cleanup_computed_docs()
     {
       if(current_time_ms > it->second)
       {
-        logger_->log_info(name.c_str(), "Removing cache %s", std::get<0>(it->first).c_str());
         robot_memory_->remove(BSON("_robmem_info.computed" << true
             << "_robmem_info.cached_until" << BSON("$lt" << current_time_ms)), std::get<0>(it->first));
         cached_querries_.erase(it->first);
diff --git a/src/plugins/robot-memory/robot_memory.cpp b/src/plugins/robot-memory/robot_memory.cpp
index d55bc79..d010b3e 100644
--- a/src/plugins/robot-memory/robot_memory.cpp
+++ b/src/plugins/robot-memory/robot_memory.cpp
@@ -83,7 +83,7 @@ void RobotMemory::init()
   try {
     debug_ = config_->get_bool("/plugins/robot-memory/more-debug-output");
   } catch (Exception &e) {}
-  database_name_ = "mobmem";
+  database_name_ = "robmem";
   try {
     database_name_ = config_->get_string("/plugins/robot-memory/database");
   } catch (Exception &e) {}

- *commit* 0bd5b96d1753dbf5b8414ceac7422ed4eda6ba2b - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Thu Nov 24 18:13:16 2016 +0100
Subject: openrave-robot-memory: add and place block objects in openrave

 src/plugins/openrave-robot-memory/Makefile         |   11 ++++++-
 .../openrave-robot-memory_thread.cpp               |   28 ++++++++++++++++++++
 .../openrave-robot-memory_thread.h                 |    6 ++++
 3 files changed, 43 insertions(+), 2 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/openrave-robot-memory/Makefile b/src/plugins/openrave-robot-memory/Makefile
index 784f101..5c2e504 100644
--- a/src/plugins/openrave-robot-memory/Makefile
+++ b/src/plugins/openrave-robot-memory/Makefile
@@ -26,20 +26,27 @@ OBJS_openrave_robot_memory = openrave-robot-memory_plugin.o openrave-robot-memor
 
 OBJS_all = $(OBJS_openrave-robot-memory)
 
-ifeq ($(HAVE_MONGODB),1)
+ifeq ($(HAVE_CPP11)$(HAVE_MONGODB),11)
+  CFLAGS  += $(CFLAGS_CPP11)
+  
   PLUGINS_all = $(PLUGINDIR)/openrave-robot-memory.$(SOEXT)
 else
   ifneq ($(HAVE_MONGODB),1)
     WARN_TARGETS += warning_mongodb
   endif
+  ifneq ($(HAVE_CLIPS),1)
+    WARN_TARGETS = warning_clips
+  endif
 endif
 
 ifeq ($(OBJSSUBMAKE),1)
 all: $(WARN_TARGETS)
 
-.PHONY: warning_mongodb
+.PHONY: warning_mongodb warning_cpp11
 warning_mongodb:
 	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TRED)Omitting openrave-robot-memory Plugin$(TNORMAL) (mongodb[-devel] not installed)"
+warning_cpp11:
+	$(SILENT)echo -e "$(INDENT_PRINT)--> $(TRED)Cannot build clips-robot-memory plugin$(TNORMAL) (C++11 not supported)"
 endif
 
 
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 545b8d3..3e34936 100644
--- a/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.cpp
+++ b/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.cpp
@@ -22,6 +22,7 @@
 #include "openrave-robot-memory_thread.h"
 
 using namespace fawkes;
+using namespace mongo;
 
 /** @class OpenraveRobotMemoryThread 'openrave-robot-memory_thread.h' 
  * Creates an OpenRave Scene for motion planning from data in the robot memory
@@ -66,4 +67,31 @@ void
 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())
+  {
+    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())
+    {
+      //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);
+    }
+    //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);
+  }
+  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 e237104..7aba40b 100644
--- a/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.h
+++ b/src/plugins/openrave-robot-memory/openrave-robot-memory_thread.h
@@ -29,6 +29,9 @@
 #include <aspect/configurable.h>
 #include <interfaces/OpenRaveInterface.h>
 #include <interfaces/OpenraveRobotMemoryInterface.h>
+#include <plugins/robot-memory/aspect/robot_memory_aspect.h>
+#include <list>
+#include <algorithm>
 
 namespace fawkes {
   // add forward declarations here, e.g., interfaces
@@ -39,6 +42,7 @@ class OpenraveRobotMemoryThread
   public fawkes::BlockedTimingAspect,
   public fawkes::LoggingAspect,
   public fawkes::ConfigurableAspect,
+  public fawkes::RobotMemoryAspect,
   public fawkes::BlackBoardAspect
 {
 
@@ -56,6 +60,8 @@ class OpenraveRobotMemoryThread
   std::string collection_;
   fawkes::OpenRaveInterface* openrave_if_;
   fawkes::OpenraveRobotMemoryInterface* or_rm_if_;
+  std::list<std::string> added_objects_;
+
   void construct_scene();
 };
 




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


More information about the fawkes-commits mailing list