ROS TF静态坐标变换实现
法一:编码实现
①发布方代码实现:创建功能包并添加依赖
catkin_create_pkg tf_static roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs
②新建python文件,为文件添加可执行权限,修改CMakelists文件
③编写发布方代码,编译执行
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf.transformations
"""
发布方实现:用于发布两个坐标系的新相对关系(小车底盘——baselink和雷达——laser)
流程:1、导包
2、初始化ROS节点
3、创建发布对象
4、组织被发布的数据
5、循环发布数据
6、spin()
"""
if __name__=="__main__":
rospy.init_node("tf_static_pub")
pub = tf2_ros.StaticTransformBroadcaster()
ts = TransformStamped()
ts.header.stamp=rospy.Time.now()
ts.header.frame_id = "baselink"
ts.child_frame_id = "laser"
ts.transform.translation.x = 0.2
ts.transform.translation.y = 0.0
ts.transform.translation.z = 0.5
qtn = tf.transformations.quaternion_from_euler(0,0,0)
ts.transform.rotation.x =qtn[0]
ts.transform.rotation.y = qtn[1]
ts.transform.rotation.z = qtn[2]
ts.transform.rotation.w = qtn[3]
pub.sendTransform(ts)
rospy.spin()
④查看发布方发布的数据
rostopic list,可以看到有话题名为tf_static的话题
rostopic echo tf_static可以看到发布的具体信息
也可以借助可视化工具rviz直观看到两个坐标系:终端中输入rviz打开可视化窗口,左上角Fixed Frame选择baselink,左下角add,添加TF即可看到)
④订阅方代码编写
新建python文件tf_static_sub.py,为文件添加可执行权限,修改CMakelists文件
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
"""
订阅方实现:
订阅坐标变换消息,传入被转换的坐标点,调用转换算法实现坐标转换
1、导包(用到什么包导入什么包)
2、初始化ros节点
3、创建订阅对象
4、组织被转换的坐标点
5、转换逻辑编写(调用tf封装算法)
6、输出转换结果B
7、spin()/spinonce()
"""
if __name__=="__main__":
rospy.init_node("static_tf_pub")
buffer = tf2_ros.Buffer()
sub = tf2_ros.TransformListener(buffer)
ps = tf2_geometry_msgs.PointStamped()
ps.header.stamp = rospy.Time.now()
ps.header.frame_id = "laser"
ps.point.x = 2.0
ps.point.y = 3.0
ps.point.z = 5.0
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
ps_out = buffer.transform(ps,"baselink")
rospy.loginfo("转换后的坐标结果:(%.2f,%.2f,%.2f)当前参考的坐标系:%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id)
except:
rospy.logwarn("错误提示:%s",Exception)
rate.sleep()
法二:ros封装命令实现静态坐标转换
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 x轴偏转角 y轴偏转角 z轴偏转角 父坐标系名 子坐标系名
可以实现像编码一样的效果,同样可以通过rostopic查看话题名和消息格式具体信息。也可以通过rviz可视化看到两坐标系的相对位置
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)