diff --git a/nodes/odom_publisher_linus.py b/nodes/odom_publisher_linus.py index 2d99dd3..7aa8404 100644 --- a/nodes/odom_publisher_linus.py +++ b/nodes/odom_publisher_linus.py @@ -1,38 +1,38 @@ #!/usr/bin/env python import rospy -from tf.transform_broadcaster import TransformBroadcaster +import tf2_ros from nav_msgs.msg import Odometry from geometry_msgs.msg import TransformStamped def odom_callback(msg): - # Create TransformStamped message + # Create a TransformStamped message odom_trans = TransformStamped() odom_trans.header.stamp = msg.header.stamp odom_trans.header.frame_id = msg.header.frame_id odom_trans.child_frame_id = msg.child_frame_id - # Set translation + # Set the translation odom_trans.transform.translation.x = msg.pose.pose.position.x odom_trans.transform.translation.y = msg.pose.pose.position.y odom_trans.transform.translation.z = msg.pose.pose.position.z - # Set rotation + # Set the rotation odom_trans.transform.rotation = msg.pose.pose.orientation - # Broadcast the transform - tf_broadcaster.sendTransformMessage(odom_trans) + # Publish the transform + tf_broadcaster.sendTransform(odom_trans) if __name__ == "__main__": - rospy.init_node("tf_broadcaster") + rospy.init_node("tf2_broadcaster") # Create a TransformBroadcaster - tf_broadcaster = TransformBroadcaster() + tf_broadcaster = tf2_ros.TransformBroadcaster() # Subscribe to the /odom topic rospy.Subscriber("/odom", Odometry, odom_callback) - rospy.loginfo("TF broadcaster node is running...") + rospy.loginfo("TF2 broadcaster node is running...") # Keep the node running rospy.spin()