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

原文请点击。

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

需求描述:

启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

实现分析:

1. 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点

2. 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度

3. 将 pose 信息转换成 坐标系相对信息并发布

1.创建功能包

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

2.发布方

/*  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅对象
        5.回调函数处理订阅到的数据(实现TF广播)
            5-1.创建 TF 广播器
            5-2.创建 广播的数据(通过 pose 设置)
            5-3.广播器发布数据
        6.spin
*/

#include<ros/ros.h>
#include"tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/Quaternion.h"
#include "turtlesim/Pose.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/LinearMath/Quaternion.h"


void dopose(const turtlesim::Pose::ConstPtr &pose)
{
    //  5-1.创建 TF 广播器
     static  tf2_ros::TransformBroadcaster broadcast;

    // 5-2.创建 广播的数据(通过 pose 设置),(即创建父子坐标系的位置相对关系 ）
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    ts.child_frame_id = "turtle";
    //创建父子坐标系的姿态相对关系，并且把欧拉角转换为四元数
        tf2::Quaternion qtn;
        qtn.setEuler(0,0,pose->theta);
        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.3把父子坐标系的相对关系数据广播出去
    broadcast.sendTransform(ts);
}




int main(int argc, char  *argv[])
{
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dyminac_frame_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",10,dopose);
    //   5.回调函数处理订阅到的数据(实现TF广播)

    // 6.spin
    ros::spin();
    return 0;
}

配置文件此处略。

3.订阅方

#include <ros/ros.h>
#include "tf2_ros/buffer.h"
#include"tf2_ros/transform_listener.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"


int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    
    //2.初始化节点
    ros::init(argc,argv,"dyminac_frame_sub");
    ros::NodeHandle nh;

    //3.创建缓存，用于存入监听到的 变换矩阵
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener tfl(buffer);
    
    //ros::Duration(10).sleep();这条语句和 下面的try语句，二选一。使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败。
    ros::Rate rate(1);


    while(ros::ok())
    {
        //4.创建一个坐标点数据，这个就是即将要转换的对象
        geometry_msgs::PointStamped ps; //要包含#include "tf2_geometry_msgs/tf2_geometry_msgs.h"这个头文件才行
        ps.header.frame_id = "turtle";
        ps.header.stamp = ros::Time(0); //不能使用ros::time::now();因为监听做不到实时，会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据，ros::time::now()则指当下，如果非要使用ros::time::now,则需要结合waitForTransform()使用。
        ps.point.x = 1;
        ps.point.y = 1;
        ps.point.z = 0;
 
        try
        {
            //5.创建一个新的坐标点，用于接收被转换的坐标点数据
            geometry_msgs::PointStamped ps_out;
            ps_out = buffer.transform(ps,"world");//新的坐标点数据来
            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("程序错误%s..",e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

配置文件此处略。

4.执行

可以使用命令行（这里直接用命令行的方式启动发布和订阅了，而不用launch文件了，launch只用于启动了乌龟仿真器和乌龟控制节点，因为如果发布和订阅放再launch中一起启动后，有一些需要终端输出的结果不方便观察）的方式分别启动发布节点与订阅节点。

然后可以使启动rviz，再启动乌龟仿真器，和键盘控制乌龟的节点就可以直观的 查看坐标系相对关系。

PS：launch启动乌龟仿真器（turtlesim）和控制乌龟运动的键盘的launch文件的编写如下：

结果演示：

 终端分别启动了launch文件，发布节点，订阅节点。

启动rviz中的图：

全部效果放在一起的图：

结束~