[Fawkes Git] branch/fzwilling/robot-memory: 3 revs updated. (0.5.0-3442-gc909ed2)

Frederik Zwilling zwilling at kbsg.rwth-aachen.de
Wed Nov 23 13:45:28 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  c909ed2f6a3ada8913d2b8eba7359de2b97ab122 (commit)
       via  56f59c110451cefa6fe710c81fdd70c21be2ae98 (commit)
       via  e8cf33dae85c18bc0355e526c62801f76799ec25 (commit)
      from  245423ecb8eb908f6586ff3a5658b7df08beaaa5 (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 e8cf33dae85c18bc0355e526c62801f76799ec25
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Mon Nov 21 18:07:35 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Wed Nov 23 13:45:11 2016 +0100

    robot-memory: give collection in computable function
    
    This allows registering a computable for multiple collections.

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 56f59c110451cefa6fe710c81fdd70c21be2ae98
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Wed Nov 23 13:39:53 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Wed Nov 23 13:45:15 2016 +0100

    robot-memory: added transform computable
    
    This computable provides geometric positions already present in the
    robot memory in other frames. To use it you have to add the used
    collection in the config of the transform computable and query the
    wanted position with the target frame and 'allow_tf':true .

http://git.fawkesrobotics.org/fawkes.git/commit/56f59c1
http://trac.fawkesrobotics.org/changeset/56f59c1

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit c909ed2f6a3ada8913d2b8eba7359de2b97ab122
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Wed Nov 23 13:42:02 2016 +0100
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Wed Nov 23 13:45:19 2016 +0100

    robot-memory: fixed template function for trigger registration

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

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


- *Summary* -----------------------------------------------------------
 .../computables/blackboard_computable.cpp          |    4 +-
 .../computables/blackboard_computable.h            |    2 +-
 .../robot-memory/computables/computable.cpp        |    4 +-
 src/plugins/robot-memory/computables/computable.h  |    4 +-
 .../robot-memory/computables/computables_manager.h |    4 +-
 .../computables/transform_computable.cpp           |  112 ++++++++++++++++++++
 ...ckboard_computable.h => transform_computable.h} |   34 +++---
 src/plugins/robot-memory/robot_memory.cpp          |    6 +-
 src/plugins/robot-memory/robot_memory.h            |    4 +-
 src/plugins/robot-memory/robot_memory_thread.cpp   |    4 +-
 src/plugins/robot-memory/robot_memory_thread.h     |    4 +
 .../robot-memory/test-plugin/robot_memory_test.cpp |   19 +++-
 .../robot-memory/test-plugin/robot_memory_test.h   |    6 +-
 src/tools/gtest/gtest_fawkes.cpp                   |    2 +-
 14 files changed, 169 insertions(+), 40 deletions(-)
 create mode 100644 src/plugins/robot-memory/computables/transform_computable.cpp
 copy src/plugins/robot-memory/computables/{blackboard_computable.h => transform_computable.h} (51%)


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

- *commit* e8cf33dae85c18bc0355e526c62801f76799ec25 - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Mon Nov 21 18:07:35 2016 +0100
Subject: robot-memory: give collection in computable function

 .../computables/blackboard_computable.cpp          |    4 ++--
 .../computables/blackboard_computable.h            |    2 +-
 .../robot-memory/computables/computable.cpp        |    4 ++--
 src/plugins/robot-memory/computables/computable.h  |    4 ++--
 .../robot-memory/computables/computables_manager.h |    4 ++--
 src/plugins/robot-memory/robot_memory.h            |    2 +-
 .../robot-memory/test-plugin/robot_memory_test.h   |    6 +++---
 7 files changed, 13 insertions(+), 13 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robot-memory/computables/blackboard_computable.cpp b/src/plugins/robot-memory/computables/blackboard_computable.cpp
