{{{#!wiki tip Using ROS 2? Check out the [[https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Tf2-Main.html|ROS 2 tf2 tutorials]]. }}} #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note =This tutorial assumes you have a working knowledge of compiling programs in ROS and you have completed the [[tf2/Tutorials/Introduction to tf2 | Introduction to tf2]] tutorial ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= ## descriptive title for the tutorial ## title =Writing a tf2 static broadcaster (Python) ## multi-line description to be displayed in search ## description = This tutorial teaches you how to broadcast static coordinate frames to tf2 ## the next tutorial description (optional) ## next =Writing a tf2 broadcaster ## links to next tutorial (optional) ## next.0.link=[[tf2/Tutorials/Writing a tf2 broadcaster (Python)|(Python)]] ## what level user is this tutorial for ## level=BeginnerCategory ## keywords = #################################### <> <> === The Code === Let's start by creating a new directory called `nodes` where we will store our Python nodes. {{{ $ mkdir nodes }}} Fire up your favourite editor to paste the following code into a new file called '''nodes/static_turtle_tf2_broadcaster.py''' {{{ #!python block=broadcaster #!/usr/bin/env python import rospy # to get commandline arguments import sys # because of transformations import tf import tf2_ros import geometry_msgs.msg if __name__ == '__main__': if len(sys.argv) < 8: rospy.logerr('Invalid number of parameters\nusage: ' './static_turtle_tf2_broadcaster.py ' 'child_frame_name x y z roll pitch yaw') sys.exit(0) else: if sys.argv[1] == 'world': rospy.logerr('Your static turtle name cannot be "world"') sys.exit(0) rospy.init_node('my_static_tf2_broadcaster') broadcaster = tf2_ros.StaticTransformBroadcaster() static_transformStamped = geometry_msgs.msg.TransformStamped() static_transformStamped.header.stamp = rospy.Time.now() static_transformStamped.header.frame_id = "world" static_transformStamped.child_frame_id = sys.argv[1] static_transformStamped.transform.translation.x = float(sys.argv[2]) static_transformStamped.transform.translation.y = float(sys.argv[3]) static_transformStamped.transform.translation.z = float(sys.argv[4]) quat = tf.transformations.quaternion_from_euler( float(sys.argv[5]),float(sys.argv[6]),float(sys.argv[7])) static_transformStamped.transform.rotation.x = quat[0] static_transformStamped.transform.rotation.y = quat[1] static_transformStamped.transform.rotation.z = quat[2] static_transformStamped.transform.rotation.w = quat[3] broadcaster.sendTransform(static_transformStamped) rospy.spin() }}} === The code explained === Now let's look at the code that is relevant to publishing the static_turtle_pose to tf2 <> The tf2_ros package provides a `StaticTransformBroadcaster` to make easy the publishing of static transforms. To use the `StaticTransformBroadcaster`, we need to import the '''tf2_ros''' module. <> Here we create a `StaticTransformBroadcaster` object that we'll use later to send transformations over the wire. <> Here we create a `TransformStamped` object which will be the message we will send over once populated. Before stuffing the actual transform values we need to give it the appropriate metadata. 1. We need to give the transform being published a timestamp, we'll just stamp it with the current time, `rospy.Time.now()`. 1. Then, we need to set the name of the parent frame of the link we're creating, in this case `"world"` 1. Finally, we need to set the name of the child frame of the link we're creating. <> Here we populate the 6D pose (translation and rotation) of the turtle. <> Finally we send the transform using the `StaticTransformBroadcaster` `sendTransform` function. == Running the Static Broadcaster == First we need to mark the file as executable. {{{#!bash $ chmod +x ~/tutorial_ws/src/learning_tf2/src/nodes/static_turtle_tf2_broadcaster.py }}} Now that we created the code, let's compile it first. and try to build your package {{{ $ catkin_make }}} You can now run your `static_turtle_tf2_broadcaster.py`. First start a roscore in a separate terminal: {{{#!bash $ . devel/setup.bash $ roscore }}} and then run: {{{ $ rosrun learning_tf2 static_turtle_tf2_broadcaster.py mystaticturtle 0 0 1 0 0 0 }}} This sets a turtle pose broadcast for mystaticturtle to float 1m above the ground. <> <> ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## LearningCategory ## PythonCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE