Replace tf with tf2 (demos)

Author: SivertHavso

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.
 
@@ -400,7 +400,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_demos/launch/simple_demo.launch

@@ -16,7 +16,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

@@ -20,7 +20,7 @@
   <exec_depend>pcl_ros</exec_depend>
   <exec_depend>point_cloud_io</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>
 

elevation_mapping_demos/scripts/tf_to_pose_publisher.py

@@ -2,33 +2,25 @@
 
 import rospy
 import geometry_msgs.msg
-import tf
+import tf2_ros
 
 def callback(newPose):
     """Listens to a transform between from_frame and to_frame and publishes it
        as a pose with a zero covariance."""
-    global publisher, tf_listener, from_frame, to_frame
+    global publisher, tf_buffer, tf_listener, from_frame, to_frame
 
     # Listen to transform and throw exception if the transform is not
     # available.
     try:
-        (trans, rot) = tf_listener.lookupTransform(
-            from_frame, to_frame, rospy.Time(0))
-    except (tf.LookupException, tf.ConnectivityException,
-            tf.ExtrapolationException):
+        trans = tf_buffer.lookup_transform(from_frame, to_frame, rospy.Time())
+    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
         return
 
     # Create and fill pose message for publishing
     pose = geometry_msgs.msg.PoseWithCovarianceStamped()
-    pose.header.stamp = rospy.Time(0)
-    pose.header.frame_id = from_frame
-    pose.pose.pose.position.x = trans[0]
-    pose.pose.pose.position.y = trans[1]
-    pose.pose.pose.position.z = trans[2]
-    pose.pose.pose.orientation.x = rot[0]
-    pose.pose.pose.orientation.y = rot[1]
-    pose.pose.pose.orientation.z = rot[2]
-    pose.pose.pose.orientation.w = rot[3]
+    pose.header = trans.header
+    pose.pose.pose.position = trans.transform.translation
+    pose.pose.pose.orientation = trans.transform.rotation
 
     # Since tf transforms do not have a covariance, pose is filled with
     # a zero covariance.
@@ -45,14 +37,15 @@ def callback(newPose):
 def main_program():
     """ Main function initializes node and subscribers and starts
         the ROS loop. """
-    global publisher, tf_listener, from_frame, to_frame
+    global publisher, tf_buffer, tf_listener, from_frame, to_frame
     rospy.init_node('tf_to_pose_publisher')
     # Read frame id's for tf listener
     from_frame = rospy.get_param("~from_frame")
     to_frame = rospy.get_param("~to_frame")
     pose_name = str(to_frame) + "_pose"
 
-    tf_listener = tf.TransformListener()
+    tf_buffer = tf2_ros.Buffer()
+    tf_listener = tf2_ros.TransformListener(tf_buffer)
     publisher = rospy.Publisher(
         pose_name, geometry_msgs.msg.PoseWithCovarianceStamped, queue_size=10)