index aff81c5..528f80f 100644
--- a/src/plugins/robot-memory/computables/blackboard_computable.cpp
+++ b/src/plugins/robot-memory/computables/blackboard_computable.cpp
@@ -23,7 +23,7 @@
 
 /** @class BlackboardComputable  blackboard_computable.h
  * Computable providing access to blackboard interfaces.
- * The Query has to match
+ * The Query has to match {interface:{$exists:true}} on the blackboard collection
  * @author Frederik Zwilling
  */
 
@@ -46,7 +46,7 @@ BlackboardComputable::~BlackboardComputable()
   robot_memory_->remove_computable(computable);
 }
 
-std::list<mongo::BSONObj> BlackboardComputable::compute_interfaces(mongo::BSONObj query)
+std::list<mongo::BSONObj> BlackboardComputable::compute_interfaces(mongo::BSONObj query, std::string collection)
 {
   std::list<mongo::BSONObj> res;
   std::string type = query.getField("interface").String();
diff --git a/src/plugins/robot-memory/computables/blackboard_computable.h b/src/plugins/robot-memory/computables/blackboard_computable.h
index bfee458..6019c20 100644
--- a/src/plugins/robot-memory/computables/blackboard_computable.h
+++ b/src/plugins/robot-memory/computables/blackboard_computable.h
@@ -37,7 +37,7 @@ class BlackboardComputable
     virtual ~BlackboardComputable();
 
   private:
-    std::list<mongo::BSONObj> compute_interfaces(mongo::BSONObj query);
+    std::list<mongo::BSONObj> compute_interfaces(mongo::BSONObj query, std::string collection);
 
     RobotMemory* robot_memory_;
     fawkes::BlackBoard* blackboard_;
diff --git a/src/plugins/robot-memory/computables/computable.cpp b/src/plugins/robot-memory/computables/computable.cpp
index 1b9e60b..592b45d 100644
--- a/src/plugins/robot-memory/computables/computable.cpp
+++ b/src/plugins/robot-memory/computables/computable.cpp
@@ -28,7 +28,7 @@ using namespace mongo;
  * @author Frederik Zwilling
  */
 
-Computable::Computable(Query query_to_compute, std::string collection, const boost::function<std::list<BSONObj> (BSONObj)> &compute_function)
+Computable::Computable(Query query_to_compute, std::string collection, const boost::function<std::list<BSONObj> (BSONObj, std::string)> &compute_function)
 {
   this->compute_function = compute_function;
   this->query_to_compute = query_to_compute;
@@ -48,7 +48,7 @@ Computable::~Computable()
 std::list<BSONObj> Computable::compute(BSONObj query)
 {
   // use provided function to compute demanded documents
-  std::list<BSONObj> docs = compute_function(query);
+  std::list<BSONObj> docs = compute_function(query, collection);
   //add metainformation for each document
   for(BSONObj &obj : docs)
   {
diff --git a/src/plugins/robot-memory/computables/computable.h b/src/plugins/robot-memory/computables/computable.h
index 5232b69..a15f474 100644
--- a/src/plugins/robot-memory/computables/computable.h
+++ b/src/plugins/robot-memory/computables/computable.h
@@ -28,7 +28,7 @@
 class Computable
 {
   public:
-    Computable(mongo::Query query_to_compute, std::string collection, const boost::function<std::list<mongo::BSONObj> (mongo::BSONObj)> &compute_function);
+    Computable(mongo::Query query_to_compute, std::string collection, const boost::function<std::list<mongo::BSONObj> (mongo::BSONObj, std::string)> &compute_function);
     virtual ~Computable();
 
     std::list<mongo::BSONObj> compute(mongo::BSONObj query);
@@ -36,7 +36,7 @@ class Computable
     std::string get_collection();
 
   private:
-    boost::function<std::list<mongo::BSONObj> (mongo::BSONObj)> compute_function;
+    boost::function<std::list<mongo::BSONObj> (mongo::BSONObj, std::string)> compute_function;
     mongo::Query query_to_compute;
     std::string collection;
 };
diff --git a/src/plugins/robot-memory/computables/computables_manager.h b/src/plugins/robot-memory/computables/computables_manager.h
index 0c3a9da..9e73945 100644
--- a/src/plugins/robot-memory/computables/computables_manager.h
+++ b/src/plugins/robot-memory/computables/computables_manager.h
@@ -51,9 +51,9 @@ class ComputablesManager
      * @return Computable Object pointer used for removing it
      */
     template<typename T>
-    Computable* register_computable(mongo::Query query_to_compute, std::string collection, std::list<mongo::BSONObj>(T::*compute_func)(mongo::BSONObj), T *obj)
+    Computable* register_computable(mongo::Query query_to_compute, std::string collection, std::list<mongo::BSONObj>(T::*compute_func)(mongo::BSONObj, std::string), T *obj)
     {
-      Computable* comp = new Computable(query_to_compute, collection, boost::bind(compute_func, obj, _1));
+      Computable* comp = new Computable(query_to_compute, collection, boost::bind(compute_func, obj, _1, _2));
       computables.push_back(comp);
       return comp;
     }
diff --git a/src/plugins/robot-memory/robot_memory.h b/src/plugins/robot-memory/robot_memory.h
index 5f032f4..bda35ba 100644
--- a/src/plugins/robot-memory/robot_memory.h
+++ b/src/plugins/robot-memory/robot_memory.h
@@ -100,7 +100,7 @@ class RobotMemory
      * @return Computable Object pointer used for removing it
      */
     template<typename T>
-    Computable* register_computable(mongo::Query query_to_compute, std::string collection, std::list<mongo::BSONObj>(T::*compute_func)(mongo::BSONObj), T *obj)
+    Computable* register_computable(mongo::Query query_to_compute, std::string collection, std::list<mongo::BSONObj>(T::*compute_func)(mongo::BSONObj, std::string), T *obj)
     {
       return computables_manager_->register_computable(query_to_compute, collection, compute_func, obj);
     }
diff --git a/src/plugins/robot-memory/test-plugin/robot_memory_test.h b/src/plugins/robot-memory/test-plugin/robot_memory_test.h
index cb12317..59413eb 100644
--- a/src/plugins/robot-memory/test-plugin/robot_memory_test.h
+++ b/src/plugins/robot-memory/test-plugin/robot_memory_test.h
@@ -89,13 +89,13 @@ class TestComputable
   };
    ~TestComputable(){};
    //Different functions for computables:
-   std::list<mongo::BSONObj> compute(mongo::BSONObj query)
+   std::list<mongo::BSONObj> compute(mongo::BSONObj query, std::string collection)
    {
      std::list<mongo::BSONObj> res;
      res.push_back(mongo::fromjson("{computed:true, result:'this is computed'}"));
      return res;
    }
-   std::list<mongo::BSONObj> compute_sum(mongo::BSONObj query)
+   std::list<mongo::BSONObj> compute_sum(mongo::BSONObj query, std::string collection)
    {
      std::list<mongo::BSONObj> res;
      int x = query.getField("x").Int();
@@ -107,7 +107,7 @@ class TestComputable
      res.push_back(b.obj());
      return res;
    }
-   std::list<mongo::BSONObj> compute_multiple(mongo::BSONObj query)
+   std::list<mongo::BSONObj> compute_multiple(mongo::BSONObj query, std::string collection)
    {
      std::list<mongo::BSONObj> res;
      res.push_back(mongo::fromjson("{compute:'multiple', count:1}"));

- *commit* 56f59c110451cefa6fe710c81fdd70c21be2ae98 - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Wed Nov 23 13:39:53 2016 +0100
Subject: robot-memory: added transform computable

 .../computables/transform_computable.cpp           |  112 ++++++++++++++++++++
 ...ckboard_computable.h => transform_computable.h} |   34 +++---
 src/plugins/robot-memory/robot_memory.cpp          |    6 +-
 src/plugins/robot-memory/robot_memory_thread.cpp   |    4 +-
 src/plugins/robot-memory/robot_memory_thread.h     |    4 +
 .../robot-memory/test-plugin/robot_memory_test.cpp |   19 +++-
 src/tools/gtest/gtest_fawkes.cpp                   |    2 +-
 7 files changed, 155 insertions(+), 26 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/robot-memory/robot_memory.cpp b/src/plugins/robot-memory/robot_memory.cpp
index d02a39b..62585cc 100644
--- a/src/plugins/robot-memory/robot_memory.cpp
+++ b/src/plugins/robot-memory/robot_memory.cpp
@@ -118,12 +118,12 @@ QResCursor RobotMemory::query(Query query, std::string collection)
   check_collection_name(collection);
   log_deb(std::string("Executing Query "+ query.toString() +" on collection "+collection));
 
-  //lock (mongo_client not thread safe)
-  MutexLocker lock(mutex_);
-
   //check if computation on demand is necessary and execute Computables
   computables_manager_->check_and_compute(query, collection);
 
+  //lock (mongo_client not thread safe)
+  MutexLocker lock(mutex_);
+
   //set read preference of query to nearest to read from the local replica set member first
   query.readPref(ReadPreference_Nearest, BSONArray());
 
diff --git a/src/plugins/robot-memory/robot_memory_thread.cpp b/src/plugins/robot-memory/robot_memory_thread.cpp
index 5cf279e..fb2559a 100644
--- a/src/plugins/robot-memory/robot_memory_thread.cpp
+++ b/src/plugins/robot-memory/robot_memory_thread.cpp
@@ -59,8 +59,9 @@ RobotMemoryThread::init()
   //prepare aspect initializer
   robot_memory_inifin_.set_robot_memory(robot_memory);
 
-  //register blackboard computable
+  //register computables
   blackboard_computable = new BlackboardComputable(robot_memory, blackboard, logger);
+  transform_computable = new TransformComputable(robot_memory, tf_listener, logger, config);
 }
 
 
@@ -68,6 +69,7 @@ void
 RobotMemoryThread::finalize()
 {
   delete blackboard_computable;
+  delete transform_computable;
   robot_memory_inifin_.set_robot_memory(NULL);
   delete robot_memory;
 }
diff --git a/src/plugins/robot-memory/robot_memory_thread.h b/src/plugins/robot-memory/robot_memory_thread.h
index 23ce0ef..80bb7d2 100644
--- a/src/plugins/robot-memory/robot_memory_thread.h
+++ b/src/plugins/robot-memory/robot_memory_thread.h
@@ -30,11 +30,13 @@
 #include <plugins/mongodb/aspect/mongodb.h>
 #include <aspect/blackboard.h>
 #include <aspect/blocked_timing.h>
+#include <aspect/tf.h>
 #include <mongo/client/dbclient.h>
 #include <aspect/aspect_provider.h>
 #include "robot_memory.h"
 #include "aspect/robot_memory_inifin.h"
 #include "computables/blackboard_computable.h"
+#include "computables/transform_computable.h"
 
 #include <string>
 
@@ -51,6 +53,7 @@ class RobotMemoryThread
   public fawkes::MongoDBAspect,
 	public fawkes::BlockedTimingAspect,
   public fawkes::BlackBoardAspect,
+  public fawkes::TransformAspect,
   public fawkes::AspectProviderAspect
 {
  public:
@@ -68,6 +71,7 @@ class RobotMemoryThread
    RobotMemory* robot_memory;
    fawkes::RobotMemoryIniFin robot_memory_inifin_;
    BlackboardComputable* blackboard_computable;
+   TransformComputable* transform_computable;
 };
 
 #endif
diff --git a/src/plugins/robot-memory/test-plugin/robot_memory_test.cpp b/src/plugins/robot-memory/test-plugin/robot_memory_test.cpp
index 823abfc..ee138f1 100644
--- a/src/plugins/robot-memory/test-plugin/robot_memory_test.cpp
+++ b/src/plugins/robot-memory/test-plugin/robot_memory_test.cpp
@@ -23,6 +23,7 @@
 #include <interfaces/Position3DInterface.h>
 #include <list>
 #include <algorithm>
+#include <math.h>
 
 using namespace fawkes;
 using namespace mongo;
@@ -287,16 +288,16 @@ TEST_F(RobotMemoryTest, ComputableMultiple)
 
 TEST_F(RobotMemoryTest, BlackboardComputable)
 {
-  Position3DInterface* if3d = blackboard->open_for_writing<Position3DInterface>("test");
+  Position3DInterface* if3d = blackboard->open_for_writing<Position3DInterface>("test1");
   if3d->set_frame("test_frame");
   if3d->set_translation(0, 1.1);
   if3d->set_translation(1, 2.2);
   if3d->set_translation(2, 3.3);
   if3d->write();
-  QResCursor qres = robot_memory->query(fromjson("{interface:'Position3DInterface',id:'test'}"), "robmem.blackboard");
+  QResCursor qres = robot_memory->query(fromjson("{interface:'Position3DInterface',id:'test1'}"), "robmem.blackboard");
   ASSERT_TRUE(qres->more());
   ASSERT_TRUE(contains_pairs(qres->next(), fromjson(
-      "{interface:'Position3DInterface',id:'test',frame:'test_frame',translation:[1.1, 2.2, 3.3]}")));
+      "{interface:'Position3DInterface',id:'test1',frame:'test_frame',translation:[1.1, 2.2, 3.3]}")));
   blackboard->close(if3d);
 }
 
@@ -317,3 +318,15 @@ TEST_F(RobotMemoryTest, BlackboardComputableMultiple)
   blackboard->close(if3d);
   blackboard->close(if3d_2);
 }
+
+
+TEST_F(RobotMemoryTest, TransformComputable)
+{
+  robot_memory->insert("{name:'test pos', frame:'cam_tag', translation:[0.0, 0.0, 0.0], rotation:[0.0, 0.0, 0.0, 1.0]}", "robmem.test");
+  QResCursor qres = robot_memory->query(fromjson("{name:'test pos', frame:'base_link', allow_tf:true}"), "robmem.test");
+  ASSERT_TRUE(qres->more());
+  BSONObj res = qres->next();
+  ASSERT_EQ("base_link", res.getField("frame").String());
+  ASSERT_TRUE(fabs(0.1 - res.getField("translation").Array()[0].Double()) < 0.001);
+  ASSERT_TRUE(fabs(-0.5 - res.getField("rotation").Array()[0].Double()) < 0.001);
+}
diff --git a/src/tools/gtest/gtest_fawkes.cpp b/src/tools/gtest/gtest_fawkes.cpp
index 6c89db3..e601ad5 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], "m-robot-memory,robot_memory_test");
+    strcpy(fawkes_argv[2], "static-transforms,m-robot-memory,robot_memory_test");
     fawkes_argv[3] = new char[2];
     strcpy(fawkes_argv[3], "-c");
     fawkes_argv[4] = new char[128];

- *commit* c909ed2f6a3ada8913d2b8eba7359de2b97ab122 - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Wed Nov 23 13:42:02 2016 +0100
Subject: robot-memory: fixed template function for trigger registration

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

_Diff for modified files_:
diff --git a/src/plugins/robot-memory/robot_memory.h b/src/plugins/robot-memory/robot_memory.h
index bda35ba..a701b57 100644
--- a/src/plugins/robot-memory/robot_memory.h
+++ b/src/plugins/robot-memory/robot_memory.h
@@ -86,7 +86,7 @@ class RobotMemory
     template<typename T>
     EventTrigger* register_trigger(std::string query_str, std::string collection, void(T::*callback)(mongo::BSONObj), T *_obj)
     {
-      return register_trigger(mongo::fromjson(query_str), collection, callback);
+      return register_trigger(mongo::fromjson(query_str), collection, callback, _obj);
     }
     void remove_trigger(EventTrigger* trigger);
 




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


More information about the fawkes-commits mailing list