ROS入门5.1.2静态坐标转换《ROS入门-理论与实践》视频教程镇楼静态坐标转换是指两个坐标系之间的相对位置固定。
需求描述：
在传统机器人模型中，核心结构包括机身和雷达，分别对应坐标系，坐标系原点分别位于机身和雷达的物理中心，已知雷达原点相对于机身原点的位移关系为： x 0.2 y0.0 z0.5，如下所示。 目前，雷达已检测到障碍物。 雷达坐标系中障碍物的坐标为(2.0 )~(3.0 )~(5.0 )，该障碍物相对于主体的坐标是多少？
结果演示：
实现分析：
坐标系的相对关系是发行者发行注册者，注册发行的坐标系的相对关系，传递坐标点信息，用tf实现坐标变换，将结果输出到实现流程:C，可以与Python一致
创建新的功能包，添加并运行依赖于创建发布器实现的创建读者实现，以验证结果方案A:C的实现
1 .创建功能包创建项目功能包取决于tf2、tf2_ros、tf2_geometry_msgs、roscpprospystd _ msgs geometry _ msgs
2 .发行方/*静态坐标变换发行方：发行关于laser坐标系位置信息的安装流程： 1.包含标题文件的2.ROS节点的初始化3 .静态坐标变换广播器的制作4 .坐标系信息的制作5 .广播器为坐标系信息6.spin 包括的ROS.h ' # include ' tf2 _ ROS/static _ transform _ broadcaster.h ' # include ' geometry _ msgs/transform stamped //3 .静态坐标转换广播电台tf2 _ ROS :3360 statictransformbroadcasterbroadcaster； //4 .创建坐标系信息geometry _ msgs 33603360 transformstampedts； //----设置报头信息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.se trpy (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 (； 返回0； }此处省略配置文件。
3 .注册注册方/*坐标系信息，生成相对于子坐标系的坐标点数据，向父坐标系的坐标点变换实现流程： 1.包含头文件的2 .初始化ROS节点3 .创建TF订阅节点4 .生成相对于子坐标系的坐标点5 .相对于父坐标系的坐标点数据1.include ' tf2 _ ROS/transform _ listener.h ' # include ' tf2 _ ROS/buffer.h ' '包含头文件注意：调用的transform必须包含头文件intmain(intargc，char *argv[] () (setlocale ) LC_all，'') )。 //2.ROS节点ROS :3360 init (初始化argv、argv，
"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 = "laser"; point_laser.header.stamp = ros::Time::now(); point_laser.point.x = 1; point_laser.point.y = 2; point_laser.point.z = 7.3; // 5.转换坐标点(相对于父级坐标系) //新建一个坐标点，用于接收转换结果 //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------ try { geometry_msgs::PointStamped point_base; point_base = buffer.transform(point_laser,"base_link"); ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str()); } catch(const std::exception& e) { // std::cerr << e.what() << 'n'; ROS_INFO("程序异常....."); } r.sleep(); ros::spinOnce(); } return 0;}配置文件此处略。
4.执行可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，控制台将输出，坐标转换后的结果。
方案B:Python实现
1.创建功能包创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
2.发布方 #! /usr/sxdfn/env python""" 静态坐标变换发布方: 发布关于 laser 坐标系的位置信息 实现流程: 1.导包 2.初始化 ROS 节点 3.创建 静态坐标广播器 4.创建并组织被广播的消息 5.广播器发送消息 6.spin"""# 1.导包import rospyimport tf2_rosimport tffrom geometry_msgs.msg import TransformStampedif __name__ == "__main__": # 2.初始化 ROS 节点 rospy.init_node("static_tf_pub_p") # 3.创建 静态坐标广播器 broadcaster = tf2_ros.StaticTransformBroadcaster() # 4.创建并组织被广播的消息 tfs = TransformStamped() # --- 头信息 tfs.header.frame_id = "world" tfs.header.stamp = rospy.Time.now() tfs.header.seq = 101 # --- 子坐标系 tfs.child_frame_id = "radar" # --- 坐标系相对信息 # ------ 偏移量 tfs.transform.translation.x = 0.2 tfs.transform.translation.y = 0.0 tfs.transform.translation.z = 0.5 # ------ 四元数 qtn = tf.transformations.quaternion_from_euler(0,0,0) tfs.transform.rotation.x = qtn[0] tfs.transform.rotation.y = qtn[1] tfs.transform.rotation.z = qtn[2] tfs.transform.rotation.w = qtn[3] # 5.广播器发送消息 broadcaster.sendTransform(tfs) # 6.spin rospy.spin()权限设置以及配置文件此处略。
3.订阅方 #! /usr/sxdfn/env python""" 订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据， 转换成父级坐标系中的坐标点 实现流程: 1.导包 2.初始化 ROS 节点 3.创建 TF 订阅对象 4.创建一个 radar 坐标系中的坐标点 5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标 6.spin"""# 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 = "radar" 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.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()权限设置以及配置文件此处略。
PS: 在 tf2 的 python 实现中，tf2 已经封装了一些消息类型，不可以使用 geometry_msgs.msg 中的类型
4.执行可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，控制台将输出，坐标转换后的结果。
补充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 组件，即可显示坐标关系。另请参考:
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28C%2B%2B%29
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28Python%29
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28C%2B%2B%29
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28Python%29