TF静态坐标转换

TF静态坐标转换

清歌

2022 年 12 月 30 日

437 次浏览

暂无评论

7393字数

ROS-Fundamentals

简介

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

需求描述

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

基本流程

新建功能包，添加依赖

编写发布方实现

编写订阅方实现

执行并查看结果

发布方实现

---------------------------------------------------------------------------------------
    静态坐标变换发布方:
        发布关于 laser 坐标系的位置信息 
    
    实现流程:
         1.包含头文件
         2.初始化 ROS 节点
         3.创建静态坐标转换广播器
         4.创建坐标系信息
         5.广播器发布坐标系信息
         6.spin()
-------------------------------------------------------------------------------------
// 1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"static_brocast");
    // 3.创建静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    // 4.创建坐标系信息
    geometry_msgs::TransformStamped ts;
    //----设置头信息
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";
    //----设置子级坐标系
    ts.child_frame_id = "laser";
    //----设置子级相对于父级的偏移量
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;
    //----设置四元数:将 欧拉角数据转换成四元数
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 5.广播器发布坐标系信息
    broadcaster.sendTransform(ts);
    ros::spin();
    return 0;
}

发布方基本结果

请输入图片描述

订阅方实现

------------------------------------------------------------------------------------
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，
    转换成父级坐标系中的坐标点

    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建 TF 订阅对象
        4.创建一个 radar 坐标系中的坐标点
        5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
        6.spin
------------------------------------------------------------------------------------

//1.头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
    //1.1buffer缓存头文件
#include "tf2_ros/buffer.h"
    //1.2信息转换头文件
#include "geometry_msgs/PointStamped.h"
    //1.3转换坐标头文件
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
  

  int main(int argc, char  *argv[])
  {
    //2.初始化
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    //3.创建订阅对象----》订阅坐标信息并且进行转换
        //3.1创建存储空间buffer
    tf2_ros::Buffer buffer;
        //3.2创建监听对象
    tf2_ros::TransformListener listener(buffer);
    //4.组织一个坐标点数据
    geometry_msgs::PointStamped ps;
    ps.header.frame_id= "laser";
    ps.header.stamp=ros::Time::now();
    ps.point.x=2.0;
    ps.point.x=3.0;
    ps.point.x=5.0;
    //5.转换算法
        //5.1循环转换坐标，实现动态转换
    ros::Rate rate(10);
    while (ros::ok())
    {
    //核心代码实现
    geometry_msgs::PointStamped ps_out;
    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

在执行坐标转换之前，执行延时操作

添加休眠函数
ros::Duration (2).sleep();

请输入图片描述

方案2

执行异常检测操作

    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)
{
    ROS_INFO("异常消息：%s",e.what());
}

请输入图片描述

订阅方最终实现

//1.头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
    //1.1buffer缓存头文件
#include "tf2_ros/buffer.h"
    //1.2信息转换头文件
#include "geometry_msgs/PointStamped.h"
    //1.3转换坐标头文件
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
  

  int main(int argc, char  *argv[])
  {
    //2.初始化
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    //3.创建订阅对象----》订阅坐标信息并且进行转换
        //3.1创建存储空间buffer
    tf2_ros::Buffer buffer;
        //3.2创建监听对象
    tf2_ros::TransformListener listener(buffer);
    //4.组织一个坐标点数据
    geometry_msgs::PointStamped ps;
    ps.header.frame_id= "laser";
    ps.header.stamp=ros::Time::now();
    ps.point.x=2.0;
    ps.point.x=3.0;
    ps.point.x=5.0;
    //添加休眠函数
    //ros::Duration (2).sleep();
    //5.转换算法
        //5.1循环转换坐标，实现动态转换
    ros::Rate rate(10);
    while (ros::ok())
    {
    //核心代码实现
    geometry_msgs::PointStamped ps_out;
    /*
            调用了buffer的转换函数transfer
            参数1：被转化的坐标点
            参数2：目标坐标系
            参数3：输出的坐标点
            ps:必须包含头文件"tf2_geometry_msgs/tf2_geometry_msgs.h"
            ps:运行时存在问题，抛出一个异常base_link不存在
                    原因：订阅数据是一个耗时操作，可能再调用transform转换函数时，坐标系的相对关系还没有订阅到，因此出现异常
                    解决：
                                方案1：调用函数之前，执行一段时间的休眠
                                方案2：进行异常处理
    */

        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)
{
    ROS_INFO("异常消息：%s",e.what());
}




    // 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;
  }