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

一、引言

在一个小车上，雷达主体和小车主体的物理中心点不同，具有一定的偏移量。为了获得雷达探测到的物体相对于小车主体物理中心的坐标，需要知道雷达相对于小车主体的偏移量和旋转量，通过偏移量和旋转量进行坐标变换

二、实现

2.1 实现思路

1. 坐标相对关系，可以通过发布方发布
2. 订阅方接收到坐标相对关系，再根据坐标点信息，借助tf实现坐标变换，并将坐标输出

2.2 C++实现

• 1. 添加功能包依赖：tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
• 2. 发布方实现：
  • 代码实现

    // 1.包含头文件
    #include"ros/ros.h"
    #include"tf2_ros/static_transform_broadcaster.h"
    #include"geometry_msgs/TransformStamped.h"
    #include"tf2/LinearMath/Quaternion.h"
    
    int main(int argc, char *argv[]){
        setlocale(LC_ALL,"");
        // 2.初始化ros节点
        ros::init(argc,argv,"static_brocast");
        // 3.创建静态坐标转换广播器
        tf2_ros::StaticTransformBroadcaster broadcaster;
        // 4.创建坐标系信息:反应不同坐标系之间的关系
        geometry_msgs::TransformStamped ts;
        // 设置头信息
        ts.header.seq = 100;//序列号
        ts.header.stamp = ros::Time::now();//时间戳
        ts.header.frame_id = "base_link";//父级坐标系ID
        ts.child_frame_id = "laser";//子级坐标系ID
        //设置子级坐标系相对于父级坐标系的偏移量
        ts.transform.translation.x = 0.2;
        ts.transform.translation.y = 0.0;
        ts.transform.translation.z = 0.5;
        //设置四元数，将欧拉角数据转换为四元数
        tf2::Quaternion qtn;
        qtn.setRPY(0,0,0);//绕x,y,z轴旋转的度数（弧度）
        ts.transform.rotation.x = qtn.getX();
        ts.transform.rotation.y = qtn.getY();
        ts.transform.rotation.z = qtn.getZ();
        ts.transform.rotation.w = qtn.getW();
        // 5.广播器发布坐标信息
        broadcaster.sendTransform(ts);
        ros::spin();
        return 0;
    }
    

  • 测试是否发布成功
    • 方法一：

      bash1:查看话题列表
      ostopic list 
      /rosout
      /rosout_agg
      /tf_static
      
      bash2：查看话题信息
      rostopic echo /tf_static 
      transforms: 
      - 
          header: 
          seq: 100
          stamp: 
              secs: 1657352305
              nsecs: 155934039
          frame_id: "base_link"
          child_frame_id: "laser"
          transform: 
          translation: 
              x: 0.2
              y: 0.0
              z: 0.5
          rotation: 
              x: 0.0
              y: 0.0
              z: 0.0
              w: 1.0
      ---
      

    • 方法二：通过rviz
      1. 输入命令rviz
      2. 设置Fixed Frame，选择父级坐标系作为参考坐标系
      3. 点击Add，选择TF即可，效果如下图所示
         
• 3. 订阅方实现：
  • 代码实现：

    // 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[]){
        setlocale(LC_ALL,"");
        // 2.初始化ros节点
        ros::init(argc,argv,"tf_sub");
        ros::NodeHandle nh;
        // 3.创建TF订阅节点
        tf2_ros::Buffer buffer;//存放坐标系关系数据
        tf2_ros::TransformListener listener(buffer);
    
        ros::Rate r(1);
        while (ros::ok())
        {
            // 4.生成一个相对于子级坐标系的坐标点
            geometry_msgs::PointStamped point_laser;
            point_laser.header.frame_id = "laser";
            point_laser.header.stamp = ros::Time::now();
            point_laser.point.x = 1;
            point_laser.point.y = 2;
            point_laser.point.z = 7.3;
            // 5.转换坐标点（相对于父级坐标系）
            // 为了防止发布节点发布较晚，可使用try语句或休眠
            try
            {
                geometry_msgs::PointStamped point_base;
                point_base = buffer.transform(point_laser,"base_link");
                ROS_INFO("转换后的数据：%.2f,%.2f,%.2f\n参考坐标系是：%s\n",
                            point_base.point.x,
                            point_base.point.y,
                            point_base.point.z,
                            point_base.header.frame_id.c_str());
                
            }
            catch(const std::exception& e)
            {
                // std::cerr << e.what() << '\n';
                ROS_INFO("程序异常...\n");
            }
            r.sleep();
            ros::spinOnce();
        }
        return 0;
    }
    

  • 运行结果：

    bash1:运行发布方
    rosrun tf_static_01 static_pub_01
    
    bash2:运行订阅方
    rosrun tf_static_01 static_sub_01
    [ INFO] [1657382764.648824863]: 程序异常...
    
    [ INFO] [1657382765.645452420]: 转换后的数据：1.20,2.00,7.80
    参考坐标系是：base_link
    

