ROS常用组件（二）-----TF坐标变换

最新推荐文章于 2024-03-18 01:09:19 发布

不爱敲代码的郑小可

最新推荐文章于 2024-03-18 01:09:19 发布

阅读量799

点赞数 1

分类专栏： ROS笔记

本文链接：https://blog.csdn.net/dawenzideboke/article/details/114010576

版权

ROS笔记专栏收录该内容

8 篇文章 0 订阅

订阅专栏

坐标变换是机器人系统中常用的基础功能，ROS中坐标变换由TF功能包维护。它可以根据时间缓冲并维护多个坐标系之间的坐标变换关系，目前 tf2 比 tf 用的多。

1. 坐标变换常用msg

1.1 geometry_msgs/TransformStamped–>用于传输坐标系相关位置信息
命令行键入:rosmsg info geometry_msgs/TransformStamped

std_msgs/Header header                      #头信息
  uint32 seq                                #序列号
  time stamp                                #时间戳
  string frame_id                           #父级坐标名称
string child_frame_id                       #子级坐标名称
geometry_msgs/Transform transform           #坐标信息
  geometry_msgs/Vector3 translation         #偏移量
    float64 x                                
    float64 y                                 
    float64 z                               
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

1.2 geometry_msgs/PointStamped–>用于传输某个坐标系内坐标点的信息
命令行键入:rosmsg info geometry_msgs/PointStamped

std_msgs/Header header                    #头信息
  uint32 seq                              #序号
  time stamp                              #时间戳
  string frame_id                         #所属坐标系的名称
geometry_msgs/Point point                 #固定坐标点坐标
  float64 x                               
  float64 y
  float64 z

注：创建坐标变换功能包要添加的依赖roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

2 . 静态坐标信息

所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。
要想实现静态坐标变换，可以通过话题通信发布订阅消息，发布方发布坐标系相对关系，订阅方订阅发布的坐标系相对关系，再传入坐标点信息（可以是固定的一点），借助TF实现坐标变换.

发布方核心：
创建静态发布对象–>tf2_ros::StaticTransformBroadcaster pub;
创建坐标系信息–>
geometry_msgs::TransformStamped ts;（头信息、子级坐标系、偏移量、四元数）
发布消息–> pub.sendTransform(ts);
订阅方核心：
创建订阅对象–>

tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);

创建一个坐标点（相对于子级）–>geometry_msgs::PointStamped ps;
坐标转换,需要新建一个坐标点（父级）接收转换结果–>

geometry_msgs::PointStamped ps_out;
ps_out = buffer.transform(ps_out,"父级坐标系名称");

注意：在进行坐标变换时，使用try语句或者在坐标变换前实行休眠，可以避免由于缓存接收延迟而导致坐标变换失败

3. 动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。
实现和静态坐标变换差不多，订阅方是一样的，发布方在实现的时候是要先订阅一个动态的对象（比如：小乌龟的位姿），然后回调函数处理订阅到的数据，处理过程和静态坐标变换发布方实现一样

4. 多坐标变换

多个坐标系之间的相对位置关系变换。例如：现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

实现:首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息，然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换，最后实现坐标点的转换
关于发布方，用launch文件封装，且使用静态坐标发布变换

<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
<node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />    
</launch>

订阅方实现核心：
解析son1中的点相对于son2的坐标（坐标变换）

geometry_msgs::TransformStamped tfs =buffer.lookupTransform("son2","son1",ros::Time(0))；
实现坐标点的转换 
geometry_msgs::PointStamped ps; //创建一个坐标点（相对于子级son1）
geometry_msgs::PointStamped psAtSon2; //新建一个坐标点（相对父级son2）接收转换结果
psAtSon2 = buffer.transform(ps,"son2");

5 坐标关系查看

可以通过tf2_tools工具查看，安装指令：
sudo apt install ros-noetic-tf2-tools
启动坐标系发布程序之后，运行如下命令:rosrun tf2_tools view_frames.py
当前目录会生成一个 frames.pdf 文件。

案例实现：乌龟例程中的TF变换
需求描述：运行程序，产生两只乌龟 turtle1 和 turtle2,键盘控制 turtle1 运动，turtle2 可以实现跟随运动
流程：
（1）启动乌龟显示节点
（2）编写服务客户端，生成一只新乌龟（test01_new_turtle）

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

int main(int argc, char *argv[])
{

    setlocale(LC_ALL,"");
    //执行初始化
    ros::init(argc,argv,"create_turtle2");
    //创建节点
    ros::NodeHandle nh;
    //创建服务客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    ros::service::waitForService("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 1.57;
    client.waitForExistence();
    bool flag = client.call(spawn);
    //处理响应
    if (flag)
    {
        ROS_INFO("乌龟生成成功，新乌龟叫：%s",spawn.response.name.c_str());
    } else
    {
        ROS_INFO("请求失败！！！");
    }
    
    return 0;
}

