devbox
代码架构师
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

ROS学习之路——第八章：tf坐标变换_rostf轨迹转换

作者：代码架构师 | 2024-01-31 16:45:17

赞

踩

rostf轨迹转换

translation：平移量         rotation：翻转量

一、静态坐标变换

1.概述

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

2.发布方发布

  1. #include "ros/ros.h"
  2. #include "tf2_ros/static_transform_broadcaster.h"   //静态的依赖文件
  3. #include "geometry_msgs/TransformStamped.h" //用于实现坐标转换
  4. #include "tf2/LinearMath/Quaternion.h"  //数据处理用的文件
  5. /*
  6.     发布两个坐标的相对关系
  7. */
  8.  
  9. int main(int argc, char *argv[])
  10. {
  11.     /* code */
  12.     setlocale(LC_ALL,"");
  13.     ros::init(argc, argv, "static_pub");
  14.     ros::NodeHandle nh;
  15.     //创建发布对象
  16.     tf2_ros::StaticTransformBroadcaster pub;
  17.     geometry_msgs::TransformStamped tfs;
  18.     //stamp指时间戳,now可以获取当前时刻
  19.     tfs.header.stamp = ros::Time::now();
  20.     //frame指的是相对坐标系中被参考的那一个
  21.     tfs.header.frame_id = "base";
  22.     //child_frame表示雷达的坐标系
  23.     tfs.child_frame_id="laser";
  24.     //偏移量
  25.     tfs.transform.translation.x=0.2;
  26.     tfs.transform.translation.y=0.0;
  27.     tfs.transform.translation.z=0.5;
  28.     //四元数根据欧拉角来转换
  29.     tf2::Quaternion qtn;    //创建四元数对象
  30.     //向该对象设置欧拉角
  31.     qtn.setRPY(0,0,0);  //单位是弧度！
  32.     tfs.transform.rotation.x=qtn.getX();
  33.     tfs.transform.rotation.y=qtn.getY();
  34.     tfs.transform.rotation.z=qtn.getZ();
  35.     tfs.transform.rotation.w=qtn.getW();
  36.     //发布数据
  37.     pub.sendTransform(tfs);
  38.     ros::spin();
  39.     return 0;
  40. }

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

  1. #include "ros/ros.h"
  2. #include "tf2_ros/transform_listener.h"             //创建订阅对象（监听）
  3. #include "tf2_ros/buffer.h"                         //将订阅到的数据存到buffer中
  4. #include "geometry_msgs/PointStamped.h"             //组织一个坐标点数据
  5. #include "tf2_geometry_msgs/tf2_geometry_msgs.h"    //tf自己封装的消息
  6.  
  7. int main(int argc, char *argv[])
  8. {
  9.     /* code */
  10.     setlocale(LC_ALL,"");
  11.     ros::init(argc, argv, "static_sub");
  12.     ros::NodeHandle nh; //必须创建，内部函数需要用到
  13.     //创建订阅对象（需要对象+缓存）
  14.         //创建buffer缓存
  15.     tf2_ros::Buffer buffer;
  16.         //创建监听对象
  17.     tf2_ros::TransformListener listener(buffer);
  18.  
  19.     //创建坐标点对象
  20.     geometry_msgs::PointStamped ps;
  21.     ps.header.frame_id = "laser";
  22.     ps.header.stamp = ros::Time::now();
  23.     ps.point.x=2.0;
  24.     ps.point.y=3.0;
  25.     ps.point.z=5.0;
  26.  
  27.     //添加休眠:以便坐标点被订阅方订阅（这个不加会报错！）
  28.     ros::Duration(2).sleep();
  29.  
  30.     //转换算法，需要调用TF内置实现
  31.     ros::Rate rate(10); //发布频率10Hz
  32.     while(ros::ok())
  33.     {
  34.         //将ps转换为base（pub中）的坐标点
  35.         geometry_msgs::PointStamped ps_out; //输出的坐标
  36.         /*buffer.transform()
  37.         第一个参数为被转化的坐标，
  38.         第二个参数为视为参考点的名称
  39.         返回值是转化后的坐标
  40.         */
  41.         ps_out=buffer.transform(ps,"base");
  42.         ROS_INFO("参考坐标系为:%s  转化后的坐标值:%.2f, %.2f, %.2f",
  43.                 ps_out.header.frame_id.c_str(),
  44.                 ps_out.point.x,
  45.                 ps_out.point.y,
  46.                 ps_out.point.z);
  47.  
  48.         rate.sleep();
  49.         ros::spinOnce();
  50.     }
  51.     return 0;
  52. }
  53. //调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
  54.

