ROS学习笔记(四)

TF坐标变换

tf2是转换库的第二代,它允许用户随时间跟踪多个坐标帧。tf2在时间缓冲的树结构中维护坐标帧之间的关系,并允许用户在任何所需的时间点在任意两个坐标帧之间转换点、向量等。

坐标msgs消息

在坐标转换实现中常用的

msg:geometry_msgs/TransformStampedgeometry_msgs/PointStamped

前者用于传输坐标系相关位置信息,后者用于传输某个坐标系内坐标点的信息。在坐标变换中,频繁的需要使用到坐标系的相对关系以及坐标点信息。

ROS学习笔记(四)_第1张图片

ROS学习笔记(四)_第2张图片

ROS学习笔记(四)_第3张图片

静态坐标变换

是指两个坐标系之间的相对位置是固定的

创建项目功能包依赖于 tf2tf2_rostf2_geometry_msgsroscpp rospy std_msgs geometry_msgs

ROS学习笔记(四)_第4张图片

 发布方代码

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"//发布对象头文件,静态坐标广播器
#include "geometry_msgs/TransformStamped.h"//用于传输坐标系相关位置信息
#include "tf2/LinearMath/Quaternion.h"
/*
    需求:发布两个坐标系的相对关系
    流程:
        1.包含头文件
        2.设置编码 节点初始化 NodeHandle
        3.创建发布者消息
        4.组织被发布的消息
        5.发布数据
        6.spin()
*/
int main(int argc, char *argv[])
{
    // 1.包含头文件
    // 2.设置编码 节点初始化 NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_pub");
    ros::NodeHandle nh;
    // 3.创建发布者消息,,静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster pub;
    // 4.组织被发布的消息,,创建坐标信息
    geometry_msgs::TransformStamped tfs;
    tfs.header.stamp = ros::Time::now();//获取当前时刻
    tfs.header.frame_id = "base_link";//相对坐标系中被参考的那一个
    tfs.child_frame_id = "laser";
    //设置子级相对于父级的偏移量(translation)
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0;
    tfs.transform.translation.z = 0.5;

    //需要根据欧辣角转换
    tf2::Quaternion qtn; //创建 四元数(rotation) 对象
    //向该对象设置欧拉角,这个对象可以将欧拉角转换成四元数
    qtn.setRPY(0,0,0);//欧拉角的单位是弧度,如果雷达装反了,设置成3.14

    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();

    // 5.发布数据
    pub.sendTransform(tfs);
    // 6.spin()
    ros::spin();
    return 0;
}

订阅方代码

#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" //注意: 调用 transform 必须包含该头文件
/*
    流程:
        1.包含头文件
        2.编码、初始化、NodeHandle
        3.创建订阅者对象------>订阅坐标系相对关系
        4.组织一个坐标点数据
        5.转换算法,需要调用TF内置实现
        6.最后输出
*/
int main(int argc, char *argv[])
{
    // 2.编码、初始化、NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    // 3.创建订阅者对象------>订阅坐标系相对关系
    //3-1 创建一个 buffer 缓存
    tf2_ros::Buffer buffer;
    //3-2 再创建监听对象(监听对象可以将订阅的数据存入buffer)
    tf2_ros::TransformListener listener(buffer);
    // 4.组织一个坐标点数据
    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();
    // 5.转换算法,需要调用TF内置实现
    ros::Rate rate(10);
    while (ros::ok())
    {
        //将 ps 转换成相对于 base_link 的坐标点
        geometry_msgs::PointStamped ps_out;
        /*
            调用了 buffer 的转换函数 transform
            参数1:被转换的坐标点
            参数2.目标坐标系
            返回值:输出的坐标点

            PS1: 调用 transform 必须包含该头文件  tf2_geometry_msgs/tf2_geometry_msgs.h
            PS2:运行时存在的问题:抛出一个异常 base_link 不存在
                原因:订阅数据是一个耗时操作,可能再调用 transform 转换函数时,坐标系的相对关系  还没有订阅到,因此异常。
                解决1:在调用转换函数前,执行休眠
                    2:进行异常处理 try
        */
       try
       {
            ps_out = buffer.transform(ps,"base_link");
    // 6.最后输出
            ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                    ps_out.point.x,
                    ps_out.point.y,
                    ps_out.point.z,
                    ps_out.header.frame_id.c_str());
            rate.sleep();
            ros::spinOnce();
       }
       catch(const std::exception& e)
       {
        //std::cerr << e.what() << '\n';
        ROS_INFO("异常消息:%s",e.what());
       }
       
        
    }
    
    return 0;
}

