From 69f93f900f658e8cb0e22180300d9bed38ade16b Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sun, 5 Apr 2020 17:19:02 +0200
Subject: [PATCH] Migration to tf2

---
 CMakeLists.txt                                |  5 +++--
 .../detail/message_context.h                  | 10 ++++-----
 .../detail/single_client.h                    |  6 ++---
 .../interactive_marker_client.h               |  6 ++---
 package.xml                                   |  3 ++-
 src/interactive_marker_client.cpp             |  2 +-
 src/message_context.cpp                       | 19 ++++++++--------
 src/single_client.cpp                         |  2 +-
 src/test/bursty_tf.cpp                        | 17 ++++++++------
 src/test/client_test.cpp                      | 16 +++++---------
 src/test/missing_tf.cpp                       | 18 +++++++++------
 src/test/server_client_test.cpp               |  6 ++---
 src/tools.cpp                                 | 22 +++++++++----------
 13 files changed, 68 insertions(+), 64 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 946b2997..207a4378 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,13 +6,14 @@ find_package(catkin REQUIRED
   rospy
   rostest
   std_msgs
-  tf
+  tf2_ros
+  tf2_geometry_msgs
   visualization_msgs
 )
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES interactive_markers
-  CATKIN_DEPENDS roscpp rosconsole rospy tf std_msgs visualization_msgs
+  CATKIN_DEPENDS roscpp rosconsole rospy tf2_ros std_msgs visualization_msgs
 )
 catkin_python_setup()
 
diff --git a/include/interactive_markers/detail/message_context.h b/include/interactive_markers/detail/message_context.h
index 684ac3ef..1e84f730 100644
--- a/include/interactive_markers/detail/message_context.h
+++ b/include/interactive_markers/detail/message_context.h
@@ -37,7 +37,7 @@
 #ifndef MESSAGE_CONTEXT_H_
 #define MESSAGE_CONTEXT_H_
 
-#include <tf/tf.h>
+#include <tf2_ros/buffer.h>
 
 #include <visualization_msgs/InteractiveMarkerInit.h>
 #include <visualization_msgs/InteractiveMarkerUpdate.h>
@@ -49,7 +49,7 @@ template<class MsgT>
 class MessageContext
 {
 public:
-  MessageContext( tf::Transformer& tf,
+  MessageContext( tf2_ros::Buffer& tf,
       const std::string& target_frame,
       const typename MsgT::ConstPtr& msg,
       bool enable_autocomplete_transparency = true);
@@ -76,15 +76,15 @@ class MessageContext
   // array indices of marker/pose updates with missing tf info
   std::list<size_t> open_marker_idx_;
   std::list<size_t> open_pose_idx_;
-  tf::Transformer& tf_;
+  tf2_ros::Buffer& tf_;
   std::string target_frame_;
   bool enable_autocomplete_transparency_;
 };
 
-class InitFailException: public tf::TransformException
+class InitFailException: public tf2::TransformException
 {
 public:
-  InitFailException(const std::string errorDescription) : tf::TransformException(errorDescription) { ; }
+  InitFailException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }
 };
 
 
diff --git a/include/interactive_markers/detail/single_client.h b/include/interactive_markers/detail/single_client.h
index b1299913..ce906ef0 100644
--- a/include/interactive_markers/detail/single_client.h
+++ b/include/interactive_markers/detail/single_client.h
@@ -47,7 +47,7 @@
 #include <boost/function.hpp>
 #include <boost/unordered_map.hpp>
 
-#include <tf/tf.h>
+#include <tf2_ros/buffer.h>
 
 #include <deque>
 
@@ -65,7 +65,7 @@ class SingleClient
 
   SingleClient(
       const std::string& server_id,
-      tf::Transformer& tf,
+      tf2_ros::Buffer& tf,
       const std::string& target_frame,
       const InteractiveMarkerClient::CbCollection& callbacks );
 
@@ -131,7 +131,7 @@ class SingleClient
   // queue for init messages
   M_InitMessageContext init_queue_;
 
-  tf::Transformer& tf_;
+  tf2_ros::Buffer& tf_;
   std::string target_frame_;
 
   const InteractiveMarkerClient::CbCollection& callbacks_;
diff --git a/include/interactive_markers/interactive_marker_client.h b/include/interactive_markers/interactive_marker_client.h
index 01c22a36..ae457ab7 100644
--- a/include/interactive_markers/interactive_marker_client.h
+++ b/include/interactive_markers/interactive_marker_client.h
@@ -42,7 +42,7 @@
 #include <ros/subscriber.h>
 #include <ros/node_handle.h>
 