4.可能会遇到的问题

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

二、动态坐标变换

1.概述

获取动态的相对坐标

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

  1. #include "ros/ros.h"
  2. #include "turtlesim/Pose.h"
  3. #include "tf2_ros/transform_broadcaster.h"  //用于发布动态坐标消息
  4. #include "geometry_msgs/TransformStamped.h"
  5. #include "tf2/LinearMath/Quaternion.h"  //欧拉角
  6.  
  7. /*
  8.     订阅乌龟的位姿信息，转换成相对窗体的坐标
  9.     准备
  10.         话题：/turtle1/pose
  11.         消息：/turtlesim/Pose
  12. */
  13. void PoseCallback(const turtlesim::Pose::ConstPtr& pose)
  14. {
  15.     //获取位姿信息，转化为坐标系相对关系并发布
  16.     //创建TF发布对象
  17.     static tf2_ros::TransformBroadcaster pub;   //加static是为了防止每次进入回调函数都要重新定义一次
  18.     //创建发布数据：从参数pose中进行转换
  19.     geometry_msgs::TransformStamped ts;
  20.  
  21.     ts.header.frame_id="world"; //参考的坐标系（名字是固定的）
  22.     ts.header.stamp=ros::Time::now();
  23.     ts.child_frame_id="turtle1";    //相对的坐标系（名字是固定的）
  24.     //坐标系平移量的设置
  25.     ts.transform.translation.x = pose->x;   //这些值都可以通过rosmsg info turtlesim/Pose来查看的
  26.     ts.transform.translation.y = pose->y;
  27.     ts.transform.translation.z = 0;
  28.     //坐标系四元数
  29.     //乌龟是2D的，因此没有翻滚和俯仰角，因此乌龟的欧拉角为0，0，theta
  30.     tf2::Quaternion qtn;
  31.     qtn.setRPY(0,0,pose->theta);
  32.         //设置四元数
  33.     ts.transform.rotation.x=qtn.getX();
  34.     ts.transform.rotation.y=qtn.getY();
  35.     ts.transform.rotation.z=qtn.getZ();
  36.     ts.transform.rotation.w=qtn.getW();
  37.     //发布
  38.     pub.sendTransform(ts);
  39. }
  40.  
  41. int main(int argc, char *argv[])
  42. {
  43.     /* code */
  44.     setlocale(LC_ALL,"");
  45.     ros::init(argc, argv, "dynamic_pub");
  46.     ros::NodeHandle nh;
  47.     //参数一：话题名称  参数二：队列长度  参数三：回调函数
  48.     ros::Subscriber sub=nh.subscribe("turtle1/pose",100,PoseCallback);
  49.     ros::spin();
  50.     return 0;
  51. }

