二十三、TF坐标变换（三）：动态坐标变换

一、引言

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的

二、C++实现

2.1 发布方实现

• 代码实现：

  /*
      在小乌龟界面，世界坐标系的原点为左下角，向右为X轴正向，向上为Y轴正向
  
      需求：控制小乌龟运动，将两个坐标系的相对位置动态发布
  
      实现分析：
          通过订阅turtle1/pose获取小乌龟的x,y,偏移量，线速度，角速度
  */
  
  // 1.包含头文件
  #include
  #include //包含小乌龟的位置，姿态信息
  #include //创建TF广播器的功能包
  #include //包含坐标转换的相关信息
  #include //欧拉角转换四元数功能包
  void doPose(const turtlesim::Pose::ConstPtr& pose){
      // 5.1创建TF广播器
      static tf2_ros::TransformBroadcaster broadcaster;
      // 5.2创建广播的数据
      geometry_msgs::TransformStamped tfs;
      // ---头设置
      tfs.header.frame_id = "world";
      tfs.header.seq = 100;
      tfs.header.stamp = ros::Time::now();
      // ---子级坐标系ID
      tfs.child_frame_id = "turtle1";
      // ---坐标系偏移量设置
      tfs.transform.translation.x = pose->x;
      tfs.transform.translation.y = pose->y;
      tfs.transform.translation.z = 0.0; // 小乌龟坐标系为二维的，所以z为0
      // ---设置四元数，只有偏航角
      tf2::Quaternion qtn;
      qtn.setRPY(0,0,pose->theta);
      tfs.transform.rotation.x = qtn.getX();
      tfs.transform.rotation.y = qtn.getY();
      tfs.transform.rotation.z = qtn.getZ();
      tfs.transform.rotation.w = qtn.getW();
      // 5.3广播器发布数据
      broadcaster.sendTransform(tfs);
  }
  int main(int argc, char *argv[]){
      setlocale(LC_ALL,"");
      // 2.初始化ROS节点
      ros::init(argc,argv,"tf_dynamic_pub");
      // 3.创建ROS句柄
      ros::NodeHandle nh;
      // 4.创建订阅对象，订阅小乌龟的位姿信息
      ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",100,doPose);
      // 5.回调函数处理订阅到的信息
      // 6.spin函数
      ros::spin();
      return 0;
  }
  

• 代码运行：

  bash1:
  roscore
  
  bash2:运行小乌龟节点和键盘控制节点
  rosrun turtlesim turtlesim_node
  osrun turtlesim turtle_teleop_key 
  
  bash3：查看运行信息
  rostopic list
  rostopic echo /tf
  
  bash4:rviz查看坐标关系
  rviz
  

• 运行结果：

  transforms: 
  - 
      header: 
      seq: 100
      stamp: 
          secs: 1657514334
          nsecs: 406772823
      frame_id: "world"
      child_frame_id: "turtle1"
      transform: 
      translation: 
          x: 5.544444561004639
          y: 5.544444561004639
          z: 0.0
      rotation: 
          x: 0.0
          y: 0.0
          z: 0.0
          w: 1.0
  ---
  

2.2 订阅方实现

• 代码实现：

  // 1.包含头文件
  #include"ros/ros.h"
  #include"tf2_ros/transform_listener.h"//创建订阅方功能包
  #include"tf2_ros/buffer.h"//订阅的信息存储区创建功能包
  #include"geometry_msgs/PoseStamped.h"//坐标点表示功能包
  #include"tf2_geometry_msgs/tf2_geometry_msgs.h"//调用transform必须包含该头文件
  
  int main(int argc, char *argv[]){
      // 2.初始化节点
      setlocale(LC_ALL,"");
      ros::init(argc,argv,"dynamic_tf_sub");
      ros::NodeHandle nh;
      // 3.创建TF订阅节点
      tf2_ros::Buffer buffer;//存放订阅到的消息
      tf2_ros::TransformListener listener(buffer);//创建TF订阅对象,将消息存放至buffer中
  
      ros::Rate r(1);
  
      while(ros::ok()){
          // 4.生成一个相对于小乌龟坐标系的坐标点
          geometry_msgs::PointStamped point;
          point.header.frame_id = "turtle1";
          /*
              在此处，不可以使用ros::Time::now()，获取当前时间。
              以为在ROS中，是通过比较时间戳的方式来判断坐标点相对于世界坐标系的关系的。
              如果时间相差过大，则认为该坐标系无效，从而无法计算当前坐标点相对于世界坐标系的位置。
              为了避免这一问题，可以直接忽略时间戳
          */
          point.header.stamp = ros::Time(0.0);//该格式表示直接忽略时间戳
          // 设置相对于乌龟坐标系的坐标点
          point.point.x = 1;
          point.point.y = 1;
          point.point.z = 0;//二维坐标系，所以Z为0
          // 5.转换坐标点（相对于世界坐标系）
          try
          {
              geometry_msgs::PointStamped point_base;
              point_base = buffer.transform(point,"world");//world：参考的父级坐标系
              ROS_INFO("相对于world坐标系的坐标点：%.2f,%.2f,%.2f",point_base.point.x,point_base.point.y,point_base.point.z);
          }
          catch(const std::exception& e)
          {
              ROS_INFO("程序异常：%s",e.what());
          }
          r.sleep();
          ros::spinOnce();
      }
  
      return 0;
  }
  

• 运行结果：

  bash1：
  roscore
  
  bash2：节点启动
  rosrun turtlesim turtlesim_node 
  rosrun turtlesim turtle_teleop_key 
  rosrun pkg02_tf_dynamic demo_01_dynamic_pub
  rosrun pkg02_tf_dynamic demo_02_dynamic_sub
  
  bash3:结果
  [ INFO] [1657532054.682116287]: 相对于world坐标系的坐标点：4.59,3.10,0.00
  
  bash4:
  rviz
  

