ROS学习之路——第八章：tf坐标变换

最新推荐文章于 2024-05-29 16:54:03 发布

linixia

最新推荐文章于 2024-05-29 16:54:03 发布

阅读量644

点赞数

文章标签：学习

本文链接：https://blog.csdn.net/linixia/article/details/133656110

版权

translation：平移量 rotation：翻转量

一、静态坐标变换

1.概述

两个坐标系之间的相对位置是固定的，知道主体的坐标和雷达的坐标，通过雷达感知可以推算出主体距离障碍物的距离是多少

2.发布方发布

#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[])
{
    /* code */
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "static_pub");
    ros::NodeHandle nh;
    //创建发布对象
    tf2_ros::StaticTransformBroadcaster pub;
    geometry_msgs::TransformStamped tfs;
    //stamp指时间戳,now可以获取当前时刻
    tfs.header.stamp = ros::Time::now();
    //frame指的是相对坐标系中被参考的那一个
    tfs.header.frame_id = "base";
    //child_frame表示雷达的坐标系
    tfs.child_frame_id="laser";
    //偏移量
    tfs.transform.translation.x=0.2;
    tfs.transform.translation.y=0.0;
    tfs.transform.translation.z=0.5;
    //四元数根据欧拉角来转换
    tf2::Quaternion qtn;    //创建四元数对象
    //向该对象设置欧拉角
    qtn.setRPY(0,0,0);  //单位是弧度！
    tfs.transform.rotation.x=qtn.getX();
    tfs.transform.rotation.y=qtn.getY();
    tfs.transform.rotation.z=qtn.getZ();
    tfs.transform.rotation.w=qtn.getW();
    //发布数据
    pub.sendTransform(tfs);
    ros::spin();
    return 0;
}

3.订阅方订阅（tf算法实现坐标变换）

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"             //创建订阅对象（监听）
#include "tf2_ros/buffer.h"                         //将订阅到的数据存到buffer中
#include "geometry_msgs/PointStamped.h"             //组织一个坐标点数据
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"    //tf自己封装的消息

int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "static_sub");
    ros::NodeHandle nh; //必须创建，内部函数需要用到
    //创建订阅对象（需要对象+缓存）
        //创建buffer缓存
    tf2_ros::Buffer buffer;
        //创建监听对象
    tf2_ros::TransformListener listener(buffer);

    //创建坐标点对象
    geometry_msgs::PointStamped ps;
    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;

    //添加休眠:以便坐标点被订阅方订阅（这个不加会报错！）
    ros::Duration(2).sleep();

    //转换算法，需要调用TF内置实现
    ros::Rate rate(10); //发布频率10Hz
    while(ros::ok())
    {
        //将ps转换为base（pub中）的坐标点
        geometry_msgs::PointStamped ps_out; //输出的坐标
        /*buffer.transform()
        第一个参数为被转化的坐标，
        第二个参数为视为参考点的名称
        返回值是转化后的坐标
        */
        ps_out=buffer.transform(ps,"base");
        ROS_INFO("参考坐标系为:%s  转化后的坐标值:%.2f, %.2f, %.2f",
                ps_out.header.frame_id.c_str(),
                ps_out.point.x,
                ps_out.point.y,
                ps_out.point.z);

        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}
//调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h

4.可能会遇到的问题

（1）由于发布的坐标点不能马上被订阅方订阅到，所以在运行发布方后，可能会报错，因此我们需要再发布完坐标点后，让发布方休眠一段时间。

二、动态坐标变换

1.概述

获取动态的相对坐标

2.坐标发布方（同时需要订阅乌龟位姿信息）

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"  //用于发布动态坐标消息
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"  //欧拉角