3.坐标订阅方

  1. #include "ros/ros.h"
  2. #include "tf2_ros/transform_listener.h"             //创建订阅对象（监听）
  3. #include "tf2_ros/buffer.h"                         //将订阅到的数据存到buffer中
  4. #include "geometry_msgs/PointStamped.h"             //组织一个坐标点数据
  5. #include "tf2_geometry_msgs/tf2_geometry_msgs.h"    //tf自己封装的消息
  6.  
  7. int main(int argc, char *argv[])
  8. {
  9.     /* code */
  10.     setlocale(LC_ALL,"");
  11.     ros::init(argc, argv, "static_sub");
  12.     ros::NodeHandle nh; //必须创建，内部函数需要用到
  13.     //创建订阅对象（需要对象+缓存）
  14.         //创建buffer缓存
  15.     tf2_ros::Buffer buffer;
  16.         //创建监听对象
  17.     tf2_ros::TransformListener listener(buffer);
  18.  
  19.     //创建坐标点对象
  20.     geometry_msgs::PointStamped ps;
  21.     //参考的坐标系
  22.     ps.header.frame_id = "turtle1";
  23.     //时间戳
  24.     ps.header.stamp = ros::Time(0.0);   //注意，发布动态消息时不可以发布实时的时间戳（不然会报错）
  25.     ps.point.x=2.0;
  26.     ps.point.y=3.0;
  27.     ps.point.z=5.0;
  28.  
  29.     //添加休眠:以便坐标点被订阅方订阅（这个不加会报错！）
  30.     ros::Duration(2).sleep();
  31.  
  32.     //转换算法，需要调用TF内置实现
  33.     ros::Rate rate(10); //发布频率10Hz
  34.     while(ros::ok())
  35.     {
  36.         //将ps转换为base（pub中）的坐标点
  37.         geometry_msgs::PointStamped ps_out; //输出的坐标
  38.         /*buffer.transform()
  39.         第一个参数为被转化的坐标，
  40.         第二个参数为视为参考点的名称
  41.         返回值是转化后的坐标
  42.         */
  43.         ps_out=buffer.transform(ps,"world");
  44.         ROS_INFO("参考坐标系为:%s  转化后的坐标值:%.2f, %.2f, %.2f",
  45.                 ps_out.header.frame_id.c_str(),
  46.                 ps_out.point.x,
  47.                 ps_out.point.y,
  48.                 ps_out.point.z);
  49.  
  50.         rate.sleep();
  51.         ros::spinOnce();
  52.     }
  53.     return 0;
  54. }
  55. //调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
  56.

4.可能会遇到的问题

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

三、多坐标变换

1.概述

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

2.发布方

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

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

该情况下，我们发布的

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

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

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

3.订阅方

  1. #include "ros/ros.h"
  2. #include "tf2_ros/transform_listener.h"
  3. #include "tf2_ros/buffer.h"
  4. #include "geometry_msgs/PoseStamped.h"
  5. #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
  6. #include "geometry_msgs/TransformStamped.h"
  7.  
  8. /*
  9.     1.计算son1与son2的相对关系  2.计算son1的某个坐标点在son2中的坐标值
  12. */
  13. int main(int argc, char *argv[])
  14. {
  15.     /* code */
  16.     setlocale(LC_ALL,"");
  17.     ros::init(argc, argv, "turtles");
  18.     ros::NodeHandle nh;
  19.     //创建订阅对象
  20.     tf2_ros::Buffer buffer;
  21.     tf2_ros::TransformListener sub(buffer);
  22.     //编写解析逻辑
  23.     ros::Rate rate(10);
  24.         //创建相对于son1的坐标点
  25.     geometry_msgs::PointStamped psson1;
  26.     psson1.header.stamp =ros::Time::now();
  27.     psson1.header.frame_id="son1";
  28.     psson1.point.x=1.0;
  29.     psson1.point.y=2.0;
  30.     psson1.point.z=3.0;
  31.  
  32.     while(ros::ok())
  33.     {
  34.         try //可以避免因为发布订阅不同步造成的报错
  35.         {
  36.             //计算son1与son2的相对关系 
  37.                 /*buffer.lookupTransform()
  38.                     参数1：根据该坐标系进行转化（父级）  
  39.                     参数2：原坐标系（子级）  
  40.                     参数3：何时的相对坐标
  41.                     返回值：geometry_msgs::TransformStamped 子级相当于父级的相对坐标关系
  42.                 */
  43.             geometry_msgs::TransformStamped son1toson2 = buffer.lookupTransform("son2","son1",ros::Time(0));
  44.             ROS_INFO("son1 相对于 son2 的信息：父级%s  子级%s // 偏移量：%.2f, %.2f, %.2f",
  45.                     son1toson2.header.frame_id.c_str(), 
  46.                     son1toson2.child_frame_id.c_str(),
  47.                     son1toson2.transform.translation.x,
  48.                     son1toson2.transform.translation.y,
  49.                     son1toson2.transform.translation.z);
  50.             //计算son1中的某个坐标点在son2中的坐标值
  51.                 //创建相对于son2坐标点，然后将son1的转化为相对于son2的
  52.             geometry_msgs::PointStamped psson2 = buffer.transform(psson1,"son2");
  53.             ROS_INFO("son1坐标点在son2中的值: %.2f, %.2f, %.2f",
  54.                     psson2.point.x, psson2.point.y, psson2.point.z);
  55.  
  56.         }
  57.         catch(const std::exception& e)
  58.         {
  59.             ROS_INFO("错误提示:%s",e.what());
  60.         }
  61.         
  62.         
  63.         rate.sleep();
  64.         ros::spinOnce();
  65.     }
  66.     return 0;
  67. }

