我的ROS学习之路——静态坐标变换

说明：原文在这里，虽然有文章了，不过我仍然打算写一下，方便日后查询。（关于工作空间的创建，以及vscode的如何配置，都在前面 讲得有了，往后就不细说了。）

所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。

需求描述：现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？(程序实现后：坐标是：2.2 3.0 5.5)

实现分析:

1. 坐标系相对关系，可以通过发布方发布
2. 订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出。

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2.发布方

#include <ros/ros.h>
#include "tf2_ros/static_transform_broadcaster.h"
#include"geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
    需求：发布两个坐标系的相对关系

    流程：
    1.包含头文件
    2.节点初始化，句柄等
    3.创建发布者对象
    4.组织发布的消息,（消息是要发布在话题里面的，找到这个话题，然后rostopic type [话题名] ——来查看话题消息类型 ——用rosmsg show 或者rosmsg info 来查看这个消息具体有什么参数）
                                        （其中这几个参数肯定也是需要创建对象才能引用的，这个对象怎么创建？——就是 包名：：消息名  [自己取的对象名]）
    5.发布数据(此时就用第三步创建的发布者对象，去发布消息第四步创建的消息)
    6.ros::spin();
*/
int main(int argc, char  *argv[])
{
    //2.初始化节点
    ros::init(argc,argv,"static_tf");

    ros::NodeHandle nh;//可有可无，只是为了和前面对比流程而已

    //3.创建发布的对象
    tf2_ros::StaticTransformBroadcaster pub ;

    //4.组织发布的信息
    geometry_msgs::TransformStamped ts;
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";//相对坐标系中被参考的那一个
    ts.child_frame_id = "laser";
    //设置子级相对于父级 的偏移量
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;

/*经过测试，下面这一团代码注释掉了，rviz的图还画不出来了，
也应证了tf2_msgs/TFMessage.msg文件下，所有参数（rosmsg show  tf2_msgs/TFMessage可以查看）要有赋值才行。

另外：为什么 tf需要把 欧拉角转换为 四元数？——因为欧拉角会出问题，如出现万向锁问题（当俯仰角pitch等于90度时，滚动角roll和偏航角yaw重合，即丢失一个纬度。）
。所以为了保险，这里干脆一定要转换成四元素来算
*/
     tf2::Quaternion qtn;//创建四元数对象，这个对象可以将欧拉角转换为四元数。
     qtn.setRPY(0,0,0);//向该对象设置欧拉角（ts.transform.rotation这一部分）。 注意一下：欧拉角和上面设置的偏移量（ ts.transform.translation就是这一部分）没什么关系，是两部分
     // 在机器人学那本书里，可知：这个欧拉角就是变换矩阵里的旋转矩阵类似，而偏移量就是坐标中心的位移向量。
     ts.transform.rotation.x = qtn.getX();
     ts.transform.rotation.y = qtn.getY();
     ts.transform.rotation.z = qtn.getZ();
     ts.transform.rotation.w = qtn.getW();

     pub.sendTransform(ts);//广播发布坐标系信息
    

    ros::spin();
    return 0;
}

配置文件此处略。

订阅方：

#include <ros/ros.h>
#include "geometry_msgs/PointStamped.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/transform_listener.h"  //注意: 调用 transform 必须包含该头文件（buffer.transform这个转换算法在这个头文件下）

/*
订阅坐标系信息：自己手动设置生成一个相对于子级坐标系的坐标点数据，然后利用发布的 坐标系关系，转换成父级坐标系中的坐标点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 TF 订阅节点
        4.生成一个坐标点(相对于子级坐标系)
        5.转换坐标点(相对于父级坐标系)
        （4 5生成的数据信息 都是用的geometry_msgs/PointStamped类型）
        6.spin()
*/
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_tf_sub");
    ros::NodeHandle nh;

    //3.创建 订阅监听节点
    tf2_ros::Buffer buffer;//把监听到的矩阵变换关系存到buffer缓存当中
    tf2_ros::TransformListener listener(buffer);

    geometry_msgs::PointStamped ps;//定义转换前的坐标
    geometry_msgs::PointStamped ps_out;//重新定义一个坐标点，用于等会接收转换后（相对于父级坐标系）的坐标

    ros::Rate rate(1); //这一部分有两种处理方式，一种是这种加延迟，一种是 用try 判断错误的形式。
    while(ros::ok())
    {
        rate.sleep(); //不知道为什么，延迟如果不加在这个循环里，就报错了。

        //4.生成一个坐标点（就是即将被转换的坐标点）
        ps.header.frame_id = "laser"; //
        ps.header.stamp = ros::Time::now();
        ps.point.x = 2.0;
        ps.point.y = 3.0;
        ps.point.z = 5.0;

       ps_out = buffer .transform(ps,"base_link"); 

        // 5.转换坐标点(相对于父级坐标系)
       //ROS_INFO("转换后的坐标为：(x =%.2f,y=%.2f,z=%.2f)参考的坐标系是：%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id.c_str());//ps_out.header.frame_id直接输出不行，要转换成c风格
       try
       {
                  ROS_INFO("转换后的坐标为：(x =%.2f,y=%.2f,z=%.2f)参考的坐标系是：%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id.c_str());
       }
       catch(const std::exception& e)
       {
           ROS_INFO("程序错误...");
       }
       
        ros::spinOnce();
    }
    return 0;
}

配置文件此处略。

4.执行

使用命令行分别启动发布和订阅节点。即可实现转换后的坐标。如图：

好了，结束~

补充1:

当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:

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

示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser

也建议使用该种方式直接实现静态坐标系相对信息发布。

补充2:

可以借助于rviz显示坐标系关系，具体操作:

• 新建窗口输入命令:rviz;
• 在启动的 rviz 中设置Fixed Frame 为 base_link;
• 点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系。

如图：