/*
    订阅乌龟的位姿信息，转换成相对窗体的坐标
    准备
        话题：/turtle1/pose
        消息：/turtlesim/Pose
*/
void PoseCallback(const turtlesim::Pose::ConstPtr& pose)
{
    //获取位姿信息，转化为坐标系相对关系并发布
    //创建TF发布对象
    static tf2_ros::TransformBroadcaster pub;   //加static是为了防止每次进入回调函数都要重新定义一次
    //创建发布数据：从参数pose中进行转换
    geometry_msgs::TransformStamped ts;

    ts.header.frame_id="world"; //参考的坐标系（名字是固定的）
    ts.header.stamp=ros::Time::now();
    ts.child_frame_id="turtle1";    //相对的坐标系（名字是固定的）
    //坐标系平移量的设置
    ts.transform.translation.x = pose->x;   //这些值都可以通过rosmsg info turtlesim/Pose来查看的
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数
    //乌龟是2D的，因此没有翻滚和俯仰角，因此乌龟的欧拉角为0，0，theta
    tf2::Quaternion qtn;
    qtn.setRPY(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();
    //发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "dynamic_pub");
    ros::NodeHandle nh;
    //参数一：话题名称  参数二：队列长度  参数三：回调函数
    ros::Subscriber sub=nh.subscribe("turtle1/pose",100,PoseCallback);
    ros::spin();
    return 0;
}

3.坐标订阅方

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"             //创建订阅对象（监听）
#include "tf2_ros/buffer.h"                         //将订阅到的数据存到buffer中
#include "geometry_msgs/PointStamped.h"             //组织一个坐标点数据
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"    //tf自己封装的消息

int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "static_sub");
    ros::NodeHandle nh; //必须创建，内部函数需要用到
    //创建订阅对象（需要对象+缓存）
        //创建buffer缓存
    tf2_ros::Buffer buffer;
        //创建监听对象
    tf2_ros::TransformListener listener(buffer);

    //创建坐标点对象
    geometry_msgs::PointStamped ps;
    //参考的坐标系
    ps.header.frame_id = "turtle1";
    //时间戳
    ps.header.stamp = ros::Time(0.0);   //注意，发布动态消息时不可以发布实时的时间戳（不然会报错）
    ps.point.x=2.0;
    ps.point.y=3.0;
    ps.point.z=5.0;

    //添加休眠:以便坐标点被订阅方订阅（这个不加会报错！）
    ros::Duration(2).sleep();

    //转换算法，需要调用TF内置实现
    ros::Rate rate(10); //发布频率10Hz
    while(ros::ok())
    {
        //将ps转换为base（pub中）的坐标点
        geometry_msgs::PointStamped ps_out; //输出的坐标
        /*buffer.transform()
        第一个参数为被转化的坐标，
        第二个参数为视为参考点的名称
        返回值是转化后的坐标
        */
        ps_out=buffer.transform(ps,"world");
        ROS_INFO("参考坐标系为:%s  转化后的坐标值:%.2f, %.2f, %.2f",
                ps_out.header.frame_id.c_str(),
                ps_out.point.x,
                ps_out.point.y,
                ps_out.point.z);

        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}
//调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h

4.可能会遇到的问题

（1）时间戳不可以实时发布

三、多坐标变换

1.概述

父级坐标world下，有两个子级坐标，已知两个子级坐标相对于world的相对坐标，求一个子级相对于另一个子级的坐标

2.发布方

我们采用.launch文件的形式，直接发布对象

<launch>
    <!--发布son1相对于world 以及 son2相对于world的坐标关系-->
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="5 0 0 0 0 0 /world /son1" output="screen"/>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="3 0 0 0 0 0 /world /son2" output="screen"/>

</launch>

该情况下，我们发布的

son1坐标为（5,0,0），欧拉角为（0,0,0）

son2坐标为（3,0,0），欧拉角为（0,0,0）

使用时，直接启动laun文件即可

3.订阅方

#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"
#include "geometry_msgs/TransformStamped.h"

