asp c 网站开发,一个网页,如何注册一个免费网站,江苏兴力建设集团有限公司网站题目
现有一移动机器人#xff0c;该机器人的基坐标系为“base_link”#xff0c;机器人包含3个子坐标系分别为“joint1”#xff0c;“joint2”#xff0c;“joint3”。
要求#xff1a;利用多坐标转换#xff0c;实现joint1下的坐标向joint2下的坐标转换#xff0c;…题目
现有一移动机器人该机器人的基坐标系为“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.01.01.0
joint2坐标系下的坐标为2.02.02.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 joint2source_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 version1.0?
launchnode namejoint1 pkgtf2_ros typestatic_transform_publisher args0.2 0.0 0.3 0 0 0 /base_link /joint1 outputscreen/node namejoint2 pkgtf2_ros typestatic_transform_publisher args0.5 0.1 0.4 0 0 0 /base_link /joint2 outputscreen/node namejoint3 pkgtf2_ros typestatic_transform_publisher args0.3 0.3 0 0 0 0 /base_link /joint3 outputscreen/node nametext_tf_sub pkgmy_tf typetext_tf_sub.py outputscreen//launch三.编译运行 从终端进入工作空间ros_ws cd ros_ws 编译工作空间 catkin_make 刷新工作空间 source ./devel/setup.bash 运行launch文件 roslaunch my_tf text_tf_pub.launch