四、坐标系关系查看

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.生成一只新的乌龟：

  1. #include "ros/ros.h"
  2. #include "turtlesim/Spawn.h"
  3.  
  4. int main(int argc, char *argv[])
  5. {
  6.     /* code */
  7.     ros::init(argc,argv,"newturtle");
  8.     ros::NodeHandle nh;
  9.     ros::service::waitForService("/spawn"); //等待发现/spawn服务
  10.     ros::ServiceClient add_turtle = nh.serviceClient<turtlesim::Spawn>("/spawn");
  11.  
  12.     turtlesim::Spawn srv;
  13.     srv.request.x=2.0;
  14.     srv.request.y=2.0;
  15.     srv.request.name="turtle2";
  16.     //请求服务调用
  17.     ROS_INFO("Call service to spawn turtle[x:%0.6f, y:%0.6f, name:%s]",
  18.     srv.request.x, srv.request.y, srv.request.name);
  19.     
  20.     add_turtle.call(srv);
  21.  
  22.     //显示服务调用结果
  23.     ROS_INFO("Spawn turtle successfully [name:%s]",srv.response.name.c_str());
  24.  
  25.     return 0;
  26. }

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

  1. #include "ros/ros.h"
  2. #include "turtlesim/Pose.h"
  3. #include "tf2_ros/transform_broadcaster.h"  //用于发布动态坐标消息
  4. #include "geometry_msgs/TransformStamped.h"
  5. #include "tf2/LinearMath/Quaternion.h"  //欧拉角
  6.  
  7. /*
  8.     订阅乌龟的位姿信息，转换成相对窗体的坐标
  9.     准备
  10.         话题：/turtle1/pose
  11.         消息：/turtlesim/Pose
  12. */
  13.  
  14. //声明变量，接收传递的参数
  15. std::string turtle_name;
  16.  
  17. void PoseCallback(const turtlesim::Pose::ConstPtr& pose)
  18. {
  19.     //获取位姿信息，转化为坐标系相对关系并发布
  20.     //创建TF发布对象
  21.     static tf2_ros::TransformBroadcaster pub;   //加static是为了防止每次进入回调函数都要重新定义一次
  22.     //创建发布数据：从参数pose中进行转换
  23.     geometry_msgs::TransformStamped ts;
  24.  
  25.     ts.header.frame_id="world"; //参考的坐标系（名字是固定的）
  26.     ts.header.stamp=ros::Time::now();
  27.     //=====turtle1和turtle2是动态传入的====
  28.     ts.child_frame_id=turtle_name;    //相对的坐标系（名字是固定的）
  29.     //坐标系平移量的设置
  30.     ts.transform.translation.x = pose->x;   //这些值都可以通过rosmsg info turtlesim/Pose来查看的
  31.     ts.transform.translation.y = pose->y;
  32.     ts.transform.translation.z = 0;
  33.     //坐标系四元数
  34.     //乌龟是2D的，因此没有翻滚和俯仰角，因此乌龟的欧拉角为0，0，theta
  35.     tf2::Quaternion qtn;
  36.     qtn.setRPY(0,0,pose->theta);
  37.         //设置四元数
  38.     ts.transform.rotation.x=qtn.getX();
  39.     ts.transform.rotation.y=qtn.getY();
  40.     ts.transform.rotation.z=qtn.getZ();
  41.     ts.transform.rotation.w=qtn.getW();
  42.     //发布
  43.     pub.sendTransform(ts);
  44. }
  45.  
  46. int main(int argc, char *argv[])
  47. {
  48.     setlocale(LC_ALL,"");
  49.     ros::init(argc, argv, "dynamic_pub");
  50.     ros::NodeHandle nh;
  51.  
  52.     /* 
  53.         要想动态传入turtle1和turtle2，需要解析launch文件
  54.     */
  55.     if(argc !=2 )
  56.     {
  57.         ROS_ERROR("请传入一个参数");
  58.         return 1;
  59.     }
  60.     else
  61.     {
  62.         turtle_name = argv[1];
  63.     }
  64.     //创建订阅对象
  65.         //====turtle1和turtle2是动态传入的====
  66.         //参数一：话题名称  参数二：队列长度  参数三：回调函数
  67.         //注意：动态传入话题时，pose前要加/ ！！！
  68.     ros::Subscriber sub=nh.subscribe(turtle_name+"/pose",100,PoseCallback);
  69.     ros::spin();
  70.     return 0;
  71. }