/*
    1.计算son1与son2的相对关系  2.计算son1的某个坐标点在son2中的坐标值


*/
int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "turtles");
    ros::NodeHandle nh;
    //创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    //编写解析逻辑
    ros::Rate rate(10);
        //创建相对于son1的坐标点
    geometry_msgs::PointStamped psson1;
    psson1.header.stamp =ros::Time::now();
    psson1.header.frame_id="son1";
    psson1.point.x=1.0;
    psson1.point.y=2.0;
    psson1.point.z=3.0;

    while(ros::ok())
    {
        try //可以避免因为发布订阅不同步造成的报错
        {
            //计算son1与son2的相对关系 
                /*buffer.lookupTransform()
                    参数1：根据该坐标系进行转化（父级）  
                    参数2：原坐标系（子级）  
                    参数3：何时的相对坐标
                    返回值：geometry_msgs::TransformStamped 子级相当于父级的相对坐标关系
                */
            geometry_msgs::TransformStamped son1toson2 = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("son1 相对于 son2 的信息：父级%s  子级%s // 偏移量：%.2f, %.2f, %.2f",
                    son1toson2.header.frame_id.c_str(), 
                    son1toson2.child_frame_id.c_str(),
                    son1toson2.transform.translation.x,
                    son1toson2.transform.translation.y,
                    son1toson2.transform.translation.z);
            //计算son1中的某个坐标点在son2中的坐标值
                //创建相对于son2坐标点，然后将son1的转化为相对于son2的
            geometry_msgs::PointStamped psson2 = buffer.transform(psson1,"son2");
            ROS_INFO("son1坐标点在son2中的值: %.2f, %.2f, %.2f",
                    psson2.point.x, psson2.point.y, psson2.point.z);

        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }
        
        
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

四、坐标系关系查看

1.rviz图像化显示

可以得到如下界面

2.tf2_tools 功能包

（1）安装：

roslin@roslin-VirtualBox:~$ sudo apt install ros-noetic-tf2-tools

（2）启动完程序后生成pdf文件：

roslin@roslin-VirtualBox:~$ rosrun tf2_tools view_frames.py

（3）终端中打开pdf文件：

roslin@roslin-VirtualBox:~$ evince frames.pdf

得到如下界面：

五、TF坐标变换实操

1.概述：

实时将动态的相对坐标信息发送给观察者，观察者根据实时更新的坐标信息，匹配到相对应的速度信息，进而实现有效跟随。

2.生成一只新的乌龟：

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{
    /* code */
    ros::init(argc,argv,"newturtle");
    ros::NodeHandle nh;
    ros::service::waitForService("/spawn"); //等待发现/spawn服务
    ros::ServiceClient add_turtle = nh.serviceClient<turtlesim::Spawn>("/spawn");

    turtlesim::Spawn srv;
    srv.request.x=2.0;
    srv.request.y=2.0;
    srv.request.name="turtle2";
    //请求服务调用
    ROS_INFO("Call service to spawn turtle[x:%0.6f, y:%0.6f, name:%s]",
    srv.request.x, srv.request.y, srv.request.name);
    
    add_turtle.call(srv);

    //显示服务调用结果
    ROS_INFO("Spawn turtle successfully [name:%s]",srv.response.name.c_str());

    return 0;
}

3.发布方（发布两只乌龟的坐标信息）

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"  //用于发布动态坐标消息
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"  //欧拉角

/*
    订阅乌龟的位姿信息，转换成相对窗体的坐标
    准备
        话题：/turtle1/pose
        消息：/turtlesim/Pose
*/

//声明变量，接收传递的参数
std::string turtle_name;

void PoseCallback(const turtlesim::Pose::ConstPtr& pose)
{
    //获取位姿信息，转化为坐标系相对关系并发布
    //创建TF发布对象
    static tf2_ros::TransformBroadcaster pub;   //加static是为了防止每次进入回调函数都要重新定义一次
    //创建发布数据：从参数pose中进行转换
    geometry_msgs::TransformStamped ts;

    ts.header.frame_id="world"; //参考的坐标系（名字是固定的）
    ts.header.stamp=ros::Time::now();
    //=====turtle1和turtle2是动态传入的====
    ts.child_frame_id=turtle_name;    //相对的坐标系（名字是固定的）
    //坐标系平移量的设置
    ts.transform.translation.x = pose->x;   //这些值都可以通过rosmsg info turtlesim/Pose来查看的
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数
    //乌龟是2D的，因此没有翻滚和俯仰角，因此乌龟的欧拉角为0，0，theta
    tf2::Quaternion qtn;
    qtn.setRPY(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();
    //发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "dynamic_pub");
    ros::NodeHandle nh;

    /* 
        要想动态传入turtle1和turtle2，需要解析launch文件
    */
    if(argc !=2 )
    {
        ROS_ERROR("请传入一个参数");
        return 1;
    }
    else
    {
        turtle_name = argv[1];
    }
    //创建订阅对象
        //====turtle1和turtle2是动态传入的====
        //参数一：话题名称  参数二：队列长度  参数三：回调函数
        //注意：动态传入话题时，pose前要加/ ！！！
    ros::Subscriber sub=nh.subscribe(turtle_name+"/pose",100,PoseCallback);
    ros::spin();
    return 0;
}

4.控制乌龟跟随

#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"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

/*
    1.换算turtle1相对于turtle2的关系  2.计算角速度和线速度并发布


*/
int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "turtles");
    ros::NodeHandle nh;
    //创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    //创建发布对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);
    //编写解析逻辑
    ros::Rate rate(10);

    while(ros::ok())
    {
        try //可以避免因为发布订阅不同步造成的报错
        {
            //计算son1与son2的相对关系 
                /*buffer.lookupTransform()
                    参数1：根据该坐标系进行转化（父级）  
                    参数2：原坐标系（子级）  
                    参数3：何时的相对坐标
                    返回值：geometry_msgs::TransformStamped 子级相当于父级的相对坐标关系
                */
            geometry_msgs::TransformStamped son1toson2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
            ROS_INFO("turtle1 相对于 turtle2 的信息：父级%s  子级%s // 偏移量：%.2f, %.2f, %.2f",
                    son1toson2.header.frame_id.c_str(), 
                    son1toson2.child_frame_id.c_str(),
                    son1toson2.transform.translation.x,
                    son1toson2.transform.translation.y,
                    son1toson2.transform.translation.z);

            //根据相对坐标计算并组织速度消息
            geometry_msgs::Twist twist;
            /*
                twist.linear.x = 系数*开二次方（x^2+y^2)
                twist.angular.z = 系数*反正切（对边/邻边）
            */
            twist.linear.x = 0.5 * sqrt(pow(son1toson2.transform.translation.x,2)+pow(son1toson2.transform.translation.y,2));
            twist.angular.z = 4*atan2(son1toson2.transform.translation.y,son1toson2.transform.translation.x);
            
            //发布
            pub.publish(twist);

        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }
        
        
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

linixia

关注

0
点赞
踩
4

收藏

觉得还不错? 一键收藏
1
评论
ROS学习之路——第八章：tf坐标变换

获取动态的相对坐标父级坐标world下，有两个子级坐标，已知两个子级坐标相对于world的相对坐标，求一个子级相对于另一个子级的坐标实时将动态的相对坐标信息发送给观察者，观察者根据实时更新的坐标信息，匹配到相对应的速度信息，进而实现有效跟随。
复制链接

扫一扫