题目

现有一移动机器人，该机器人的基坐标系为“base_link”，机器人包含3个子坐标系分别为“joint1”，“joint2”，“joint3”。

要求：利用多坐标转换，实现joint1下的坐标向joint2下的坐标转换，joint2下的坐标向joint3下的坐标转换。静态坐标的位置发布和最终的程序启动采用launch文件形式，基于已有功能包进行发布，编写多坐标变换的订阅方实现，打印输出两个转换后的坐标信息

已知坐标关系：

​ joint1相对于base_link的坐标为：x = 0.2 y = 0.0 z = 0.3

​ joint2相对于base_link的坐标为：x = 0.5 y = 0.1 z = 0.4

​ joint3相对于base_link的坐标为：x = 0.3 y = 0.3 z = 0.0

已知坐标点：

joint1坐标系下的坐标为：（1.0，1.0，1.0）

joint2坐标系下的坐标为：（2.0，2.0，2.0）

方案

一.订阅方（text_tf_sub.py）

• 在my_tf功能包下的scripts文件夹下创建python文件text_tf_sub.py

• 撰写代码

#! /usr/bin/env python"""
订阅方
"""import rospy
import tf2_ros
from tf2_geometry_msgs import PointStampeddef one_two():# tfs = sub.lookup_transform("joint2","joint1",rospy.Time(0))# 创建消息对象source_point = PointStamped()  # 使用正确的消息类型    source_point.header.stamp = rospy.Time.now()source_point.header.frame_id = "joint1"    source_point.point.x = 1.0     # x偏移量source_point.point.y = 1.0 source_point.point.z = 1.0target_point_1 = sub.transform(source_point, "joint2",rospy.Duration(1))  return target_point_1def two_three():# tfs = sub.lookup_transform("joint2","joint1",rospy.Time(0))# 创建消息对象source_point = PointStamped()  # 使用正确的消息类型    source_point.header.stamp = rospy.Time.now()source_point.header.frame_id = "joint2"source_point.point.x = 2.0source_point.point.y = 2.0source_point.point.z = 2.0target_point_2 = sub.transform(source_point, "joint3",rospy.Duration(1))  return target_point_2if __name__ == '__main__':rospy.init_node("text_tf_sub_p")  # 初始化节点# 创建缓冲区对象sub = tf2_ros.Buffer()# 创建TF订阅对象listener = tf2_ros.TransformListener(sub)# 设置发布频率rate = rospy.Rate(1)while not rospy.is_shutdown():try:target_point_1 = one_two()target_point_2 = two_three()rospy.loginfo("joint1向joint2的坐标转换: x = %.2f, y = %.2f, z = %.2f ",target_point_1.point.x,target_point_1.point.y,target_point_1.point.z)rospy.loginfo("joint2向joint3的坐标转换: x = %.2f, y = %.2f, z = %.2f \n",target_point_2.point.x,target_point_2.point.y,target_point_2.point.z)except Exception as e:rospy.logwarn("不可用")rate.sleep()rospy.spin()

• 在CMakeLists.txt中添加scripts/text_tf_sub.py

• 从终端进入工作空间下的src下的功能包的scripts ~/ros_ws/src/my_tf/scripts为python文件添加可执行权限

  chmod +x *.py

二.发布方（text_tf_pub.launch）

• 在my_tf下的launch文件夹下创建 text_tf_pub.launch

• 撰写launch文件

<?xml version="1.0"?>
<launch><node name="joint1" pkg="tf2_ros" type="static_transform_publisher" args="0.2 0.0 0.3 0 0 0 /base_link /joint1" output="screen"/><node name="joint2" pkg="tf2_ros" type="static_transform_publisher" args="0.5 0.1 0.4 0 0 0 /base_link /joint2" output="screen"/><node name="joint3" pkg="tf2_ros" type="static_transform_publisher" args="0.3 0.3 0 0 0 0 /base_link /joint3" output="screen"/><node name="text_tf_sub" pkg="my_tf" type="text_tf_sub.py" output="screen"/></launch>

三.编译运行

• 从终端进入工作空间ros_ws

  cd ros_ws

• 编译工作空间 catkin_make

• 刷新工作空间 source ./devel/setup.bash

• 运行launch文件 roslaunch my_tf text_tf_pub.launch