2.3 Python实现

• 1. 发布方实现

     #! /usr/bin/env python
     # 1.导包
     import rospy
     import tf2_ros
     import tf
     from geometry_msgs.msg import TransformStamped
     
     if __name__ == "__main__":
         # 2.初始化ROS节点
         rospy.init_node("static_pub_p")
         # 3.创建静态坐标广播器
         broadcaster = tf2_ros.StaticTransformBroadcaster()
         # 4.创建并组织被广播的信息
         tfs = TransformStamped()
         # 设置头信息
         tfs.header.frame_id = "base_link"
         tfs.header.stamp = rospy.Time.now()
         tfs.header.seq = 10
         # 子坐标系
         tfs.child_frame_id = "laser"
         # -----坐标系相对位置
         # ---偏移量
         tfs.transform.translation.x = 0.3
         tfs.transform.translation.y = 0.4
         tfs.transform.translation.z = 0.2
         # ---四元数
         qtn = tf.transformations.quaternion_from_euler(0,0,0)
         tfs.transform.rotation.x = qtn[0]
         tfs.transform.rotation.y = qtn[1]
         tfs.transform.rotation.z = qtn[2]
         tfs.transform.rotation.w = qtn[3]
     
         # 5.广播器发送消息
         broadcaster.sendTransform(tfs)
     
         # 6.spin函数
         rospy.spin()
     

• 2. 订阅方实现

     #!/usr/bin/env python3
     
     # 1.导包
     import rospy
     import tf2_ros
     from tf2_geometry_msgs import PointStamped
     # 不使用下列方法导入 PointStamped
     # from geometry_msgs import PointSpamped
     
     if __name__ == "__main__":
         # 2.初始化ROS节点
         rospy.init_node("static_sub_p")
         # 3.创建TF订阅对象
         #---buffer用于存放订阅到的信息
         buffer = tf2_ros.Buffer()
         listener = tf2_ros.TransformListener(buffer)
     
         rate = rospy.Rate(1)
     
         while not rospy.is_shutdown():
             # 4.创建一个radar坐标系中的坐标点
             point_source = PointStamped()
             point_source.header.frame_id = "laser"
             point_source.header.stamp = rospy.Time.now()
             point_source.point.x = 0.3
             point_source.point.y = 2.3
             point_source.point.z = 1.2
     
             try:
                 # 5.坐标转换
                 point_target = buffer.transform(point_source,"base_link")
                 rospy.loginfo("转换结果：x=%.2f,y=%.2f,z=%.2f\n 参考坐标系：%s",
                                 point_target.point.x,
                                 point_target.point.y,
                                 point_target.point.z,
                                 point_target.header.frame_ed
                                 )
             except Exception as e:
                 rospy.logerr("异常：%s\n",e)
             rate.sleep()
     

• 3. 运行结果

     bash1:启动发布方
     rosrun tf_static_01 static_pub_p_01.py 
     
     bash2：启动订阅方
     rosrun tf_static_01 static_sub_p_01.py 
     [ERROR] [1657422517.511912]: 异常："base_link" passed to lookupTransform argument target_frame does not exist. 
     
     [INFO] [1657422518.513463]: 转换结果：x=0.60,y=2.70,z=1.40
     参考坐标系：base_link
     [INFO] [1657422519.513232]: 转换结果：x=0.60,y=2.70,z=1.40
     

三、命令行实现坐标变换

3.1 介绍

在进行坐标转换的时候，其逻辑是一样的，只是参数不同。那么，我们可以考虑改变不同的参数来进行坐标变换，从而达到坐标复用的目的

3.2 实现

• 1. 坐标变换所需参数：
  • 父级坐标系
  • 子级坐标系
  • x偏移量
  • y偏移量
  • z偏移量
  • x翻滚角度
  • y俯仰角度
  • z偏航角度
• 2. 命令行转换格式：

  rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
  

• 3. 示例

  rosrun tf2_ros static_transform_publisher 0.5 0.3 0.1 0 0 0 /base /camera
  [ INFO] [1657424343.072175586]: Spinning until killed publishing /base to /camera