< launch> < nodepkg = " turtlesim" type = " turtlesim_node" name = " sim" /> < nodepkg = " turtlesim" type = " turtle_teleop_key" name = " teleop" output = " screen" /> < paramname = " scale_linear" value = " 2" type = " double" /> < paramname = " scale_angular" value = " 2" type = " double" /> < nodename = " turtle1_tf_broadcaster" pkg = " turtle_tf" type = " turtle_tf_broadcaster.py" respawn = " false" output = " screen" > < paramname = " turtle" type = " string" value = " turtle1" /> </ node> < nodename = " turtle2_tf_broadcaster" pkg = " turtle_tf" type = " turtle_tf_broadcaster.py" respawn = " false" output = " screen" > < paramname = " turtle" type = " string" value = " turtle2" /> </ node> < nodename = " turtle_pointer" pkg = " turtle_tf" type = " turtle_tf_listener.py" respawn = " false" output = " screen" > </ node> </ launch> turtlesim的功能包中的turtlesim_node，节点名称为sim，以往调用该功能包时，turtlesim_node节点名称通常为turtlesim； turtle_teleop_key可执行文件，之前也有了解过； 定义全局参数，角速度和线速度；这里注意它的type为double，其他的参数类型还有int、string。 执行turtle_tf功能包的发布文件 ，同时，该节点配备了一个string类型的参数，表示乌龟的名字turtle1/2。 执行turtle_tf功能包的订阅文件 。 import rospy
import tf
import turtlesim. msgdef handle_turtle_pose ( msg, turtlename) : br = tf. TransformBroadcaster( ) br. sendTransform( ( msg. x, msg. y, 0 ) , tf. transformations. quaternion_from_euler( 0 , 0 , msg. theta) , rospy. Time. now( ) , turtlename, "world" ) if __name__ == '__main__' : rospy. init_node( 'tf_turtle' ) turtlename = rospy. get_param( '~turtle' ) rospy. Subscriber( '/%s/pose' % turtlename, turtlesim. msg. Pose, handle_turtle_pose, turtlename) rospy. spin( )
c++中的头文件在python中就成了库； 与roscpp不同的是，① rospy中如果要注册一个订阅者、发布者，并不需要提前创建话柄（ros::NodeHandle n）；② roscpp中注册的参数是：话题名称、消息队列（缓冲）、回调函数；rospy中注册的参数是话题名称、消息类型、回调函数、某个参数服务器中的参数。__init__(self, name, data_class, callback=None, callback_args=None, queue_size=None, buff_size=DEFAULT_BUFF_SIZE, tcp_nodelay=False) 回调函数中的sendTransform函数def sendTransform(self, translation, rotation, time, child, parent)，平移参数(x, y, z)、旋转参数，欧拉四元数(x, y, z, w)、转换时间、子坐标系的名称、父坐标系的名称。 import rospyimport math
import tf
import geometry_msgs. msg
import turtlesim. srvif __name__ == '__main__' : rospy. init_node( 'tf_turtle' ) listener = tf. TransformListener( ) rospy. wait_for_service( 'spawn' ) spawner = rospy. ServiceProxy( 'spawn' , turtlesim. srv. Spawn) spawner( 4 , 2 , 0 , 'turtle2' ) turtle_vel = rospy. Publisher( 'turtle2/cmd_vel' , geometry_msgs. msg. Twist, queue_size= 1 ) rate = rospy. Rate( 10.0 ) while not rospy. is_shutdown( ) : try : ( trans, rot) = listener. lookupTransform( '/turtle2' , '/turtle1' , rospy. Time( ) ) except ( tf. LookupException, tf. ConnectivityException, tf. ExtrapolationException) : continue angular = 4 * math. atan2( trans[ 1 ] , trans[ 0 ] ) linear = 0.5 * math. sqrt( trans[ 0 ] ** 2 + trans[ 1 ] ** 2 ) msg = geometry_msgs. msg. Twist( ) msg. linear. x = linear msg. angular. z = angularturtle_vel. publish( msg) rate. sleep( )
rospy.wait_for_service('spawn')之前未见过的语法。表示，阻塞，直到服务可用。 如果您的程序依赖于已在运行的服务，请在初始化代码中使用它。geometry_msgs.Twist消息的数据结构为：roser@ubuntu: / opt/ ros/ melodic/ lib/ turtle_tf$ rosmsg show geometry_msgs/ Twist
geometry_msgs/ Vector3 linearfloat64 xfloat64 yfloat64 z
geometry_msgs/ Vector3 angularfloat64 xfloat64 yfloat64 z
msg.linear.x = linear # TODO: 为什么只赋予给x轴线速度，该线速度表示前行，y则表示乌龟横向移动。如图所示。locate listener.py
cd /opt/ros/melodic/lib/turtle_tf
sudo chmod 777 turtle_tf_listener.py
rosed turtle_tf turtle_tf_listener.py
其他的看代码中的注释。 echo "node---------------" & rosnode list & wait & echo "params--------------" & rosparam list & wait & echo "topics----------" & rostopic list
roser@ubuntu: ~ / Documents/ catkin_ws$ rosnode list
/ rosout
/ sim
/ teleop
/ turtle1_tf_broadcaster
/ turtle2_tf_broadcaster
/ turtle_pointer
rosout是Master标准输出节点； sim是turtlesim_node可执行文件(节点)的名称； 其他节点名称见turtle_tf_demo.launch文件的内容。 roser@ubuntu: ~ / Documents/ catkin_ws$ rosparam list
/ rosdistro
/ roslaunch/ uris/ host_ubuntu__39761
/ rosversion
/ run_id
/ scale_angular
/ scale_linear
/ sim/ background_b
/ sim/ background_g
/ sim/ background_r
/ turtle1_tf_broadcaster/ turtle
/ turtle2_tf_broadcaster/ turtle
/rosdistro——‘melodic’； /roslaunch/uris/host_ubuntu__39761 —— http://ubuntu:39761/ /rosversion —— ‘1.14.13’ /run_id —— 35a5f89e-477f-11ed-a88e-000c2909fef3 /scale_angular —— 2.0 /scale_linear —— 2.0 /sim/background_b —— 255 /sim/background_g —— 86 /sim/background_r —— 69 /turtle1_tf_broadcaster/turtle —— turtle1 /turtle2_tf_broadcaster/turtle —— turtle2 roser@ubuntu: ~ / Documents/ catkin_ws$ rostopic list - vPublished topics: * / turtle1/ color_sensor [ turtlesim/ Color] 1 publisher* / turtle2/ color_sensor [ turtlesim/ Color] 1 publisher* / rosout [ rosgraph_msgs/ Log] 5 publishers* / turtle2/ pose [ turtlesim/ Pose] 1 publisher* / rosout_agg [ rosgraph_msgs/ Log] 1 publisher* / tf [ tf2_msgs/ TFMessage] 2 publishers* / turtle2/ cmd_vel [ geometry_msgs/ Twist] 1 publisher* / turtle1/ cmd_vel [ geometry_msgs/ Twist] 1 publisher* / turtle1/ pose [ turtlesim/ Pose] 1 publisherSubscribed topics: * / turtle2/ pose [ turtlesim/ Pose] 1 subscriber * / rosout [ rosgraph_msgs/ Log] 1 subscriber * / tf [ tf2_msgs/ TFMessage] 1 subscriber* / tf_static [ tf2_msgs/ TFMessage] 1 subscriber* / turtle2/ cmd_vel [ geometry_msgs/ Twist] 1 subscriber* / turtle1/ cmd_vel [ geometry_msgs/ Twist] 1 subscriber* / turtle1/ pose [ turtlesim/ Pose] 1 subscriber
标准输出需要的两个话题：/rosout和/rosout_agg； 消息turtlesim/Pose，在二维中该pose信息包含三个值roser@ubuntu:/opt/ros/melodic/lib/turtle_tf$ rosmsg show turtlesim/Pose
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity
geometry_msgs/Twist的数据结构为：geometry_msgs/Vector3 linearfloat64 xfloat64 yfloat64 z
geometry_msgs/Vector3 angularfloat64 xfloat64 yfloat64 z
tf2_msgs/TFMessage 的数据结构为：geometry_msgs/TransformStamped[] transformsstd_msgs/Header headeruint32 seqtime stampstring frame_idstring child_frame_idgeometry_msgs/Transform transformgeometry_msgs/Vector3 translationfloat64 xfloat64 yfloat64 zgeometry_msgs/Quaternion rotationfloat64 xfloat64 yfloat64 zfloat64 w
rosgraph_msgs/Log 的数据结构为：byte DEBUG=1
byte INFO=2
byte WARN=4
byte ERROR=8
byte FATAL=16
std_msgs/Header headeruint32 seqtime stampstring frame_id
byte level
string name
string msg
string file
string function
uint32 line
string[] topics
turtlesim/Color 的数据结构：uint8 r
uint8 g
uint8 b
roser@ubuntu:~/Documents/catkin_ws$ rosservice list
/clear
/kill
/reset
/rosout/get_loggers
/rosout/set_logger_level
/sim/get_loggers
/sim/set_logger_level
/spawn
/teleop/get_loggers
/teleop/set_logger_level
/turtle1/set_pen
/turtle1/teleport_absolute
/turtle1/teleport_relative
/turtle1_tf_broadcaster/get_loggers
/turtle1_tf_broadcaster/set_logger_level
/turtle2/set_pen
/turtle2/teleport_absolute
/turtle2/teleport_relative
/turtle2_tf_broadcaster/get_loggers
/turtle2_tf_broadcaster/set_logger_level
/turtle_pointer/get_loggers
/turtle_pointer/set_logger_level
TF树结构是以世界坐标系作为根节点。rosrun tf view_frames 查询两个坐标系之间的变换关系 roser@ubuntu: ~ / Documents/ catkin_ws$ rosrun tf tf_echo turtle1 turtle2
At time 1665302020.047
- Translation: [ 0.000 , 0.000 , 0.000 ]
- Rotation: in Quaternion [ 0.000 , 0.000 , 1.000 , - 0.006 ] in RPY ( radian) [ 0.000 , 0.000 , - 3.129 ] in RPY ( degree) [ 0.000 , 0.000 , - 179.305 ] roser@ubuntu: ~ / Documents/ catkin_ws$ rosrun tf tf_echo turtle2 turtle1
At time 1665302083.392
- Translation: [ 0.000 , 0.000 , 0.000 ]
- Rotation: in Quaternion [ 0.000 , 0.000 , 1.000 , 0.006 ] in RPY ( radian) [ 0.000 , - 0.000 , 3.129 ] in RPY ( degree) [ 0.000 , - 0.000 , 179.305 ]
通过rosservice call /turtle2/teleport_absolute 1 1 0.2和rosservice call /turtle2/teleport_absolute 10 10 0.2得知，乌龟案例中北京坐标系的原点在左下角。Y轴上，X轴右。 
