[Fawkes Git] branch/labgramrs-3/navgraph-listener: created (0.5.0-3368-g86e6b9c)

Lab GRAMRS 2016 Group 3 labgramrs2016 at lists.kbsg.rwth-aachen.de
Tue Nov 22 16:54:48 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, labgramrs-3/navgraph-listener has been created
        at  86e6b9c84bd76a721f949f3d34bbb476749a070f (commit)

http://git.fawkesrobotics.org/fawkes.git/labgramrs-3/navgraph-listener

- *Log* ---------------------------------------------------------------
commit 86e6b9c84bd76a721f949f3d34bbb476749a070f
Author:     Felix Bier <felix.bier at rwth-aachen.de>
AuthorDate: Fri Nov 18 18:07:50 2016 +0100
Commit:     Nick Rossenbach <nick.rossenbach at rwth-aachen.de>
CommitDate: Tue Nov 22 16:52:49 2016 +0100

    implemented NavGraph listener

http://git.fawkesrobotics.org/fawkes.git/commit/86e6b9c
http://trac.fawkesrobotics.org/changeset/86e6b9c

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


- *Summary* -----------------------------------------------------------


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

- *commit* 86e6b9c84bd76a721f949f3d34bbb476749a070f - - - - - - - - - -
Author:  Felix Bier <felix.bier at rwth-aachen.de>
Date:    Fri Nov 18 18:07:50 2016 +0100
Subject: implemented NavGraph listener

 src/plugins/navgraph/navgraph_thread.cpp |   21 ++++++++++++
 src/plugins/navgraph/navgraph_thread.h   |   53 ++++++++++++++++++++++++++++++
 2 files changed, 74 insertions(+), 0 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/navgraph/navgraph_thread.cpp b/src/plugins/navgraph/navgraph_thread.cpp
index 4e96587..28de753 100644
--- a/src/plugins/navgraph/navgraph_thread.cpp
+++ b/src/plugins/navgraph/navgraph_thread.cpp
@@ -119,6 +119,27 @@ NavGraphThread::init()
     graph_ = LockPtr<NavGraph>(new NavGraph("generated"), /* recursive mutex */ true);
   }
 
+  //Launch NavgraphListener
+  //
+  graphListener = new BlackboardGraphListener(graph_, blackboard);
+  graph_->add_change_listener(graphListener);
+
+  //Dump Graph to Blackboard
+  //
+  //
+  /*std::vector<NavGraphNode> nodes = graph_->nodes();
+
+  std::vector<NavGraphNode>::iterator node_iterator;
+  for(node_iterator = nodes.begin(); node_iterator != nodes.end(); node_iterator++)
+  {
+    std::string node_name = node_iterator->name();
+    std::string interface_name = node_name + std::string("_position");
+    Position3DInterface* position_interface = blackboard->open_for_writing<Position3DInterface>(interface_name.c_str());
+    position_interface->set_translation(0, node_iterator->x());
+    position_interface->set_translation(1, node_iterator->y());
+    position_interface->write()
+  }*/
+
   if (! graph_->has_default_property("travel_tolerance")) {
     throw Exception("Graph must specify travel tolerance");
   }
diff --git a/src/plugins/navgraph/navgraph_thread.h b/src/plugins/navgraph/navgraph_thread.h
index a2b51b1..ce875a4 100644
--- a/src/plugins/navgraph/navgraph_thread.h
+++ b/src/plugins/navgraph/navgraph_thread.h
@@ -37,6 +37,7 @@
 
 #include <interfaces/NavigatorInterface.h>
 #include <interfaces/NavPathInterface.h>
+#include <interfaces/Position3DInterface.h>
 
 #include <navgraph/navgraph.h>
 #include <utils/system/fam.h>
@@ -47,6 +48,54 @@ namespace fawkes {
   class Time;
 }
 
+//BlackboardGraphListener
+//
+
+using namespace fawkes;
+using namespace navgraph;
+
+class BlackboardGraphListener : public NavGraph::ChangeListener
+{
+  private:
+    LockPtr<NavGraph> graph_;
+    BlackBoard* blackboard_;
+    std::map<std::string, Position3DInterface*> interface_map;  
+
+  public:
+
+  BlackboardGraphListener(LockPtr<NavGraph> graph, BlackBoard* blackboard) {
+    graph_ = graph;
+    blackboard_ = blackboard;
+  }
+
+  void graph_changed() throw(){
+    std::vector<NavGraphNode> nodes = graph_->nodes();
+
+    std::vector<NavGraphNode>::iterator node_iterator;
+    for(node_iterator = nodes.begin(); node_iterator != nodes.end(); node_iterator++)
+    {
+      if(node_iterator->property_as_bool(std::string("generated")) == false)
+      {
+        std::string node_name = node_iterator->name();
+        std::string interface_name = node_name + std::string("_position");
+        Position3DInterface* position_interface;
+        if(interface_map.find(interface_name) == interface_map.end()) {
+          position_interface = blackboard_->open_for_writing<Position3DInterface>(interface_name.c_str());
+          interface_map[interface_name] = position_interface;
+        } else {
+          position_interface = interface_map[interface_name];
+        }
+        
+        position_interface->set_translation(0, node_iterator->x());
+        position_interface->set_translation(1, node_iterator->y());
+        position_interface->write();
+      }
+    }
+  }
+
+};
+
+
 class NavGraphThread
 : public fawkes::Thread,
   public fawkes::ClockAspect,
@@ -76,6 +125,10 @@ class NavGraphThread
  protected: virtual void run() { Thread::run();}
 
  private:
+  //new listener
+  //
+  BlackboardGraphListener* graphListener;
+
   bool generate_plan(std::string goal);
   bool generate_plan(std::string goal, float ori);
   bool generate_plan(float x, float y, float ori);




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


More information about the fawkes-commits mailing list