三、Python实现

3.1 发布方实现

• 代码实现

  #! /usr/bin/env python 
  # 1.导包
  import rospy
  import tf2_ros #创建TF广播器
  import tf # 欧拉角 转换 四元数
  from turtlesim.msg import Pose # 小乌龟位置信息
  from geometry_msgs.msg import TransformStamped # 创建广播的数据：坐标系关系
  
  def doPose(pose):#函数的消息和接收到的消息相同
      # 4.1 创建TF广播器
      broadcaster = tf2_ros.TransformBroadcaster()
      # 4.2 创建广播的数据
      tfs = TransformStamped()
      tfs.header.frame_id = "world"
      tfs.header.stamp = rospy.Time.now()
      tfs.child_frame_id = "turtle1"
      # 坐标系原点偏移量
      tfs.transform.translation.x = pose.x
      tfs.transform.translation.y = pose.y
      tfs.transform.translation.z = 0
      # 四元数转换
      
      qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
      tfs.transform.rotation.x = qtn[0]
      tfs.transform.rotation.y = qtn[1]
      tfs.transform.rotation.z = qtn[2]
      tfs.transform.rotation.w = qtn[3]
      # 4.3 广播器发送数据
      broadcaster.sendTransform(tfs)
  
  
  if __name__ == "__main__":
      # 2.初始化ros节点
      rospy.init_node("tf_dynamic_pub_p")
      # 3.订阅话题消息：/turtle1/pose
      sub = rospy.Subscriber("/turtle1/pose",Pose,doPose)
      # 4.回调函数
      # 5.spin
      rospy.spin()
  

• 运行结果：

  rostopic echo /tf
  ransforms: 
  - 
      header: 
      seq: 0
      stamp: 
          secs: 1657540622
          nsecs: 623632907
      frame_id: "world"
      child_frame_id: "turtle1"
      transform: 
      translation: 
          x: 5.411976337432861
          y: 8.38175106048584
          z: 0.0
      rotation: 
          x: 0.0
          y: 0.0
          z: 0.6987099429325695
          w: 0.7154050710242174
  ---
  

3.2 订阅方实现

• 代码实现：

  #! /usr/bin/env python
  # 1.导包
  from ctypes import pointer
  from turtle import pos
  import rospy
  import tf2_ros
  # 不要使用geometry_msgs消息类型定义坐标点，要使用tf2内置的消息类型定义坐标点
  # from geometry_msgs.msg import PointStamped
  from tf2_geometry_msgs import PointStamped
  if __name__ == "__main__":
      # 2.初始化ros节点
      rospy.init_node("dynamic_sub_p")
      # 3.创建TF订阅对象
      buffer = tf2_ros.Buffer()
      listener = tf2_ros.TransformListener(buffer)
  
      rate = rospy.Rate(1)
      while not rospy.is_shutdown():
          # 4.创建一个相对于小乌龟坐标系的坐标点
          point_source = PointStamped()
          point_source.header.frame_id = "turtle1"# 注意和发布方子级坐标系名字相同
          point_source.header.stamp = rospy.Time()
          point_source.point.x = 1
          point_source.point.y = 2
          point_source.point.z = 0
  
          try:
              # 5.坐标转换
              point_target = buffer.transform(point_source,"world",rospy.Duration(1))
              rospy.loginfo("转换结果：%.2f,%.2f,%.2f",
                              point_target.point.x,
                              point_target.point.y,
                              point_target.point.z
                              )
          except Exception as e:
              rospy.loginfo("程序异常：%s",e)
          # 6.spin
          rate.sleep()
  

• 运行结果：

  bash1：
  roscore
  
  bash2：节点启动
  rosrun turtlesim turtlesim_node 
  rosrun turtlesim turtle_teleop_key 
  rosrun pkg02_tf_dynamic demo_01_dynamic_pub_p.py
  rosrun pkg02_tf_dynamic demo_02_dynamic_sub_p.py
  
  bash3:结果
  [INFO] [1657548740.886822]: 转换结果：12.88,1.33,0.00
  
  bash4:
  rviz
  

四、时间戳相关（订阅方）

• 在ROS中，程序先通过buffer存储订阅到的不同坐标系的相对关系，然后获取子级坐标系中的坐标点，二者通过时间戳进行匹配

4.1 静态坐标变换

在静态坐标变换中，不同坐标系的相对关系不变，所以时间戳时间既可以是当前的，也可以忽略时间戳，格式如下：

• C++：下列三种都可以
  • tfs.header.stamp = ros::Time::now();
  • tfs.header.stamp = ros::Time(0.0);
  • tfs.header.stamp = ros::Time();
• Python：下列两种都可以
  • point_source.header.stamp = rospy.Time.now()
  • point_source.header.stamp = rospy.Time()

4.2 动态坐标变换

在动态坐标变换中，ROS系统会根据时间戳进行坐标系和坐标点的匹配，来保证坐标变换的准确性，当时间戳相差较大时，就会报错。由于存放坐标关系的buffer时间和坐标点的时间戳一个在循环外，一个在循环内，时间相差较大，所以系统会报错。虽然buffer的时间戳一直不变，但是其内容是一直变得，在一直更新，所以，如果忽略时间戳，并不会影响转换精度，因此，在动态坐标变换中，要忽略时间戳，格式如下：

• C++:下列两种都可以
  • tfs.header.stamp = ros::Time(0.0);
  • tfs.header.stamp = ros::Time();
• Python:
  • point_source.header.stamp = rospy.Time()