简介

静态坐标变换,是指两个坐标系之间的相对位置是固定的

需求描述

现有一机器人模型,核心构成包含主体与雷达,各对应一坐标系,坐标系的原点分别位于主体与雷达的物理中心,已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物,在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问,该障碍物相对于主体的坐标是多少?

基本流程

新建功能包,添加依赖

编写发布方实现

编写订阅方实现

执行并查看结果

发布方实现

--------------------------------------------------------------------------------------- 静态坐标变换发布方: 发布关于 laser 坐标系的位置信息 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建静态坐标转换广播器 4.创建坐标系信息 5.广播器发布坐标系信息 6.spin() ------------------------------------------------------------------------------------- // 1.包含头文件 #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[]) { setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"static_brocast"); // 3.创建静态坐标转换广播器 tf2_ros::StaticTransformBroadcaster broadcaster; // 4.创建坐标系信息 geometry_msgs::TransformStamped ts; //----设置头信息 ts.header.seq = 100; ts.header.stamp = ros::Time::now(); ts.header.frame_id = "base_link"; //----设置子级坐标系 ts.child_frame_id = "laser"; //----设置子级相对于父级的偏移量 ts.transform.translation.x = 0.2; ts.transform.translation.y = 0.0; ts.transform.translation.z = 0.5; //----设置四元数:将 欧拉角数据转换成四元数 tf2::Quaternion qtn; qtn.setRPY(0,0,0); 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.广播器发布坐标系信息 broadcaster.sendTransform(ts); ros::spin(); return 0; }

发布方基本结果

请输入图片描述

请输入图片描述
请输入图片描述
请输入图片描述

订阅方实现

------------------------------------------------------------------------------------ 订阅坐标系信息,生成一个相对于 子级坐标系的坐标点数据, 转换成父级坐标系中的坐标点 实现流程: 1.导包 2.初始化 ROS 节点 3.创建 TF 订阅对象 4.创建一个 radar 坐标系中的坐标点 5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标 6.spin ------------------------------------------------------------------------------------ //1.头文件 #include "ros/ros.h" #include "tf2_ros/transform_listener.h" //1.1buffer缓存头文件 #include "tf2_ros/buffer.h" //1.2信息转换头文件 #include "geometry_msgs/PointStamped.h" //1.3转换坐标头文件 #include "tf2_geometry_msgs/tf2_geometry_msgs.h" int main(int argc, char *argv[]) { //2.初始化 setlocale(LC_ALL,""); ros::init(argc,argv,"static_sub"); ros::NodeHandle nh; //3.创建订阅对象----》订阅坐标信息并且进行转换 //3.1创建存储空间buffer tf2_ros::Buffer buffer; //3.2创建监听对象 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.x=3.0; ps.point.x=5.0; //5.转换算法 //5.1循环转换坐标,实现动态转换 ros::Rate rate(10); while (ros::ok()) { //核心代码实现 geometry_msgs::PointStamped ps_out; 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(); } return 0; }

订阅方代码改进

因为订阅方在订阅消息的时候不是立刻就能够订阅到消息,会导致程序因为异常中断。 请输入图片描述

请输入图片描述

解决方法

  1. 对于程序进行一个延时操作
  2. 对于程序进行异常操作判断

方案1

在执行坐标转换之前,执行延时操作

添加休眠函数 ros::Duration (2).sleep();

请输入图片描述

请输入图片描述

方案2

执行异常检测操作

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() ); } catch(const std::exception& e) { ROS_INFO("异常消息:%s",e.what()); }

请输入图片描述

请输入图片描述

订阅方最终实现

//1.头文件 #include "ros/ros.h" #include "tf2_ros/transform_listener.h" //1.1buffer缓存头文件 #include "tf2_ros/buffer.h" //1.2信息转换头文件 #include "geometry_msgs/PointStamped.h" //1.3转换坐标头文件 #include "tf2_geometry_msgs/tf2_geometry_msgs.h" int main(int argc, char *argv[]) { //2.初始化 setlocale(LC_ALL,""); ros::init(argc,argv,"static_sub"); ros::NodeHandle nh; //3.创建订阅对象----》订阅坐标信息并且进行转换 //3.1创建存储空间buffer tf2_ros::Buffer buffer; //3.2创建监听对象 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.x=3.0; ps.point.x=5.0; //添加休眠函数 //ros::Duration (2).sleep(); //5.转换算法 //5.1循环转换坐标,实现动态转换 ros::Rate rate(10); while (ros::ok()) { //核心代码实现 geometry_msgs::PointStamped ps_out; /* 调用了buffer的转换函数transfer 参数1:被转化的坐标点 参数2:目标坐标系 参数3:输出的坐标点 ps:必须包含头文件"tf2_geometry_msgs/tf2_geometry_msgs.h" ps:运行时存在问题,抛出一个异常base_link不存在 原因:订阅数据是一个耗时操作,可能再调用transform转换函数时,坐标系的相对关系还没有订阅到,因此出现异常 解决: 方案1:调用函数之前,执行一段时间的休眠 方案2:进行异常处理 */ 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() ); } catch(const std::exception& e) { ROS_INFO("异常消息:%s",e.what()); } // 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(); } return 0; }

最终现象

方案1

请输入图片描述

请输入图片描述

方案2

请输入图片描述

请输入图片描述

最后修改:2023 年 11 月 10 日
如果觉得我的文章对你有用,请随意赞赏