补充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 组件,即可显示坐标关系。

动态坐标变换

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

需求描述:

启动 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. 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
  3. 创建坐标相对关系订阅方
  4. 执行

编写发布方程序

#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
        消息:/turtle1/Pose
    流程:
        1.包含头文件
        2.设置编码、初始化、句柄。
        3.创建订阅对象,订阅 /turtle1/pose
        4.回调函数处理订阅的消息:将位姿信息转换成坐标相对关系并发布
        5.回旋spin()
*/
//const 只读,确保引用的数据不被修改
void doPose(const turtlesim::Pose::ConstPtr& pose){
    //获取位姿信息,转换成坐标系相对关系(核心),并发布
    //a.创建发布对象
    static tf2_ros::TransformBroadcaster pub;//使用static关键字,变成静态的。每次回调都使用同一个pub对象
    //b.组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";

    //坐标偏移量设置,2D 所以z是0
    ts.transform.translation.x = pose->x;
    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();

    //c.发布
    pub.sendTransform(ts);
}
int main(int argc, char *argv[])
{
    // 2.设置编码、初始化、句柄。
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    // 3.创建订阅对象,订阅 /turtle1/pose
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
    // 4.回调函数处理订阅的消息:将位姿信息转换成坐标相对关系并发布
    // 5.回旋spin()
    ros::spin();
    return 0;
}

                                 roscore

启动乌龟节点          rosrun turtlesim turtlesim_node

启动键盘控制节点      rosrun turtlesim turtle_teleop_key

启动文件              source ./devel/setup.bash

rosrun tf02_dynamic demo01_dynamic_pub(注意回旋函数)

rostopic echo /tf(移动乌龟数据会有变化)

编写订阅方代码(除参考的坐标系,目标坐标系,时间戳以外,和静态坐标转换相同)

#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" //注意: 调用 transform 必须包含该头文件
/*
    流程:
        1.包含头文件
        2.编码、初始化、NodeHandle
        3.创建订阅者对象------>订阅坐标系相对关系
        4.组织一个坐标点数据
        5.转换算法,需要调用TF内置实现
        6.最后输出
*/
int main(int argc, char *argv[])
{
    // 2.编码、初始化、NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_sub");
    ros::NodeHandle nh;
    // 3.创建订阅者对象------>订阅坐标系相对关系
    //3-1 创建一个 buffer 缓存
    tf2_ros::Buffer buffer;
    //3-2 再创建监听对象(监听对象可以将订阅的数据存入buffer)
    tf2_ros::TransformListener listener(buffer);
    // 4.组织一个坐标点数据
    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 = 0;
    //添加休眠,防止接受不到信息
    //ros::Duration(2).sleep();
    // 5.转换算法,需要调用TF内置实现
    ros::Rate rate(10);
    while (ros::ok())
    {
        //将 ps 转换成相对于 base_link 的坐标点
        geometry_msgs::PointStamped ps_out;
        /*
            调用了 buffer 的转换函数 transform
            参数1:被转换的坐标点
            参数2.目标坐标系
            返回值:输出的坐标点

            PS1: 调用 transform 必须包含该头文件  tf2_geometry_msgs/tf2_geometry_msgs.h
            PS2:运行时存在的问题:抛出一个异常 base_link 不存在
                原因:订阅数据是一个耗时操作,可能再调用 transform 转换函数时,坐标系的相对关系  还没有订阅到,因此异常。
                解决1:在调用转换函数前,执行休眠
                    2:进行异常处理 try
        */
       try
       {
            ps_out = buffer.transform(ps,"world");
    // 6.最后输出
            ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                    ps_out.point.x,
                    ps_out.point.y,
                    ps_out.point.z,
                    ps_out.header.frame_id.c_str());
            rate.sleep();
            ros::spinOnce();
       }
       catch(const std::exception& e)
       {
        //std::cerr << e.what() << '\n';
        ROS_INFO("异常消息:%s",e.what());
       }
       
        
    }
    
    return 0;
}

roscore

启动乌龟节点          rosrun turtlesim turtlesim_node

启动文件              source ./devel/setup.bash

rosrun tf02_dynamic demo01_dynamic_pub(注意回旋函数)

新建窗口                     source ./devel/setup.bash

rosrun tf02_dynamic demo02_dynamic_sub