（3）编写发布方，发布两只乌龟相对于世界坐标系的坐标信息(test02_pub_turtle)

订阅乌龟的位姿信息，然后再转换成坐标信息，两只乌龟的实现逻辑相同，只是话题名称、各自的坐标不同，可通过参数传入:
该节点需要启动两次；
每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

/*  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后发布相对 world 的坐标系信息

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.解析传入的命名空间
        4.创建 ros 句柄
        5.创建订阅对象
        6.回调函数处理订阅的 pose 信息
            6-1.创建 TF 广播器
            6-2.将 pose 信息转换成 TransFormStamped
            6-3.发布
        7.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"


//声明变量接收传递的参数----保存乌龟名称
std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr& pose){
    // 6-1.创建 TF 广播器 ---------------------------------------- 注意 static
    static tf2_ros::TransformBroadcaster pub;
    // 6-2.将 pose 信息转换成 TransFormStamped
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    //动态传入
    tfs.child_frame_id = turtle_name;
    //坐标系偏移量设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0;
    //坐标系四元数----欧拉角转换四元数
    //乌龟位姿信息没有四元数，但有偏航角度，乌龟位姿的欧拉角：0 0 theta
    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();
    //  6-3.发布消息
    pub.sendTransform(tfs);

} 

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"pub_tf");
    // 3.解析传入的命名空间
    if (argc != 2)
    {
        ROS_ERROR("请传入一个参数");
        return 1;
    } else {
        turtle_name = argv[1];
    }

    // 4.创建 ros 句柄
    ros::NodeHandle nh;
    // 5.创建订阅对象--订阅乌龟的pose信息（动态传入乌龟名称）
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",100,doPose);
    // 6.回调函数处理订阅的 pose 信息
     
    // 7.spin
    ros::spin();
    return 0;
}

（4）编写订阅方，订阅两只乌龟信息并生成新的坐标相对关系生成速度信息
(test03_control_turtle)

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

/*
    订阅 turtle1 和 turtle2 的发布信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

    流程：
        1.包含头文件
        2.编码初始化、NodeHandle;
        3.创建订阅对象
        4.编写解析逻辑
        5.spinOnce()

*/
int main(int argc, char *argv[])
{
    // 2.编码初始化、NodeHandle;
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_pub");
    ros::NodeHandle nh;
    // 3.创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

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

    ros::Rate rate(1);
    // ros::Duration(2).sleep();
    while (ros::ok())
    {
        //核心
        try
        {
            //计算turtle1相对于turtle2的关系
            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(), //turtle2
            // son1ToSon2.child_frame_id.c_str(),  // turtle1
            // son1ToSon2.transform.translation.x,
            // son1ToSon2.transform.translation.y,
            // son1ToSon2.transform.translation.z);           
            
            //B. 根据相对关系计算并组织速度消息
            geometry_msgs::Twist twist;
            /*
                组织速度，只需要设置线速度的 x 与角速度的 z
                x = 系数 * 开方（y^2 + x^2)
                z = 系数 * 反正切（对边，邻边）
            */
            twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x,2) + pow(son1ToSon2.transform.translation.y,2));
            twist.angular.z = 1.0 * atan2(son1ToSon2.transform.translation.y,son1ToSon2.transform.translation.x);
            //C. 发布
            pub.publish(twist);

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

（5）实现乌龟跟随运动
编写一个launch文件(test.launch)，让所有节点运动起来

<launch>
    <!--1.启动乌龟GUI-->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />

    <!--2.生成新的乌龟节点-->
    <node pkg="tf04_test" type="test01_new_turtle" name="turtle2" output="screen" />

    <!--3.需要启动两个乌龟相对于世界的坐标关系发布-->
    <!--
        基本实现思路：
            1.节点编写一个
            2.这个节点需要启动两次
            3.节点启动时动态传参：第一次传递turtle1 第二次启动传递turtle2
    -->
    <node pkg="tf04_test" type="test02_pub_turtle" name="pub1" args="turtle1" output="screen" />
    <node pkg="tf04_test" type="test02_pub_turtle" name="pub2" args="turtle2" output="screen" />

    <!--4.订阅turtle1 与 turtle2 相对于世界坐标系的坐标消息，并转换成turtle1相对于turtle2的坐标关系
            再生成速度关系
    -->
    <node pkg="tf04_test" type="test03_control_turtle2" name="control" output="screen" />


</launch>

不爱敲代码的郑小可

关注

1
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
ROS常用组件（二）-----TF坐标变换

坐标变换是机器人系统中常用的基础功能，ROS中坐标变换由TF功能包维护。它可以根据时间缓冲并维护多个坐标系之间的坐标变换关系，目前 tf2 比 tf 用的多。1. 坐标变换常用msg1.1 geometry_msgs/TransformStamped–>用于传输坐标系相关位置信息命令行键入:rosmsg info geometry_msgs/TransformStampedstd_msgs/Header header #头信息 uint32 .
复制链接

扫一扫