4.控制乌龟跟随

  1. #include "ros/ros.h"
  2. #include "tf2_ros/transform_listener.h"
  3. #include "tf2_ros/buffer.h"
  4. #include "geometry_msgs/PoseStamped.h"
  5. #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
  6. #include "geometry_msgs/TransformStamped.h"
  7. #include "geometry_msgs/Twist.h"
  8.  
  9. /*
  10.     1.换算turtle1相对于turtle2的关系  2.计算角速度和线速度并发布
  13. */
  14. int main(int argc, char *argv[])
  15. {
  16.     /* code */
  17.     setlocale(LC_ALL,"");
  18.     ros::init(argc, argv, "turtles");
  19.     ros::NodeHandle nh;
  20.     //创建订阅对象
  21.     tf2_ros::Buffer buffer;
  22.     tf2_ros::TransformListener sub(buffer);
  23.  
  24.     //创建发布对象
  25.     ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);
  26.     //编写解析逻辑
  27.     ros::Rate rate(10);
  28.  
  29.     while(ros::ok())
  30.     {
  31.         try //可以避免因为发布订阅不同步造成的报错
  32.         {
  33.             //计算son1与son2的相对关系 
  34.                 /*buffer.lookupTransform()
  35.                     参数1：根据该坐标系进行转化（父级）  
  36.                     参数2：原坐标系（子级）  
  37.                     参数3：何时的相对坐标
  38.                     返回值：geometry_msgs::TransformStamped 子级相当于父级的相对坐标关系
  39.                 */
  40.             geometry_msgs::TransformStamped son1toson2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
  41.             ROS_INFO("turtle1 相对于 turtle2 的信息：父级%s  子级%s // 偏移量：%.2f, %.2f, %.2f",
  42.                     son1toson2.header.frame_id.c_str(), 
  43.                     son1toson2.child_frame_id.c_str(),
  44.                     son1toson2.transform.translation.x,
  45.                     son1toson2.transform.translation.y,
  46.                     son1toson2.transform.translation.z);
  47.  
  48.             //根据相对坐标计算并组织速度消息
  49.             geometry_msgs::Twist twist;
  50.             /*
  51.                 twist.linear.x = 系数*开二次方（x^2+y^2)
  52.                 twist.angular.z = 系数*反正切（对边/邻边）
  53.             */
  54.             twist.linear.x = 0.5 * sqrt(pow(son1toson2.transform.translation.x,2)+pow(son1toson2.transform.translation.y,2));
  55.             twist.angular.z = 4*atan2(son1toson2.transform.translation.y,son1toson2.transform.translation.x);
  56.             
  57.             //发布
  58.             pub.publish(twist);
  59.  
  60.         }
  61.         catch(const std::exception& e)
  62.         {
  63.             ROS_INFO("错误提示:%s",e.what());
  64.         }
  65.         
  66.         
  67.         rate.sleep();
  68.         ros::spinOnce();
  69.     }
  70.     return 0;
  71. }

声明：本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号