-#include <tf/tf.h>
+#include <tf2_ros/buffer.h>
 
 #include <visualization_msgs/InteractiveMarkerInit.h>
 #include <visualization_msgs/InteractiveMarkerUpdate.h>
@@ -87,7 +87,7 @@ class InteractiveMarkerClient : boost::noncopyable
   /// @param target_frame tf frame to transform timestamped messages into.
   /// @param topic_ns     The topic namespace (will subscribe to topic_ns/update, topic_ns/init)
   INTERACTIVE_MARKERS_PUBLIC
-  InteractiveMarkerClient( tf::Transformer& tf,
+  InteractiveMarkerClient(tf2_ros::Buffer &tf,
       const std::string& target_frame = "",
       const std::string &topic_ns = "" );
 
@@ -165,7 +165,7 @@ class InteractiveMarkerClient : boost::noncopyable
   M_SingleClient publisher_contexts_;
   boost::mutex publisher_contexts_mutex_;
 
-  tf::Transformer& tf_;
+  tf2_ros::Buffer& tf_;
   std::string target_frame_;
 
 public:
diff --git a/package.xml b/package.xml
index 5aa5028f..3e3fd2b3 100644
--- a/package.xml
+++ b/package.xml
@@ -18,6 +18,7 @@
   <depend>rospy</depend>
   <depend>rostest</depend>
   <depend>std_msgs</depend>
-  <depend>tf</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>visualization_msgs</depend>
 </package>
diff --git a/src/interactive_marker_client.cpp b/src/interactive_marker_client.cpp
index 3e3c5ef4..9092d9ee 100644
--- a/src/interactive_marker_client.cpp
+++ b/src/interactive_marker_client.cpp
@@ -43,7 +43,7 @@ namespace interactive_markers
 {
 
 InteractiveMarkerClient::InteractiveMarkerClient(
-    tf::Transformer& tf,
+    tf2_ros::Buffer& tf,
     const std::string& target_frame,
     const std::string &topic_ns )
 : state_("InteractiveMarkerClient",IDLE)
diff --git a/src/message_context.cpp b/src/message_context.cpp
index 75b2a981..89602337 100644
--- a/src/message_context.cpp
+++ b/src/message_context.cpp
@@ -32,6 +32,7 @@
 #include "interactive_markers/detail/message_context.h"
 #include "interactive_markers/tools.h"
 
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <boost/make_shared.hpp>
 
 #define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ );
@@ -42,7 +43,7 @@ namespace interactive_markers
 
 template<class MsgT>
 MessageContext<MsgT>::MessageContext(
-    tf::Transformer& tf,
+    tf2_ros::Buffer& tf,
     const std::string& target_frame,
     const typename MsgT::ConstPtr& _msg,
     bool enable_autocomplete_transparency)
@@ -74,29 +75,27 @@ bool MessageContext<MsgT>::getTransform( std_msgs::Header& header, geometry_msgs
     if ( header.frame_id != target_frame_ )
     {
       // get transform
-      tf::StampedTransform transform;
-      tf_.lookupTransform( target_frame_, header.frame_id, header.stamp, transform );
+      geometry_msgs::TransformStamped transform;
+      transform = tf_.lookupTransform( target_frame_, header.frame_id, header.stamp );
       DBG_MSG( "Transform %s -> %s at time %f is ready.", header.frame_id.c_str(), target_frame_.c_str(), header.stamp.toSec() );
 
       // if timestamp is given, transform message into target frame
       if ( header.stamp != ros::Time(0) )
       {
-        tf::Pose pose;
-        tf::poseMsgToTF( pose_msg, pose );
-        pose = transform * pose;
-        // store transformed pose in original message
-        tf::poseTFToMsg( pose, pose_msg );
+        tf2::doTransform(pose_msg, pose_msg, transform);
         ROS_DEBUG_STREAM("Changing " << header.frame_id << " to "<< target_frame_);
         header.frame_id = target_frame_;
       }
     }
   }
-  catch ( tf::ExtrapolationException& e )
+  catch ( const tf2::ExtrapolationException& e )
   {
     ros::Time latest_time;
     std::string error_string;
 
-    tf_.getLatestCommonTime( target_frame_, header.frame_id, latest_time, &error_string );
+    tf_._getLatestCommonTime( tf_._lookupFrameNumber(target_frame_),
+                              tf_._lookupFrameNumber(header.frame_id),
+                              latest_time, &error_string );
 
     // if we have some tf info and it is newer than the requested time,
     // we are very unlikely to ever receive the old tf info in the future.
diff --git a/src/single_client.cpp b/src/single_client.cpp
index 6b2613e5..093a9b77 100644
--- a/src/single_client.cpp
+++ b/src/single_client.cpp
@@ -42,7 +42,7 @@ namespace interactive_markers
 
 SingleClient::SingleClient(
     const std::string& server_id,
-    tf::Transformer& tf,
+    tf2_ros::Buffer &tf,
     const std::string& target_frame,
     const InteractiveMarkerClient::CbCollection& callbacks
 )
diff --git a/src/test/bursty_tf.cpp b/src/test/bursty_tf.cpp
index 26fcb389..c59f155b 100644
--- a/src/test/bursty_tf.cpp
+++ b/src/test/bursty_tf.cpp
@@ -30,8 +30,8 @@
 
 #include <ros/ros.h>
 
-#include <tf/transform_broadcaster.h>
-#include <tf/tf.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include <interactive_markers/interactive_marker_server.h>
 
@@ -131,7 +131,7 @@ void make6DofMarker( bool fixed )
 
 void frameCallback(const ros::TimerEvent&)
 {
-  static tf::TransformBroadcaster br;
+  static tf2_ros::TransformBroadcaster br;
   static bool sending = true;
 
   geometry_msgs::Pose pose;
@@ -149,12 +149,15 @@ void frameCallback(const ros::TimerEvent&)
 
   if ( seconds % 2 < 1 )
   {
-    tf::Transform t;
-    t.setOrigin(tf::Vector3(0.0, 0.0, 1.0));
-    t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
+    geometry_msgs::TransformStamped stf;
+    stf.header.frame_id = "base_link";
+    stf.header.stamp = time;
+    stf.child_frame_id = "bursty_frame";
+    stf.transform.translation = toMsg(tf2::Vector3(0.0, 0.0, 1.0));
+    stf.transform.rotation = toMsg(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
     if (!sending) ROS_INFO("on");
     sending = true;
-    br.sendTransform(tf::StampedTransform(t, time, "base_link", "bursty_frame"));
+    br.sendTransform(stf);
     std::this_thread::sleep_for(std::chrono::microseconds(10000));
   }
   else
diff --git a/src/test/client_test.cpp b/src/test/client_test.cpp
index 0ff45c44..425da264 100644
--- a/src/test/client_test.cpp
+++ b/src/test/client_test.cpp
@@ -33,8 +33,6 @@
 
 #include <gtest/gtest.h>
 
-#include <tf/transform_listener.h>
-
 #include <interactive_markers/interactive_marker_server.h>
 #include <interactive_markers/interactive_marker_client.h>
 
@@ -120,9 +118,7 @@ class SequenceTest
 public:
   void test( std::vector<Msg> messages )
   {
-    tf::Transformer tf;
-
-    //tf.setTransform();
+    tf2_ros::Buffer tf;
 
     // create an interactive marker server on the topic namespace simple_marker
 
@@ -226,11 +222,11 @@ class SequenceTest
       case Msg::TF_INFO:
       {
         DBG_MSG_STREAM( i << " TF_INFO: " << msg.frame_id << " -> " << target_frame << " at time " << msg.stamp.toSec() );
-        tf::StampedTransform stf;
-        stf.frame_id_=msg.frame_id;
-        stf.child_frame_id_=target_frame;
-        stf.stamp_=msg.stamp;
-        stf.setIdentity();
+        geometry_msgs::TransformStamped stf;
+        stf.header.frame_id = msg.frame_id;
+        stf.header.stamp = msg.stamp;
+        stf.child_frame_id = target_frame;
+        stf.transform.rotation.w = 1.0;
         tf.setTransform( stf, msg.server_id );
         break;
       }
diff --git a/src/test/missing_tf.cpp b/src/test/missing_tf.cpp
index 9838b040..771de79d 100644
--- a/src/test/missing_tf.cpp
+++ b/src/test/missing_tf.cpp
@@ -30,8 +30,8 @@
 
 #include <ros/ros.h>
 
-#include <tf/transform_broadcaster.h>
-#include <tf/tf.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include <interactive_markers/interactive_marker_server.h>
 
@@ -128,7 +128,7 @@ void make6DofMarker( bool fixed )
 
 void frameCallback(const ros::TimerEvent&)
 {
-  static tf::TransformBroadcaster br;
+  static tf2_ros::TransformBroadcaster br;
   static bool sending = true;
 
   geometry_msgs::Pose pose;
@@ -159,10 +159,14 @@ void frameCallback(const ros::TimerEvent&)
   server->setPose("simple_6dof",pose,header);
   server->applyChanges();
 
-  tf::Transform t;
-  t.setOrigin(tf::Vector3(0.0, 0.0, 1.0));
-  t.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
-  br.sendTransform(tf::StampedTransform(t, time, "base_link", "bursty_frame"));
+  geometry_msgs::TransformStamped stf;
+  stf.header.frame_id = "base_link";
+  stf.header.stamp = time;
+  stf.child_frame_id = "bursty_frame";
+  stf.transform.translation = toMsg(tf2::Vector3(0.0, 0.0, 1.0));
+  stf.transform.rotation = toMsg(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
+
+  br.sendTransform(stf);
 }
 
 int main(int argc, char** argv)
diff --git a/src/test/server_client_test.cpp b/src/test/server_client_test.cpp
index 6005a473..2f192d9b 100644
--- a/src/test/server_client_test.cpp
+++ b/src/test/server_client_test.cpp
@@ -33,7 +33,7 @@
 
 #include <gtest/gtest.h>
 
-#include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
 
 #include <interactive_markers/interactive_marker_server.h>
 #include <interactive_markers/interactive_marker_client.h>
@@ -112,7 +112,7 @@ void waitMsg()
 
 TEST(InteractiveMarkerServerAndClient, connect_tf_error)
 {
-  tf::TransformListener tf;
+  tf2_ros::Buffer buffer;
 
   // create an interactive marker server on the topic namespace simple_marker
   interactive_markers::InteractiveMarkerServer server("im_server_client_test","test_server",false);
@@ -124,7 +124,7 @@ TEST(InteractiveMarkerServerAndClient, connect_tf_error)
 
   resetReceivedMsgs();
 
-  interactive_markers::InteractiveMarkerClient client(tf, "valid_frame", "im_server_client_test");
+  interactive_markers::InteractiveMarkerClient client(buffer, "valid_frame", "im_server_client_test");
   client.setInitCb( &initCb );
   client.setStatusCb( &statusCb );
   client.setResetCb( &resetCb );
diff --git a/src/tools.cpp b/src/tools.cpp
index b8bb0b7f..08b1386d 100644
--- a/src/tools.cpp
+++ b/src/tools.cpp
@@ -29,8 +29,8 @@
 
 #include "interactive_markers/tools.h"
 
-#include <tf/LinearMath/Quaternion.h>
-#include <tf/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
 
 #include <math.h>
 #include <assert.h>
@@ -63,8 +63,8 @@ void autoComplete( visualization_msgs::InteractiveMarker &msg, bool enable_autoc
   }
 
   //normalize quaternion
-  tf::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
-      msg.pose.orientation.z, msg.pose.orientation.w );
+  tf2::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
+                                          msg.pose.orientation.z, msg.pose.orientation.w );
   int_marker_orientation.normalize();
   msg.pose.orientation.x = int_marker_orientation.x();
   msg.pose.orientation.y = int_marker_orientation.y();
@@ -141,9 +141,9 @@ void autoComplete( const visualization_msgs::InteractiveMarker &msg,
   }
 
   // get interactive marker pose for later
-  tf::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
-      msg.pose.orientation.z, msg.pose.orientation.w );
-  tf::Vector3 int_marker_position( msg.pose.position.x, msg.pose.position.y, msg.pose.position.z );
+  tf2::Quaternion int_marker_orientation( msg.pose.orientation.x, msg.pose.orientation.y,
+                                          msg.pose.orientation.z, msg.pose.orientation.w );
+  tf2::Vector3 int_marker_position( msg.pose.position.x, msg.pose.position.y, msg.pose.position.z );
 
   // fill in missing pose information into the markers
   for ( unsigned m=0; m<control.markers.size(); m++ )
@@ -173,8 +173,8 @@ void autoComplete( const visualization_msgs::InteractiveMarker &msg,
     }
 
     //normalize orientation
-    tf::Quaternion marker_orientation( marker.pose.orientation.x, marker.pose.orientation.y,
-        marker.pose.orientation.z, marker.pose.orientation.w );
+    tf2::Quaternion marker_orientation( marker.pose.orientation.x, marker.pose.orientation.y,
+                                        marker.pose.orientation.z, marker.pose.orientation.w );
 
     marker_orientation.normalize();
 
@@ -397,8 +397,8 @@ void assignDefaultColor(visualization_msgs::Marker &marker, const geometry_msgs:
 {
   geometry_msgs::Vector3 v;
 
-  tf::Quaternion bt_quat( quat.x, quat.y, quat.z, quat.w );
-  tf::Vector3 bt_x_axis = tf::Matrix3x3(bt_quat) * tf::Vector3(1,0,0);
+  tf2::Quaternion bt_quat( quat.x, quat.y, quat.z, quat.w );
+  tf2::Vector3 bt_x_axis = tf2::Matrix3x3(bt_quat) * tf2::Vector3(1,0,0);
 
   float x,y,z;
   x = fabs(bt_x_axis.x());