启动键盘控制节点      rosrun turtlesim turtle_teleop_key

多坐标变换

需求描述:

现有坐标系统,父级坐标系统 world,下有两子级系统 son1,son2,son1 相对于 world,以及 son2 相对于 world 的关系是已知的,求 son1原点在 son2中的坐标,又已知在 son1中一点的坐标,要求求出该点在 son2 中的坐标

实现分析:

  1. 首先,需要发布 son1 相对于 world,以及 son2 相对于 world 的坐标消息
  2. 然后,需要订阅坐标发布消息,并取出订阅的消息,借助于 tf2 实现 son1 和 son2 的转换
  3. 最后,还要实现坐标点的转换

打开launch文件


    
    

打开订阅方文件

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

/*
    订阅方实现, 1.计算son1与son2的相对关系
                2.计算son1中某坐标点在 son2 中的坐标值
*/
int main(int argc, char *argv[])
{   
    
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
    //创建订阅者对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    //创建坐标点
    geometry_msgs::PointStamped psAtSon1;
    psAtSon1.header.stamp = ros::Time::now();
    psAtSon1.header.frame_id = "son1";
    psAtSon1.point.x = 1.0;
    psAtSon1.point.y = 2.0;
    psAtSon1.point.z = 3.0;
    //编写解析逻辑
    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //1.计算son1与son2的相对关系
            /*
                A 相对于 B 的坐标系关系
                参数1:目标坐标系   B
                参数2:源坐标系     A
                参数3.ros::Time(0) 取间隔最短的两个坐标系关系帧计算相应关系
                返回值:gemetry_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             
                    );
            //2.计算son1中的某个坐标点在 son2 中的坐标值
            geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2");
            ROS_INFO("坐标点在 Son2 中的值(%.2f,%.2f,%.2f)",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.point.z
                    );

        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }
        

        rate.sleep();
        ros::spinOnce();

    }
    return 0;
}

坐标关系

sudo apt install ros-noetic-tf2-tools

rosrun tf2_tools view_frames.py

ROS学习笔记(四)_第5张图片

坐标变换实操

控制乌龟运动,编写乌龟启动launch文件

ROS学习笔记(四)_第6张图片

 

启动launch文件

启动第二只乌龟运动

rostopic list

rostopic pub -r 10 /turtle2/cmd_vel geometry_msgs/Twist

修改参数

服务客户端(生成乌龟)

/* 
    创建第二只小乌龟
 */
#include "ros/ros.h"
#include "turtlesim/Spawn.h"

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

    setlocale(LC_ALL,"");

    //执行初始化
    ros::init(argc,argv,"create_turtle");
    //创建节点
    ros::NodeHandle nh;
    //创建服务客户端
    ros::ServiceClient client = nh.serviceClient("/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 = 3.12415926;
    bool flag = client.call(spawn);
    if (flag)
    {
        ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("乌龟2创建失败!");
    }

    ros::spin();

    return 0;
}

发布两只乌龟信息

  • 该节点需要启动两次
  • 每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)
/*  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose,然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的,
          所以我们可以将所需的命名空间通过 args 动态传入

    实现流程:
        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 broadcaster;
    //  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;
    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.发布
    broadcaster.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("请传入正确的参数");
    } else {
        turtle_name = argv[1];
        ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
    }

    // 4.创建 ros 句柄
    ros::NodeHandle nh;
    // 5.创建订阅对象
    ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",1000,doPose);
    //     6.回调函数处理订阅的 pose 信息
    //         6-1.创建 TF 广播器
    //         6-2.将 pose 信息转换成 TransFormStamped
    //         6-3.发布
    // 7.spin
    ros::spin();
    return 0;
}

订阅方,解析坐标信息并生成速度信息

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

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 TF 订阅对象
        5.处理订阅到的 TF
        6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_TF");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    // 5.处理订阅到的 TF

    // 需要创建发布 /turtle2/cmd_vel 的 publisher 对象

    ros::Publisher pub = nh.advertise("/turtle2/cmd_vel",1000);

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //5-1.先获取 turtle1 相对 turtle2 的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));

            //5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);

            //5-3.发布速度信息 -- 需要提前创建 publish 对象
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("错误提示:%s",e.what());
        }



        rate.sleep();
        // 6.spin
        ros::spinOnce();
    }

    return 0;
}

运行launch文件



    
    
    
    
    
    
    
    
    
    

你可能感兴趣的:(ros仿真,学习,深度学习,人工智能)