Use shared tf listener (#83)

Author: MatthijsBurgh

ed_sensor_integration/CMakeLists.txt

@@ -18,7 +18,6 @@ find_package(catkin REQUIRED COMPONENTS
   rosconsole_bridge
   roscpp
   sensor_msgs
-  tf2_ros
   tue_config
   tue_filesystem
   visualization_msgs

ed_sensor_integration/package.xml

@@ -33,8 +33,6 @@
   <exec_depend>rosconsole_bridge</exec_depend>
   <build_depend>sensor_msgs</build_depend>
   <exec_depend>sensor_msgs</exec_depend>
-  <build_depend>tf2_ros</build_depend>
-  <exec_depend>tf2_ros</exec_depend>
   <build_depend>tue_filesystem</build_depend>
   <exec_depend>tue_filesystem</exec_depend>
 

ed_sensor_integration/src/laser/laser_plugin.cpp

@@ -19,8 +19,6 @@
 #include <ros/console.h>
 #include <ros/node_handle.h>
 
-#include <tf2_ros/transform_listener.h>
-
 #include <iostream>
 
 
@@ -47,7 +45,7 @@ void lasermsgToSensorRanges(const sensor_msgs::LaserScan::ConstPtr& scan, std::v
     }
 }
 
-LaserPlugin::LaserPlugin() : tf_buffer_(), tf_listener_(nullptr)
+LaserPlugin::LaserPlugin()
 {
 }
 
@@ -71,11 +69,6 @@ void LaserPlugin::initialize(ed::InitData& init)
 
     // Communication
     sub_scan_ = nh.subscribe<sensor_msgs::LaserScan>(laser_topic, 3, &LaserPlugin::scanCallback, this);
-
-    if (!tf_listener_)
-        tf_listener_ = std::make_unique<tf2_ros::TransformListener>(tf_buffer_);
-
-    //pose_cache.clear();
 }
 
 void LaserPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req)
@@ -92,7 +85,7 @@ void LaserPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req)
         try
         {
             geometry_msgs::TransformStamped gm_sensor_pose;
-            gm_sensor_pose = tf_buffer_.lookupTransform("map", scan->header.frame_id, scan->header.stamp);
+            gm_sensor_pose = tf_buffer_->lookupTransform("map", scan->header.frame_id, scan->header.stamp);
             scan_buffer_.pop();
             geo::Pose3D sensor_pose;
             geo::convert(gm_sensor_pose.transform, sensor_pose);
@@ -107,7 +100,7 @@ void LaserPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req)
                 // Now we have to check if the error was an interpolation or extrapolation error
                 // (i.e., the scan is too old or too new, respectively)
                 geometry_msgs::TransformStamped latest_transform;
-                latest_transform = tf_buffer_.lookupTransform("map", scan->header.frame_id, ros::Time(0));
+                latest_transform = tf_buffer_->lookupTransform("map", scan->header.frame_id, ros::Time(0));
 
                 if (scan_buffer_.front()->header.stamp > latest_transform.header.stamp)
                 {

ed_sensor_integration/src/laser/laser_plugin.h

@@ -10,18 +10,12 @@
 
 #include <ros/subscriber.h>
 #include <ros/callback_queue.h>
-#include <tf2_ros/buffer.h>
 #include <sensor_msgs/LaserScan.h>
 
 #include <queue>
 #include <map>
 #include <memory>
 
-namespace tf2_ros
-{
-class TransformListener;
-}
-
 class LaserPlugin : public ed::Plugin
 {
 
@@ -44,9 +38,6 @@ class LaserPlugin : public ed::Plugin
 
     std::queue<sensor_msgs::LaserScan::ConstPtr> scan_buffer_;
 
-    tf2_ros::Buffer tf_buffer_;
-    std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
-
     LaserUpdater updater_;
 
     void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg);