ROS--静态坐标变换（待续）

梦炫星语

已于 2022-03-21 10:16:53 修改

阅读量887

点赞数

文章标签：自动驾驶人工智能 ubuntu

于 2022-03-18 22:35:58 首次发布

本文链接：https://blog.csdn.net/ping96/article/details/123584871

版权

需求：

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

方案A:C++实现

1.1 发布方实现

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
    需求：发布2个坐标系的相对关系
    流程：
        1.包含头文件
        2.设置编码、节点初始化、NodeHandle
        3.创建发布对象
        4.组织被发布的消息
        5.发布数据
        6.spin()
*/
int main(int argc, char *argv[])
{
    // 2.设置编码、节点初始化、NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_pub");
    ros::NodeHandle nh;
    // 3.创建发布对象
    tf2_ros::StaticTransformBroadcaster pub;
    // 4.组织被发布的消息
    geometry_msgs::TransformStamped tfs;
    tfs.header.stamp = ros::Time::now();
    tfs.header.frame_id = "base_link";  //相对坐标系关系中被参考的那一个(小车)
    tfs.child_frame_id = "laser";       //雷达的
    tfs.transform.translation.x = 0.2;  //看需求
    tfs.transform.translation.y = 0.0;
    tfs.transform.translation.z = 0.5;
    //需要根据欧拉角转换
    tf2::Quaternion qtn;   //创建四元数 对象
    //向该对象设置欧拉角,这个对象可以将欧拉角转换成四元数
    qtn.setRPY(0,0,0);     //欧拉角的单位是弧度
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    // 5.发布数据
    pub.sendTransform(tfs);
    // 6.spin()
    ros::spin();
    return 0;
}

在这里插入图片描述输入：

rviz   //调出 RViZ 界面

设置：
①Fixed Frame
②添加 TF 组件

在这里插入图片描述

1.2 订阅方实现

注：在运行时如果报错：
在这里插入图片描述
解决：
在转换算法前，添加休眠（需要订阅到消息后再转化）

ros::Duration(2).sleep();

代码：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

/*
    订阅方：订阅发布的坐标系相对关系，传入一个坐标点，调用 tf 实现转换
    流程：
        1.包含头文件
        2.设置编码、节点初始化、NodeHandle(必须的)
        3.创建一个订阅对象  --->订阅坐标系相对关系
        4.组织一个坐标点数据
        5.转换算法，调用 tf 内置实现
        6.最后输出
*/

int main(int argc, char *argv[])
{
    // 2.设置编码、节点初始化、NodeHandle(必须的)
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    // 3.创建一个订阅对象  --->订阅坐标系相对关系 (3-1 与 3-2 是结合使用的)
    // 3-1. 创建一个 buffer 缓存
    tf2_ros::Buffer buffer;
    // 3-2. 再创建一个监听对象(监听对象可以将订阅的数据存入 buffer )
    tf2_ros::TransformListener listen(buffer);
    // 4.组织一个坐标点数据
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    //添加休眠  （文章“注”的部分添加休眠）
    ros::Duration(2).sleep();
    // 5.转换算法，调用 tf 内置实现
    ros::Rate rate(10);
    while (ros::ok())
    {
        // 核心代码 ---- 将 ps 转换为 base_link 的坐标点
        geometry_msgs::PointStamped ps_out;     
        /*
            调用了 buffer 的转换函数 transform 
            参数1：被转换的坐标点数据；
            参数2：目标坐标系；
            返回值：转换后的坐标点信息（输出的坐标系）

            注意：
            1.调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
            2.运行时存在的问题：抛出一个异常 base_link 不存在
                原因：订阅数据是一个耗时操作，可能在调用 transform 转换函数时，坐标系的相对关系还没有被订阅到，故出现异常
                解决：
                    方案1：在调用转换函数前执行休眠（简单）
                    方案2：进行异常处理
        */   
        ps_out = buffer.transform(ps,"base_link");  
        // 6.最后输出
        ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                    ps_out.point.x,
                    ps_out.point.y,
                    ps_out.point.z,
                    ps_out.header.frame_id.c_str());
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

方案1：在调用转换函数前执行休眠
先运行发布方，再运行订阅方：
在这里插入图片描述
方案二：try处理
修改最后 ps_out 和 ROS_INFO 部分

       try
       {
            ps_out = buffer.transform(ps,"base_link");  
            // 6.最后输出
            ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                        ps_out.point.x,
                        ps_out.point.y,
                        ps_out.point.z,
                        ps_out.header.frame_id.c_str());
       }
       catch(const std::exception& e)
       {
        //    std::cerr << e.what() << '\n';
           ROS_INFO("异常消息:%s",e.what());
       }

在这里插入图片描述

方案B:Python实现

2.1 发布方实现

#! /usr/bin/env python

import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
"""
    发布方:发布2个坐标系的相对关系(小车的底盘 ---> base_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()

输出：
在这里插入图片描述

2.2 订阅方实现

#! /usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs

"""

    订阅方:订阅坐标变换消息,传入被转换的坐标点,调用转换算法
    流程:
        1.导包
        2.初始化节点
        3.创建订阅对象
        4.组织被转换的坐标点
        5.转换逻辑实现,调用tf封装的算法
        6.输出结果
"""

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:
            # 转换实现
            """"
                参数1:被转换的坐标点
                参数2:目标坐标系
                返回值:转换后的坐标点

                注:
                问题:抛出异常 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)
        rate.sleep()

备注：Pykdl模块出错（版本不匹配，未能跑出结果）

方案C：命令行

当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、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

即：
在这里插入图片描述

梦炫星语

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