(laser) reduce tf log/migrate to tf2 (#59)

Author: MatthijsBurgh

ed_sensor_integration/CMakeLists.txt

@@ -7,6 +7,8 @@ find_package(catkin REQUIRED COMPONENTS
   geolib2
   image_geometry
   sensor_msgs
+  tf2
+  tf2_ros
   tue_config
   tue_filesystem
   visualization_msgs

ed_sensor_integration/package.xml

@@ -18,6 +18,8 @@
   <depend>geolib2</depend>
   <depend>image_geometry</depend>
   <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
   <depend>tue_config</depend>
   <depend>tue_filesystem</depend>
   <depend>visualization_msgs</depend>

ed_sensor_integration/src/laser/laser_plugin.cpp

@@ -9,16 +9,16 @@
 #include <ed/update_request.h>
 #include <ed/world_model.h>
 
-#include <geolib/ros/tf_conversions.h>
+#include <geolib/ros/msg_conversions.h>
 #include <geolib/Shape.h>
 
-#include <tue/profiling/timer.h>
-
 #include <opencv2/imgproc/imgproc.hpp>
 
 #include <ros/console.h>
 #include <ros/node_handle.h>
 
+#include "tf2/transform_datatypes.h"
+
 #include <iostream>
 
 
@@ -45,13 +45,12 @@ void lasermsgToSensorRanges(const sensor_msgs::LaserScan::ConstPtr& scan, std::v
     }
 }
 
-LaserPlugin::LaserPlugin() : tf_listener_(0)
+LaserPlugin::LaserPlugin() : tf_buffer_(), tf_listener_(nullptr)
 {
 }
 
 LaserPlugin::~LaserPlugin()
 {
-    delete tf_listener_;
 }
 
 void LaserPlugin::initialize(ed::InitData& init)
@@ -71,7 +70,8 @@ void LaserPlugin::initialize(ed::InitData& init)
     // Communication
     sub_scan_ = nh.subscribe<sensor_msgs::LaserScan>(laser_topic, 3, &LaserPlugin::scanCallback, this);
 
-    tf_listener_ = new tf::TransformListener;
+    if (!tf_listener_)
+        tf_listener_ = std::make_unique<tf2_ros::TransformListener>(tf_buffer_);
 
     //pose_cache.clear();
 }
@@ -89,24 +89,25 @@ void LaserPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req)
 
         try
         {
-            tf::StampedTransform t_sensor_pose;
-            tf_listener_->lookupTransform("map", scan->header.frame_id, scan->header.stamp, t_sensor_pose);
+            geometry_msgs::TransformStamped gm_sensor_pose;
+            gm_sensor_pose = tf_buffer_.lookupTransform("map", scan->header.frame_id, scan->header.stamp);
             scan_buffer_.pop();
             geo::Pose3D sensor_pose;
-            geo::convert(t_sensor_pose, sensor_pose);
+            geo::convert(gm_sensor_pose.transform, sensor_pose);
             update(world, scan, sensor_pose, req);
         }
-        catch(tf::ExtrapolationException& ex)
+        catch(tf2::ExtrapolationException& ex)
         {
-            ROS_WARN_STREAM_DELAYED_THROTTLE(10, "ED Laserplugin: " << ex.what());
+            ROS_WARN_STREAM_DELAYED_THROTTLE(60, "ED Laserplugin: " << ex.what());
+            ROS_DEBUG_STREAM("ED Laserplugin: " << ex.what());
             try
             {
                 // 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)
-                tf::StampedTransform latest_transform;
-                tf_listener_->lookupTransform("map", scan->header.frame_id, ros::Time(0), latest_transform);
+                geometry_msgs::TransformStamped latest_transform;
+                latest_transform = tf_buffer_.lookupTransform("map", scan->header.frame_id, ros::Time(0));
 
-                if (scan_buffer_.front()->header.stamp > latest_transform.stamp_)
+                if (scan_buffer_.front()->header.stamp > latest_transform.header.stamp)
                 {
                     // Scan is too new
                     break;
@@ -117,12 +118,12 @@ void LaserPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req)
                     scan_buffer_.pop();
                 }
             }
-            catch(tf::TransformException& exc)
+            catch(tf2::TransformException& exc)
             {
                 scan_buffer_.pop();
             }
         }
-        catch(tf::TransformException& exc)
+        catch(tf2::TransformException& exc)
         {
             ROS_ERROR_STREAM_DELAYED_THROTTLE(10, "ED Laserplugin: " << exc.what());
             scan_buffer_.pop();
@@ -133,9 +134,6 @@ void LaserPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req)
 void LaserPlugin::update(const ed::WorldModel& world, const sensor_msgs::LaserScan::ConstPtr& scan,
                          const geo::Pose3D& sensor_pose, ed::UpdateRequest& req)
 {
-    tue::Timer t_total;
-    t_total.start();
-
     // - - - - - - - - - - - - - - - - - -
     // Update laser model
 

ed_sensor_integration/src/laser/laser_plugin.h

@@ -10,11 +10,13 @@
 
 #include <ros/subscriber.h>
 #include <ros/callback_queue.h>
-#include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 #include <sensor_msgs/LaserScan.h>
 
 #include <queue>
 #include <map>
+#include <memory>
 
 class LaserPlugin : public ed::Plugin
 {
@@ -38,14 +40,15 @@ class LaserPlugin : public ed::Plugin
 
     std::queue<sensor_msgs::LaserScan::ConstPtr> scan_buffer_;
 
-    tf::TransformListener* tf_listener_;
+    tf2_ros::Buffer tf_buffer_;
+    std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 
     LaserUpdater updater_;
 
     void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
 
     /**
-     * @brief update update the worldmodel based on a novel laserscan message.
+     * @brief Update the worldmodel based on a novel laserscan message.
      *
      * @param[in] world worldmodel to be updated
      * @param[in] scan laserscan message