Merge branch 'master' into feature/add_logging

ed_sensor_integration/CMakeLists.txt

@@ -5,9 +5,9 @@ find_package(catkin REQUIRED COMPONENTS
   ed
   ed_sensor_integration_msgs
   geolib2
+  geometry_msgs
   image_geometry
   sensor_msgs
-  tf2
   tf2_ros
   tue_config
   tue_filesystem

ed_sensor_integration/include/ed/kinect/fitter.h

@@ -233,11 +233,11 @@ class Fitter
 
     /**
      * @brief checkExpectedBeamThroughEntity checks if the expected center beam passes through the entity. If
-     * not: something went wrong
+     * not: throw a fitter error
      * @param model_ranges
      * @param entity
      * @param sensor_pose_xya
-     * @param expected_center_beam
+     * @param expected_center_beam expected index of the beam through the center of the object. range: any int. indices outside bounds will also throw an error.
      */
     void checkExpectedBeamThroughEntity(const std::vector<double> &model_ranges, ed::EntityConstPtr entity,
                                         const geo::Pose3D &sensor_pose_xya, const int expected_center_beam) const;

ed_sensor_integration/include/ed/kinect/image_buffer.h

@@ -2,9 +2,13 @@
 #define ED_SENSOR_INTEGRATION_IMAGE_BUFFER_H_
 
 #include <ros/callback_queue.h>
-#include <rgbd/types.h>
+
 #include <geolib/datatypes.h>
 
+#include <rgbd/types.h>
+
+#include <tf2_ros/buffer.h>
+
 #include <forward_list>
 #include <memory>
 #include <mutex>
@@ -15,7 +19,7 @@ namespace rgbd
 class Client;
 }
 
-namespace tf
+namespace tf2_ros
 {
 class TransformListener;
 }
@@ -66,7 +70,8 @@ class ImageBuffer
 
     std::unique_ptr<rgbd::Client> rgbd_client_;
 
