ROS|乌龟TF变换案例分析

Posted 2022-10-10 MarToony|名角

1. 相关源码内容

1.1 turtle_df_demo.launch

<launch>

  <!-- Turtlesim Node-->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
  
  <!-- Axes -->
  <param name="scale_linear" value="2" type="double"/>
  <param name="scale_angular" value="2" type="double"/>

  <node name="turtle1_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle1" />
  </node>
  <node name="turtle2_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle2" />
  </node>
  <node name="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功能包的订阅文件。

1.2 turtle_tf_broadcaster.py

import rospy
import tf
import turtlesim.msg

def 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)、转换时间、子坐标系的名称、父坐标系的名称。

1.3 turtle_tf_listener.py

import rospy

import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __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')

    # 创建发布者，发布的消息是turtle2的移动指令！
    # queue_size 用于从不同线程异步发布消息的队列大小。 大小为零意味着无限队列，这可能很危险。 当未使用关键字或传递 None 时，所有发布将同步发生并打印警告消息。
    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
        
        # 根据平移矩阵计算需要的角速度和线速度。其实这里就可以看出，turtle1如果只是原地旋转，turtle2线速度依旧为0，不会触发运动。
        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 # TODO: 为什么只赋予给x轴线速度，该线速度表示前行，y则表示乌龟横向移动。
        msg.angular.z = angular
        turtle_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 linear
    float64 x
    float64 y
    float64 z
  geometry_msgs/Vector3 angular
    float64 x
    float64 y
    float64 z
  

• msg.linear.x = linear # TODO: 为什么只赋予给x轴线速度，该线速度表示前行，y则表示乌龟横向移动。如图所示。
  
  修改turtle_tf_listener.py的相关代码：

  locate listener.py
  cd /opt/ros/melodic/lib/turtle_tf
  sudo chmod 777 turtle_tf_listener.py
  rosed turtle_tf  turtle_tf_listener.py
  

  文件修改后需要重新roslaunch启动文件。
• 其他的看代码中的注释。

2. 查看参数、节点、话题以及服务

echo "node---------------" &  rosnode list &  wait & echo "params--------------" &  rosparam list & wait & echo "topics----------" &  rostopic list

2.1 节点

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文件的内容。

2.2 参数

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
// 两只乌龟的TF广播参数
/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

2.3 话题

roser@ubuntu:~/Documents/catkin_ws$ rostopic list -v

Published 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 publisher

Subscribed topics:
 * /turtle2/pose [turtlesim/Pose] 1 subscriber // 来源于 turtle_tf_broadcaster 订阅pose信息，
 * /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 // 来源于 turtle_tf_broadcaster 订阅pose信息，

• 标准输出需要的两个话题：/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 linear
    float64 x
    float64 y
    float64 z
  geometry_msgs/Vector3 angular
    float64 x
    float64 y
    float64 z
  

• tf2_msgs/TFMessage 的数据结构为：

  geometry_msgs/TransformStamped[] transforms
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    string child_frame_id
    geometry_msgs/Transform transform
      geometry_msgs/Vector3 translation
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion rotation
        float64 x
        float64 y
        float64 z
        float64 w
  

• rosgraph_msgs/Log 的数据结构为：

  byte DEBUG=1
  byte INFO=2
  byte WARN=4
  byte ERROR=8
  byte FATAL=16
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  byte level
  string name
  string msg
  string file
  string function
  uint32 line
  string[] topics
  

• turtlesim/Color 的数据结构：

  uint8 r
  uint8 g
  uint8 b
  

2.4 服务

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

• 这里服务的内容不过多介绍，通过rossrv show 和rosservice info可以查看内容。比如：

  roser@ubuntu:/opt/ros/melodic/lib/turtle_tf$ rosservice type /turtle2/teleport_absolute
  turtlesim/TeleportAbsolute
  roser@ubuntu:/opt/ros/melodic/lib/turtle_tf$ rossrv show turtlesim/TeleportAbsolute
  float32 x
  float32 y
  float32 theta
  ---
  roser@ubuntu:/opt/ros/melodic/lib/turtle_tf$ rosservice info /turtle2/teleport_absolute
  Node: /sim
  URI: rosrpc://ubuntu:39859
  Type: turtlesim/TeleportAbsolute
  Args: x y theta
  

3. TF坐标转换

1. TF树结构是以世界坐标系作为根节点。rosrun tf view_frames
   
2. 查询两个坐标系之间的变换关系
   rosrun tf tf_echo

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]

4 补充

1. 通过rosservice call /turtle2/teleport_absolute 1 1 0.2和rosservice call /turtle2/teleport_absolute 10 10 0.2得知，乌龟案例中北京坐标系的原点在左下角。Y轴上，X轴右。