ROS入门5.1.3动态坐标变换《ROS入门-理论与实践》视频教程镇楼动态坐标变换是指两个坐标系之间相对位置的变化。
需求描述：
启动turtlesim_node。 该节点的形状包含世界坐标系。 左下方是坐标系的原点。 乌龟是另一个坐标系，键盘控制乌龟的运动，动态发布两个坐标系的相对位置。
结果演示：
实现分析：
乌龟本身不仅可以看作坐标系，也是世界坐标系中的一个坐标点订阅turtle1/pose，它获取乌龟在世界坐标系中的x坐标、y坐标、偏移量、线速度和角速度，将pose信息转化为坐标系相对信息，http://www.Sina .
制作新的功能包，追加依赖于坐标相对关系的制作的发布源(同时需要订阅乌龟的姿势信息) )，坐标相对关系的制作委托源执行计划A:C的安装
1 .创建功能包创建项目功能包依赖于tf2、tf2_ros、tf2_geometry_msgs、roscpprospystd _ msgs geometry _ msgs、turtlesim
2 .发行方/*动态坐标系相对姿态发行(一个坐标系与另一个坐标系的相对姿态不断变化)启动需求：Turtlesim_node。 该节点的形状有世界坐标系(左下为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟的运动。 动态发布两坐标系的相对位置实现分析： 1。 乌龟本身不仅可以看作坐标系，还订阅了世界坐标系中的一个坐标点turtle1/pose，将可以取得乌龟在世界坐标系中的x坐标、y坐标、偏移量、线速度和角速度的pose信息变换为坐标系相对信息， 提交实现流程： 1.包括头文件2 .初始化ROS节点3 .创建ROS句柄4 .创建订阅对象5 .回调函数处理订阅的数据(实现TF广播) )。 5-1.TF创建广播5-2 .创建广播数据(在pose中设置)5-3.广播数据6.spin*///1 .标题文件# include ' ROS/ROS.h ' # include ' # inclulue include ' geometry _ msgs/transform stamped.h ' # include ' tf2/linear math/quaternion.h ' voiddopose (consturtllesion ) 5-1.TF创建广播器static tf2 _ ROS 3360333333332//5-2.创建广播数据(在pose中设置) geometry _ msgs 33603: transformstampamppppating //|----耳机tfs.header.frame_id='world '； TFS.header.stamp=ROS :3360 time :3360 now (； //|----坐标系ID tfs.child_frame_id='turtle1'； //|----坐标系相对信息设置TFS.transform.translation.x=pose-x； TFS.transform.translation.y=pose-y； TFS.transform.translation.z=0.0； //二维实现，pose中没有z，z为0 //|---------四元数设置tf2:Quaternion qtn； qtn.se trpy (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 (； //5-3 .广播站发布数据BroadCaster.sendtransform(TFS )； }intmain(intargc，char *argv[] ) setlocale ) LC_all，' '； //2 .初始化ROS节点ROS:3360init(argc，argv，' dynamic_tf_pub ' )； //3.ROS句柄创建ros:NodeHandle nh； //4 .创建订阅对象ROS :3360 subscriber sub=NH.subscribeturt
lesim::Pose>("/turtle1/pose",1000,doPose); // 5.回调函数处理订阅到的数据(实现TF广播) // // 6.spin ros::spin(); return 0;}配置文件此处略。
3.订阅方 //1.包含头文件#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 必须包含该头文件int main(int argc, char *argv[]){ setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"dynamic_tf_sub"); ros::NodeHandle nh; // 3.创建 TF 订阅节点 tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer); ros::Rate r(1); while (ros::ok()) { // 4.生成一个坐标点(相对于子级坐标系) geometry_msgs::PointStamped point_laser; point_laser.header.frame_id = "turtle1"; point_laser.header.stamp = ros::Time(); point_laser.point.x = 1; point_laser.point.y = 1; point_laser.point.z = 0; // 5.转换坐标点(相对于父级坐标系) //新建一个坐标点，用于接收转换结果 //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------ try { geometry_msgs::PointStamped point_base; point_base = buffer.transform(point_laser,"world"); ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z); } catch(const std::exception& e) { // std::cerr << e.what() << 'n'; ROS_INFO("程序异常:%s",e.what()); } r.sleep(); ros::spinOnce(); } return 0;}配置文件此处略。
4.执行可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。
可以使用 rviz 查看坐标系相对关系。
方案B:Python实现
1.创建功能包创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
2.发布方 #! /usr/碧蓝的外套/env python""" 动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的) 需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘 控制乌龟运动，将两个坐标系的相对位置动态发布 实现分析: 1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点 2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度 3.将 pose 信息转换成 坐标系相对信息并发布 实现流程: 1.导包 2.初始化 ROS 节点 3.订阅 /turtle1/pose 话题消息 4.回调函数处理 4-1.创建 TF 广播器 4-2.创建 广播的数据(通过 pose 设置) 4-3.广播器发布数据 5.spin"""# 1.导包import rospyimport tf2_rosimport tffrom turtlesim.msg import Posefrom geometry_msgs.msg import TransformStamped# 4.回调函数处理def doPose(pose): # 4-1.创建 TF 广播器 broadcaster = tf2_ros.TransformBroadcaster() # 4-2.创建 广播的数据(通过 pose 设置) tfs = TransformStamped() tfs.header.frame_id = "world" tfs.header.stamp = rospy.Time.now() tfs.child_frame_id = "turtle1" tfs.transform.translation.x = pose.x tfs.transform.translation.y = pose.y tfs.transform.translation.z = 0.0 qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta) tfs.transform.rotation.x = qtn[0] tfs.transform.rotation.y = qtn[1] tfs.transform.rotation.z = qtn[2] tfs.transform.rotation.w = qtn[3] # 4-3.广播器发布数据 broadcaster.sendTransform(tfs)if __name__ == "__main__": # 2.初始化 ROS 节点 rospy.init_node("dynamic_tf_pub_p") # 3.订阅 /turtle1/pose 话题消息 sub = rospy.Subscriber("/turtle1/pose",Pose,doPose) # 4.回调函数处理 # 4-1.创建 TF 广播器 # 4-2.创建 广播的数据(通过 pose 设置) # 4-3.广播器发布数据 # 5.spin rospy.spin()权限设置以及配置文件此处略。
3.订阅方 #! /usr/碧蓝的外套/env python""" 动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的) 需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘 控制乌龟运动，将两个坐标系的相对位置动态发布 实现分析: 1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点 2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度 3.将 pose 信息转换成 坐标系相对信息并发布 实现流程: 1.导包 2.初始化 ROS 节点 3.创建 TF 订阅对象 4.处理订阅的数据"""# 1.导包import rospyimport tf2_ros# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型from tf2_geometry_msgs import PointStamped# from geometry_msgs.msg import PointStampedif __name__ == "__main__": # 2.初始化 ROS 节点 rospy.init_node("static_sub_tf_p") # 3.创建 TF 订阅对象 buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(buffer) rate = rospy.Rate(1) while not rospy.is_shutdown(): # 4.创建一个 radar 坐标系中的坐标点 point_source = PointStamped() point_source.header.frame_id = "turtle1" point_source.header.stamp = rospy.Time.now() point_source.point.x = 10 point_source.point.y = 2 point_source.point.z = 3 try: # 5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标 point_target = buffer.transform(point_source,"world",rospy.Duration(1)) rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f", point_target.point.x, point_target.point.y, point_target.point.z) except Exception as e: rospy.logerr("异常:%s",e) # 6.spin rate.sleep()权限设置以及配置文件此处略。
4.执行可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。
可以使用 rviz 查看坐标系相对关系。
另请参考:
[http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20broadcaster%20%28C%2B%2B%29](http://wiki.ros.org/tf2/Tutorials/Writing a tf2 broadcaster (C%2B%2B)).spin
rate.sleep()
权限设置以及配置文件此处略。
4.执行可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。
可以使用 rviz 查看坐标系相对关系。
另请参考:
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20broadcaster%20%28C%2B%2B%29http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20broadcaster%20%28Python%29