ROS python实现静态坐标变换

静态坐标变换

所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。

需求描述:

现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

实现分析:

1. 坐标系相对关系，可以通过发布方发布
2. 订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2.发布方

#! /usr/bin/env python
import rospy
import tf.transformations
import tf2_ros 
import tf
from geometry_msgs.msg import TransformStamped
"""
    发布方：发布两个坐标系的相对关系（车辆底盘------basic_link和雷达------laser)
    流程：
        1、导包
        2、初始化节点
        3、创建发布对象
        4、组织被发布的数据
        5、发布数据
        6、spin()
"""
if __name__=="__main__":
    #  2、初始化节点
    rospy.init_node("static_pub_p")
    #  3、创建发布对象
    pub=tf2_ros.StaticTransformBroadcaster()
    #  4、组织被发布的数据
    ts=TransformStamped()
    #header
    ts.header.stamp=rospy.Time.now()
    ts.header.frame_id="base_link"
    #child frame
    ts.child_frame_id="laser"
    #相对关系（偏移与四元数）
    ts.transform.translation.x=0.2
    ts.transform.translation.y=0.0
    ts.transform.translation.z=0.5
    #四元数 4.1先从欧拉角转换成四元数
    qtn=tf.transformations.quaternion_from_euler(0,0,0)
    #4.2 再设置四元树
    ts.transform.rotation.x=qtn[0]
    ts.transform.rotation.y=qtn[1]
    ts.transform.rotation.z=qtn[2]
    ts.transform.rotation.w=qtn[3]
    #  5、发布数据
    pub.sendTransform(ts)
    #  6、spin()
    rospy.spin()

修改CMakeList.txt文件

修改scripts权限

若编译时显示 ：Could not find a package configuration file provided by “tf2_geometry_msgs“

原因：

安装ros时该功能包没有被下载下来

解决办法：

手动安装该功能包

sudo apt install ros-xxxxx-tf2-geometry-msgs

其中xxx为对应ROS的版本号

运行结果如下图所示

3.订阅方

#! /usr/bin/env python
import rospy
import tf2_ros
import tf2_geometry_msgs
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped
"""
    订阅方实现：订阅坐标变换消息，传入被转换的坐标点，调用转换算法

    流程：
        1、导包
        2、初始化
        3、创建订阅对象
        4、组织被转换的坐标点
        5、转换逻辑实现，调用tf封装的算法
        6、输出结果
        7、spain()  |spinOnce()
"""

if __name__=="__main__":
    # 2、初始化
    rospy.init_node("static_sub_p")
    # 3、创建订阅对象
    #3-1 创建缓存对象
    buffer=tf2_ros.Buffer()
    #3-2 创建订阅对象（将缓存传入）
    sub=tf2_ros.TransformListener(buffer)
    # 4、组织被转换的坐标点
    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
    # 5、转换逻辑实现，调用tf封装的算法
    rate=rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            #转换实现
            """
                参数一：被转换的坐标点
                参数二：目标坐标系
                返回值：转换后的坐标点

                ps
                问题：抛出异常 base_link 不存在
                原因：转换函数调用时，可能还没有订阅到坐标系的相对信息
                解决：try捕获异常并处理
            """
            ps_out=buffer.transform(ps,"base_link")
            # 6、输出结果
            rospy.loginfo("转换后的坐标：（%.2f,%.2f,%.2f）,参考的坐标系：%s",
                        ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id)
        except Exception as e:
            rospy.logwarn("错误提示：%s",e)
    # 7、spain()  |spinOnce()
    rate.sleep()

 修改CMakeList.txt文件

修改scripts权限

编译运行

参考链接：

[1]183静态坐标变换_需求描述与结果结果演示_Chapter5-ROS常用组件_哔哩哔哩_bilibili