Share TF listener across ED

Author: MatthijsBurgh

CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
   rgbd_msgs
   rosconsole_bridge
   roscpp
+  tf2_ros
   tue_config
   tue_filesystem
   tue_serialization
@@ -35,7 +36,7 @@ find_package(TinyXML2 REQUIRED)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}_core ${PROJECT_NAME}_io ${PROJECT_NAME}_server ${PROJECT_NAME}_hello_world_plugin ${PROJECT_NAME}_custom_properties_plugin
-  CATKIN_DEPENDS code_profiler ${PROJECT_NAME}_msgs geolib2 pluginlib rgbd rgbd_msgs roscpp tue_config tue_filesystem tue_serialization
+  CATKIN_DEPENDS code_profiler ${PROJECT_NAME}_msgs geolib2 pluginlib rgbd rgbd_msgs roscpp tf2_ros tue_config tue_filesystem tue_serialization
   DEPENDS OpenCV PCL
 )
 
@@ -129,10 +130,9 @@ target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_core ${PROJECT_NAME}_io ${
 
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-find_package(tf2_ros REQUIRED)
 add_library(${PROJECT_NAME}_tf_publisher_plugin plugins/tf_publisher_plugin.cpp)
-target_include_directories(${PROJECT_NAME}_tf_publisher_plugin BEFORE PRIVATE SYSTEM ${tf2_geometry_msgs_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS} ${tf2_ros_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME}_tf_publisher_plugin ${tf2_geometry_msgs_LIBRARIES} ${tf2_LIBRARIES} ${tf2_ros_LIBRARIES} ${catkin_LIBRARIES})
+target_include_directories(${PROJECT_NAME}_tf_publisher_plugin BEFORE PRIVATE SYSTEM ${tf2_geometry_msgs_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS})
+target_link_libraries(${PROJECT_NAME}_tf_publisher_plugin ${tf2_geometry_msgs_LIBRARIES} ${tf2_LIBRARIES} ${catkin_LIBRARIES})
 
 find_package(kdl_parser REQUIRED)
 add_library(${PROJECT_NAME}_robot_plugin plugins/robot_plugin.cpp)

include/ed/plugin.h

@@ -6,14 +6,15 @@
 
 #include <tue/config/configuration.h>
 
+#include "ed/types.h"
 #include "ed/init_data.h"
 
+#include <tf2_ros/buffer.h>
+
 #include <vector>
 
-namespace ed {
 
-class WorldModel;
-class UpdateRequest;
+namespace ed {
 
 struct PluginInput
 {
@@ -44,6 +45,10 @@ class Plugin
 
     const std::string& name() const { return name_; }
 
+protected:
+
+    TFBufferConstPtr tf_buffer_;
+
 private:
 
     std::string name_;

include/ed/plugin_container.h

@@ -27,7 +27,7 @@ class PluginContainer
 
 public:
 
-    PluginContainer();
+    PluginContainer(const ed::TFBufferConstPtr& tf_buffer_);
 
     virtual ~PluginContainer();
 
@@ -111,6 +111,8 @@ class PluginContainer
 
     WorldModelConstPtr world_current_;
 
+    TFBufferConstPtr tf_buffer_;
+
     std::unique_ptr<ed::LoopUsageStatus> loop_usage_status_;
 
     bool step();

include/ed/server.h

@@ -3,22 +3,29 @@
 
 #include "ed/types.h"
 
-#include <diagnostic_updater/diagnostic_updater.h>
+#include "ed/property_key_db.h"
+#include <ed/models/model_loader.h>
 
-#include <tf/transform_listener.h>
 
-#include <ros/publisher.h>
+#include <diagnostic_updater/diagnostic_updater.h>
 
-#include <ed/models/model_loader.h>
+#include <ros/publisher.h>
 
-#include "ed/property_key_db.h"
+#include <tf2_ros/buffer.h>
 
 #include "tue/config/configuration.h"
 
 #include <map>
 #include <queue>
 #include <vector>
 
+namespace tf2_ros
+{
+
+class TransformListener;
+
+}
+
 namespace ed
 {
 
@@ -89,6 +96,10 @@ class Server
     //! Profiling
     diagnostic_updater::Updater updater_;
     ros::Publisher pub_stats_;
+
+    TFBufferPtr tf_buffer_;
+    TFBufferConstPtr tf_buffer_const_;
+    ed::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 };
 
 }

include/ed/types.h

@@ -9,6 +9,12 @@
 #include <limits>
 #include <stdint.h>
 
+namespace tf2_ros {
+
+class Buffer;
+
+}
+
 namespace ed
 {
 
@@ -73,6 +79,10 @@ class UUID;
 
 typedef std::string TYPE;
 
+// tf2_ros::Buffer
+typedef shared_ptr<tf2_ros::Buffer> TFBufferPtr;
+typedef shared_ptr<const tf2_ros::Buffer> TFBufferConstPtr;
+
 }
 
 #endif

src/plugin_container.cpp

@@ -1,7 +1,8 @@
 #include "ed/plugin_container.h"
 
 #include <ed/error_context.h>
-#include <ed/plugin.h>
+#include "ed/init_data.h"
+#include "ed/plugin.h"
 
 #include <pluginlib/class_loader.h>
 
@@ -12,8 +13,9 @@ namespace ed
 
 // --------------------------------------------------------------------------------
 
-PluginContainer::PluginContainer()
-    : class_loader_(nullptr), request_stop_(false), is_running_(false), cycle_duration_(0.1), loop_frequency_(10), loop_frequency_max_(11), loop_frequency_min_(9), step_finished_(true), t_last_update_(0),
+PluginContainer::PluginContainer(const TFBufferConstPtr& tf_buffer)
+    : class_loader_(nullptr), request_stop_(false), is_running_(false), cycle_duration_(0.1), loop_frequency_(10),
+      loop_frequency_max_(11), loop_frequency_min_(9), step_finished_(true), t_last_update_(0), tf_buffer_(tf_buffer),
       loop_usage_status_(nullptr)
 {
 }
@@ -51,6 +53,7 @@ PluginPtr PluginContainer::loadPlugin(const std::string& plugin_name, const std:
         {
             name_ = plugin_name;
             plugin_->name_ = plugin_name;
+            plugin_->tf_buffer_ = tf_buffer_;
 
             configure(init, false);
 

src/server.cpp

@@ -1,29 +1,27 @@
 #include "ed/server.h"
 
 #include "ed/entity.h"
+#include "ed/error_context.h"
 #include "ed/measurement.h"
-
-#include <geolib/Box.h>
+#include "ed/plugin.h"
+#include "ed/plugin_container.h"
+#include "ed/types.h"
+#include "ed/world_model.h"
 
 // Storing measurements to disk
 #include "ed/io/filesystem/write.h"
 
-#include <tue/filesystem/path.h>
-
-#include "ed/plugin.h"
-#include "ed/plugin_container.h"
-#include "ed/world_model.h"
+#include "ed/serialization/serialization.h"
 
+#include <tue/config/writer.h>
 #include <tue/config/loaders/yaml.h>
 
-#include <boost/make_shared.hpp>
+#include <tue/filesystem/path.h>
 
 #include <std_msgs/String.h>
 
-#include "ed/serialization/serialization.h"
-#include <tue/config/writer.h>
-
-#include "ed/error_context.h"
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 namespace ed
 {
@@ -33,6 +31,10 @@ namespace ed
 Server::Server() : world_model_(new WorldModel(&property_key_db_))
 {
     updater_.setHardwareID("none");
+
+    tf_buffer_ = ed::make_shared<tf2_ros::Buffer>();
+    tf_buffer_const_ = ed::const_pointer_cast<const tf2_ros::Buffer>(tf_buffer_);
+    tf_listener_ = ed::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 }
 
 // ----------------------------------------------------------------------------------------------------