我的ROS学习之路——多坐标变换
2021/11/26 23:13:52
本文主要是介绍我的ROS学习之路——多坐标变换,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!
原文请点击。
需求描述:
现有坐标系统,父级坐标系统 world,下有两子级系统 son1,son2,son1 相对于 world,以及 son2 相对于 world 的关系是已知的,求 son1原点在 son2中的坐标,又已知在 son1中一点的坐标,要求求出该点在 son2 中的坐标
实现分析:
- 首先,需要发布 son1 相对于 world,以及 son2 相对于 world 的坐标消息
- 然后,需要订阅坐标发布消息,并取出订阅的消息,借助于 tf2 实现 son1 和 son2 的转换
- 最后,还要实现坐标点的转换
实现流程:C++ 与 Python 实现流程一致
-
新建功能包,添加依赖
-
创建坐标相对关系发布方(需要发布两个坐标相对关系)
-
创建坐标相对关系订阅方
-
执行
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
2.发布方
为了方便,使用静态坐标变换发布(直接使用内置的命令发布两个静态坐标系,就不用代码实现了,在前面讲的静态坐标变换中也讲了如何发布)
<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="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" /> </launch>
3.订阅方
#include <ros/ros.h> #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "geometry_msgs/PointStamped.h" #include "tf2/LinearMath/Transform.h" #include "tf2/LinearMath/Quaternion.h" int main(int argc, char *argv[]) { setlocale(LC_ALL,""); ros::init(argc,argv,"transform_point"); ros::NodeHandle nh; tf2_ros::Buffer buffer; tf2_ros::TransformListener sub(buffer); //设置被转换的坐标点 geometry_msgs::PointStamped ps; ps.header.frame_id = "son1"; ps.header.stamp = ros::Time(0); ps.point.x = 2;//数据写的规整一点,方便观察灯会结果对不对 ps.point.y = 0; ps.point.z = 0; ros::Rate rate(1); while(ros::ok()) { try { geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));//使用重载函数一,有三个参数:目标坐标系A(被参考的那个),源坐标系B,时间。前两个参数的意思是:B相对于A 变化了多少, //就可以从lookupTransform这个函数计算出来。就类似于 广播 transform,自己设置的 偏移量和欧拉角一样。这里只是反求了而已。 ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str()); ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str()); ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f", tfs.transform.translation.x, tfs.transform.translation.y, tfs.transform.translation.z ); geometry_msgs::PointStamped ps_out = buffer.transform(ps,"son2");//这个目标坐标系,可以填 world 也可以填son2都可以转换成功。 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("程序异常...",e.what()); } rate.sleep(); } return 0; }
配置文件此处略。
4.执行
(可以使用命令行或launch文件的方式分别启动发布节点与订阅节点,如果程序无异常,将输出换算后的结果。)
我这里是 先启动launch文件,再启动dynamic_more_tf_sub节点。
结果如图:
结束~
这篇关于我的ROS学习之路——多坐标变换的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!
- 2024-05-31全网首发第二弹!软考2024年5月《软件设计师》真题+解析+答案!(11-20题)
- 2024-05-31全网首发!软考2024年5月《软件设计师》真题+解析+答案!(21-30题)
- 2024-05-30【Java】百万数据excel导出功能如何实现
- 2024-05-30我们小公司,哪像华为一样,用得上IPD(集成产品开发)?
- 2024-05-30java excel上传--poi
- 2024-05-30安装笔记本应用商店的pycharm,再安排pandas等模块,说是没有打包工具?
- 2024-05-29java11新特性
- 2024-05-29哪些无用敏捷指标正在破坏敏捷转型?
- 2024-05-29鸿蒙原生应用再新丁!新华社 入局鸿蒙
- 2024-05-29设计模式 之 迭代器模式(Iterator)