-    std::unique_ptr<tf::TransformListener> tf_listener_;
+    tf2_ros::Buffer tf_buffer_;
+    std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 
     /**
      * @brief Newer images should be pushed on the front. This will result in the front being the most recent and the back being the oldest

ed_sensor_integration/package.xml

@@ -16,9 +16,9 @@
   <depend>ed</depend>
   <depend>ed_sensor_integration_msgs</depend>
   <depend>geolib2</depend>
+  <depend>geometry_msgs</depend>
   <depend>image_geometry</depend>
   <depend>sensor_msgs</depend>
-  <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tue_config</depend>
   <depend>tue_filesystem</depend>

ed_sensor_integration/src/kinect/fitter.cpp

@@ -377,6 +377,8 @@ std::unique_ptr<OptimalFit> Fitter::findOptimum(const EstimationInputData& input
     }
     if (valid_optimum)
         return current_optimum;
+
+    ROS_ERROR_NAMED("fitter", "optimum is invalid");
     std::unique_ptr<OptimalFit> invalid_optimum(new OptimalFit);
     return invalid_optimum;
 }
@@ -462,7 +464,8 @@ void Fitter::checkExpectedBeamThroughEntity(const std::vector<double>& model_ran
     expected_ranges = model_ranges;
     std::vector<int> expected_identifiers(nr_data_points_, 0);
     renderEntity(entity, sensor_pose_xya, 1, expected_ranges, expected_identifiers);
-
+    if (expected_center_beam < 0 || expected_center_beam >= nr_data_points_)
+        throw FitterError("Expected beam outside of measurement range(" + std::to_string(nr_data_points_) + "), index: " + std::to_string(expected_center_beam));
     if (expected_identifiers[expected_center_beam] != 1)  // expected center beam MUST contain the rendered model
         throw FitterError("Expected beam does not go through entity");
 }

ed_sensor_integration/src/kinect/image_buffer.cpp

@@ -1,13 +1,17 @@
 #include "ed/kinect/image_buffer.h"
 
+#include <geolib/ros/msg_conversions.h>
+
+#include <geometry_msgs/TransformStamped.h>
+
 #include <rgbd/client.h>
 #include <rgbd/image.h>
-#include <tf/transform_listener.h>
-#include <geolib/ros/tf_conversions.h>
+
+#include <tf2_ros/transform_listener.h>
 
 // ----------------------------------------------------------------------------------------------------
 
-ImageBuffer::ImageBuffer() : rgbd_client_(nullptr), tf_listener_(nullptr), shutdown_(false)
+ImageBuffer::ImageBuffer() : rgbd_client_(nullptr), tf_buffer_(), tf_listener_(nullptr), shutdown_(false)
 {
 }
 
@@ -32,7 +36,7 @@ void ImageBuffer::initialize(const std::string& topic, const std::string& root_f
     rgbd_client_->initialize(topic);
 
     if (!tf_listener_)
-        tf_listener_ = std::make_unique<tf::TransformListener>();
+        tf_listener_ = std::make_unique<tf2_ros::TransformListener>(tf_buffer_);
 
     worker_thread_ptr_ = std::make_unique<std::thread>(&ImageBuffer::workerThreadFunc, this, worker_thread_frequency);
 }
@@ -73,8 +77,8 @@ bool ImageBuffer::waitForRecentImage(rgbd::ImageConstPtr& image, geo::Pose3D& se
 
     // - - - - - - - - - - - - - - - - - -
     // Wait until we have a tf
-    if (!tf_listener_->canTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()))) // Get the TF when it is available now
-        if (!tf_listener_->waitForTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_end - ros::Time::now()))
+    if (!tf_buffer_.canTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()))) // Get the TF when it is available now
+        if (!tf_buffer_.canTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_end - ros::Time::now()))
         {
             ROS_ERROR_NAMED("image_buffer", "[IMAGE_BUFFER] timeout waiting for tf");
             return false;
@@ -85,11 +89,10 @@ bool ImageBuffer::waitForRecentImage(rgbd::ImageConstPtr& image, geo::Pose3D& se
 
     try
     {
-        tf::StampedTransform t_sensor_pose;
-        tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_sensor_pose);
-        geo::convert(t_sensor_pose, sensor_pose);
+        geometry_msgs::TransformStamped t_sensor_pose = tf_buffer_.lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()));
+        geo::convert(t_sensor_pose.transform, sensor_pose);
     }
-    catch(tf::TransformException& ex)
+    catch(tf2::TransformException& ex)
     {
         ROS_ERROR_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());
         return false;
@@ -155,20 +158,18 @@ bool ImageBuffer::getMostRecentImageTF()
         rgbd::ImageConstPtr& rgbd_image = *it;
         try
         {
-            tf::StampedTransform t_sensor_pose;
-            tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_sensor_pose);
-            geo::convert(t_sensor_pose, sensor_pose);
+            geometry_msgs::TransformStamped t_sensor_pose = tf_buffer_.lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()));
+            geo::convert(t_sensor_pose.transform, sensor_pose);
         }
-        catch (tf::ExtrapolationException& ex)
+        catch (tf2::ExtrapolationException& ex)
         {
             try
             {
                 // Now we have to check if the error was an interpolation or extrapolation error (i.e., the image is too old or
                 // to new, respectively). If it is too old, discard it.
-                tf::StampedTransform latest_sensor_pose;
-                tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(0), latest_sensor_pose);
+                geometry_msgs::TransformStamped latest_sensor_pose = tf_buffer_.lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(0));
                 // If image time stamp is older than latest transform, the image is too old and the tf data is not available anymore
-                if ( latest_sensor_pose.stamp_ > ros::Time(rgbd_image->getTimestamp()) )
+                if ( latest_sensor_pose.header.stamp > ros::Time(rgbd_image->getTimestamp()) )
                 {
 
                     ROS_DEBUG_STREAM_NAMED("image_buffer", "[IMAGE_BUFFER] Image too old to look-up tf. Deleting all images older than timestamp: " << std::fixed
@@ -184,14 +185,14 @@ bool ImageBuffer::getMostRecentImageTF()
                     continue;
                 }
             }
-            catch (tf::TransformException& ex)
+            catch (tf2::TransformException& ex)
             {
                 ROS_ERROR_DELAYED_THROTTLE_NAMED(10, "image_buffer", "[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what());
                 ROS_WARN_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what());
                 continue;
             }
         }
-        catch (tf::TransformException& ex)
+        catch (tf2::TransformException& ex)
         {
             ROS_ERROR_DELAYED_THROTTLE_NAMED(10, "image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());
             ROS_WARN_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());

ed_sensor_integration/src/laser/laser_plugin.cpp

@@ -12,12 +12,14 @@
 #include <geolib/ros/msg_conversions.h>
 #include <geolib/Shape.h>
 
+#include <geometry_msgs/TransformStamped.h>
+
 #include <opencv2/imgproc/imgproc.hpp>
 
 #include <ros/console.h>
 #include <ros/node_handle.h>
 
-#include "tf2/transform_datatypes.h"
+#include <tf2_ros/transform_listener.h>
 
 #include <iostream>
 

ed_sensor_integration/src/laser/laser_plugin.h

@@ -11,13 +11,17 @@
 #include <ros/subscriber.h>
 #include <ros/callback_queue.h>
 #include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
 #include <sensor_msgs/LaserScan.h>
 
 #include <queue>
 #include <map>
 #include <memory>
 
+namespace tf2_ros
+{
+class TransformListener;
+}
+
 class LaserPlugin : public ed::Plugin
 {