Merge pull request #1 from element-robotics/feature/tf2

Upgrade to tf2

Author: tenzincrouch

README.md

@@ -145,7 +145,7 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens
 
     The robot pose and covariance.
 
-* **`/tf`** ([tf/tfMessage])
+* **`/tf`** ([tf2_msgs/TFMessage])
 
     The transformation tree.
 
@@ -432,7 +432,7 @@ Please report bugs and request features using the [Issue Tracker](https://github
 [grid_map_msgs/GridMap]: https://github.com/anybotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg
 [sensor_msgs/PointCloud2]: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html
 [geometry_msgs/PoseWithCovarianceStamped]: http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html
-[tf/tfMessage]: http://docs.ros.org/kinetic/api/tf/html/msg/tfMessage.html
+[tf2_msgs/TFMessage]: http://docs.ros.org/en/noetic/api/tf2_msgs/html/msg/TFMessage.html
 [std_srvs/Empty]: http://docs.ros.org/api/std_srvs/html/srv/Empty.html
 [grid_map_msgs/GetGridMap]: https://github.com/anybotics/grid_map/blob/master/grid_map_msgs/srv/GetGridMap.srv
 [grid_map_msgs/ProcessFile]: https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/srv/ProcessFile.srv

elevation_mapping/CMakeLists.txt

@@ -7,7 +7,7 @@ add_compile_options(-Wall -Wextra -Wpedantic)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 set(
-  CATKIN_PACKAGE_DEPENDENCIES 
+  CATKIN_PACKAGE_DEPENDENCIES
     eigen_conversions
     grid_map_core
     grid_map_ros
@@ -20,11 +20,13 @@ set(
     roscpp
     sensor_msgs
     std_msgs
-    tf
-    tf_conversions
+    tf2
+    tf2_ros
+    tf2_eigen
+    tf2_geometry_msgs
 )
 
-find_package(catkin REQUIRED 
+find_package(catkin REQUIRED
   COMPONENTS
     ${CATKIN_PACKAGE_DEPENDENCIES}
 )
@@ -111,7 +113,7 @@ target_link_libraries(${PROJECT_NAME}
 #############
 
 install(
-  TARGETS 
+  TARGETS
     ${PROJECT_NAME}
     ${PROJECT_NAME}_pcl_types
     ${PROJECT_NAME}_library

elevation_mapping/include/elevation_mapping/ElevationMapping.hpp

@@ -21,7 +21,8 @@
 #include <sensor_msgs/PointCloud2.h>
 #include <std_srvs/Empty.h>
 #include <std_srvs/Trigger.h>
-#include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 // Eigen
 #include <Eigen/Core>
@@ -298,8 +299,9 @@ class ElevationMapping {
   //! Cache for the robot pose messages.
   message_filters::Cache<geometry_msgs::PoseWithCovarianceStamped> robotPoseCache_;
 
-  //! TF listener and broadcaster.
-  tf::TransformListener transformListener_;
+  //! TF listener and buffer.
+  tf2_ros::Buffer transformBuffer_;
+  tf2_ros::TransformListener transformListener_;
 
   struct Parameters {
     //! Size of the cache for the robot pose messages.

elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp

@@ -10,7 +10,8 @@
 
 // ROS
 #include <ros/ros.h>
-#include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 // Eigen
 #include <Eigen/Core>
@@ -139,8 +140,9 @@ class SensorProcessorBase {
   //! ROS nodehandle.
   ros::NodeHandle& nodeHandle_;
 
-  //! TF transform listener.
-  tf::TransformListener transformListener_;
+  //! TF transform listener and buffer.
+  tf2_ros::Buffer transformBuffer_;
+  tf2_ros::TransformListener transformListener_;
 
   //! Rotation from Base to Sensor frame (C_SB)
   kindr::RotationMatrixD rotationBaseToSensor_;

elevation_mapping/package.xml

@@ -29,8 +29,10 @@
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
-  <depend>tf</depend>
-  <depend>tf_conversions</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
 
 <!--   <test_depend>cmake_code_coverage</test_depend> -->
   <test_depend>grid_map_filters</test_depend>

elevation_mapping/src/ElevationMapping.cpp

@@ -21,6 +21,10 @@
 #include <grid_map_ros/grid_map_ros.hpp>
 #include <kindr/Core>
 #include <kindr_ros/kindr_ros.hpp>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PointStamped.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include "elevation_mapping/ElevationMap.hpp"
 #include "elevation_mapping/ElevationMapping.hpp"
@@ -35,6 +39,7 @@ namespace elevation_mapping {
 ElevationMapping::ElevationMapping(ros::NodeHandle& nodeHandle)
     : nodeHandle_(nodeHandle),
       inputSources_(nodeHandle_),
+      transformListener_(transformBuffer_),
       map_(nodeHandle),
       robotMotionMapUpdater_(nodeHandle),
       receivedFirstMatchingPointcloudAndPose_(false) {
@@ -542,8 +547,8 @@ bool ElevationMapping::updateMapLocation() {
   geometry_msgs::PointStamped trackPointTransformed;
 
   try {
-    transformListener_.transformPoint(map_.getFrameId(), trackPoint, trackPointTransformed);
-  } catch (tf::TransformException& ex) {
+    trackPointTransformed = transformBuffer_.transform(trackPoint, map_.getFrameId());
+  } catch (tf2::TransformException& ex) {
     ROS_ERROR("%s", ex.what());
     return false;
   }
@@ -627,12 +632,14 @@ bool ElevationMapping::initializeElevationMap() {
   if (parameters.initializeElevationMap_) {
     if (static_cast<elevation_mapping::InitializationMethods>(parameters.initializationMethod_) ==
         elevation_mapping::InitializationMethods::PlanarFloorInitializer) {
-      tf::StampedTransform transform;
+      geometry_msgs::TransformStamped transform_msg;
+      tf2::Stamped<tf2::Transform> transform;
 
       // Listen to transform between mapFrameId_ and targetFrameInitSubmap_ and use z value for initialization
       try {
-        transformListener_.waitForTransform(parameters.mapFrameId_, parameters.targetFrameInitSubmap_, ros::Time(0), ros::Duration(5.0));
-        transformListener_.lookupTransform(parameters.mapFrameId_, parameters.targetFrameInitSubmap_, ros::Time(0), transform);
+        transform_msg = transformBuffer_.lookupTransform(parameters.mapFrameId_, parameters.targetFrameInitSubmap_, ros::Time(0), ros::Duration(5.0));
+        tf2::fromMsg(transform_msg, transform);
+
         ROS_DEBUG_STREAM("Initializing with x: " << transform.getOrigin().x() << " y: " << transform.getOrigin().y()
                                                  << " z: " << transform.getOrigin().z());
 
@@ -645,7 +652,7 @@ bool ElevationMapping::initializeElevationMap() {
         map_.setRawSubmapHeight(positionRobot, transform.getOrigin().z() + parameters.initSubmapHeightOffset_,
                                 parameters.initSubmapVariance_, parameters.lengthInXInitSubmap_, parameters.lengthInYInitSubmap_);
         return true;
-      } catch (tf::TransformException& ex) {
+      } catch (tf2::TransformException& ex) {
         ROS_DEBUG("%s", ex.what());
         ROS_WARN("Could not initialize elevation map with constant height. (This warning can be ignored if TF tree is not available.)");
         return false;

elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp

@@ -8,6 +8,9 @@
 
 #include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp"
 
+// ROS
+#include <geometry_msgs/TransformStamped.h>
+
 // PCL
 #include <pcl/common/io.h>
 #include <pcl/common/transforms.h>
@@ -17,7 +20,7 @@
 #include <pcl/pcl_base.h>
 
 // TF
-#include <tf_conversions/tf_eigen.h>
+#include <tf2_eigen/tf2_eigen.h>
 
 // STL
 #include <cmath>
@@ -29,7 +32,9 @@
 namespace elevation_mapping {
 
 SensorProcessorBase::SensorProcessorBase(ros::NodeHandle& nodeHandle, const GeneralParameters& generalConfig)
-    : nodeHandle_(nodeHandle), firstTfAvailable_(false) {
+    : nodeHandle_(nodeHandle),
+      transformListener_(transformBuffer_),
+      firstTfAvailable_(false) {
   pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
   transformationSensorToMap_.setIdentity();
   generalParameters_ = generalConfig;
@@ -92,31 +97,33 @@ bool SensorProcessorBase::process(const PointCloudType::ConstPtr pointCloudInput
 bool SensorProcessorBase::updateTransformations(const ros::Time& timeStamp) {
   const Parameters parameters{parameters_.getData()};
   try {
-    transformListener_.waitForTransform(sensorFrameId_, generalParameters_.mapFrameId_, timeStamp, ros::Duration(1.0));
-
-    tf::StampedTransform transformTf;
-    transformListener_.lookupTransform(generalParameters_.mapFrameId_, sensorFrameId_, timeStamp, transformTf);
-    poseTFToEigen(transformTf, transformationSensorToMap_);
 
-    transformListener_.lookupTransform(generalParameters_.robotBaseFrameId_, sensorFrameId_, timeStamp,
-                                       transformTf);  // TODO(max): Why wrong direction?
-    Eigen::Affine3d transform;
-    poseTFToEigen(transformTf, transform);
-    rotationBaseToSensor_.setMatrix(transform.rotation().matrix());
-    translationBaseToSensorInBaseFrame_.toImplementation() = transform.translation();
-
-    transformListener_.lookupTransform(generalParameters_.mapFrameId_, generalParameters_.robotBaseFrameId_, timeStamp,
-                                       transformTf);  // TODO(max): Why wrong direction?
-    poseTFToEigen(transformTf, transform);
-    rotationMapToBase_.setMatrix(transform.rotation().matrix());
-    translationMapToBaseInMapFrame_.toImplementation() = transform.translation();
+    geometry_msgs::TransformStamped transformGeom;
+    transformGeom = transformBuffer_.lookupTransform(generalParameters_.mapFrameId_, sensorFrameId_, timeStamp, ros::Duration(1.0));
+    transformationSensorToMap_ = tf2::transformToEigen(transformGeom);
+
+    transformGeom = transformBuffer_.lookupTransform(generalParameters_.robotBaseFrameId_, sensorFrameId_, timeStamp,
+                                                      ros::Duration(1.0));  // TODO(max): Why wrong direction?
+    Eigen::Quaterniond rotationQuaternion;
+    tf2::fromMsg(transformGeom.transform.rotation, rotationQuaternion);
+    rotationBaseToSensor_.setMatrix(rotationQuaternion.toRotationMatrix());
+    Eigen::Vector3d translationVector;
+    tf2::fromMsg(transformGeom.transform.translation, translationVector);
+    translationBaseToSensorInBaseFrame_.toImplementation() = translationVector;
+
+    transformGeom = transformBuffer_.lookupTransform(generalParameters_.mapFrameId_, generalParameters_.robotBaseFrameId_,
+                                                    timeStamp, ros::Duration(1.0));  // TODO(max): Why wrong direction?
+    tf2::fromMsg(transformGeom.transform.rotation, rotationQuaternion);
+    rotationMapToBase_.setMatrix(rotationQuaternion.toRotationMatrix());
+    tf2::fromMsg(transformGeom.transform.translation, translationVector);
+    translationMapToBaseInMapFrame_.toImplementation() = translationVector;
 
     if (!firstTfAvailable_) {
       firstTfAvailable_ = true;
     }
 
     return true;
-  } catch (tf::TransformException& ex) {
+  } catch (tf2::TransformException& ex) {
     if (!firstTfAvailable_) {
       return false;
     }
@@ -131,22 +138,20 @@ bool SensorProcessorBase::transformPointCloud(PointCloudType::ConstPtr pointClou
   timeStamp.fromNSec(1000 * pointCloud->header.stamp);
   const std::string inputFrameId(pointCloud->header.frame_id);
 
-  tf::StampedTransform transformTf;
   try {
-    transformListener_.waitForTransform(targetFrame, inputFrameId, timeStamp, ros::Duration(1.0), ros::Duration(0.001));
-    transformListener_.lookupTransform(targetFrame, inputFrameId, timeStamp, transformTf);
-  } catch (tf::TransformException& ex) {
+    geometry_msgs::TransformStamped transformGeom;
+    transformGeom = transformBuffer_.lookupTransform(targetFrame, inputFrameId, timeStamp, ros::Duration(1.0));  // FIXME: missing 0.001 retry duration
+    Eigen::Affine3d transform = tf2::transformToEigen(transformGeom);
+    pcl::transformPointCloud(*pointCloud, *pointCloudTransformed, transform.cast<float>());
+    pointCloudTransformed->header.frame_id = targetFrame;
+
+    ROS_DEBUG_THROTTLE(5, "Point cloud transformed to frame %s for time stamp %f.", targetFrame.c_str(),
+                      pointCloudTransformed->header.stamp / 1000.0);
+  } catch (tf2::TransformException& ex) {
     ROS_ERROR("%s", ex.what());
     return false;
   }
 
-  Eigen::Affine3d transform;
-  poseTFToEigen(transformTf, transform);
-  pcl::transformPointCloud(*pointCloud, *pointCloudTransformed, transform.cast<float>());
-  pointCloudTransformed->header.frame_id = targetFrame;
-
-  ROS_DEBUG_THROTTLE(5, "Point cloud transformed to frame %s for time stamp %f.", targetFrame.c_str(),
-                     pointCloudTransformed->header.stamp / 1000.0);
   return true;
 }
 

elevation_mapping_demos/launch/simple_demo.launch

@@ -17,7 +17,7 @@
   </node> -->
 
   <!-- Setup a transform between the world and the robot -->
-  <node pkg="tf" type="static_transform_publisher" name="world_to_robot" args="2.0 6.0 0 0 0.0 0 /map /base 100"/>
+  <node pkg="tf2_ros" type="static_transform_publisher" name="world_to_robot" args="2.0 6.0 0.0 0.0 0.0 0.0 /map /base"/>
 
   <!-- Launch visualizations for the resulting elevation map -->
   <include file="$(find elevation_mapping_demos)/launch/visualization.launch" />

elevation_mapping_demos/launch/turtlesim3_waffle_demo.launch

@@ -15,6 +15,7 @@
     <arg name="gui" value="false"/> <!-- Launch the user interface window of Gazebo-->
     <arg name="headless" value="false"/> <!-- Enable gazebo state log recording-->
     <arg name="debug" value="false"/> <!-- Start gzserver (Gazebo Server) in debug mode using gdb-->
+    <arg name="verbose" value="false"/> <!-- Start gazebo in verbose mode-->
   </include>
 
   <!-- Load robot_description param for tf, rviz and gazebo spawn. -->

elevation_mapping_demos/package.xml

@@ -19,7 +19,7 @@
   <exec_depend>grid_map_visualization</exec_depend>
   <exec_depend>pcl_ros</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>tf</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
   <exec_depend>turtlebot3_gazebo</exec_depend>
   <exec_depend>xacro</exec_depend>