【ROS进阶篇】第五讲 ROS中的TF坐标变换

最新推荐文章于 2024-06-21 12:54:16 发布
最新推荐文章于 2024-06-21 12:54:16 发布
阅读量936 收藏 12
点赞数 2
分类专栏： ROS进阶教程 文章标签： 学习 c++ 自动驾驶 ubuntu 交互
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/lc1852109/article/details/125525770
版权

【ROS进阶篇】第五讲 ROS中的TF坐标变换

在这里插入图片描述

文章目录

前言

在入门篇的ROS教程中，我们已经简单研究了有关TF坐标变换的内容，进阶篇的目的在于更加深入的从编程角度理解TF坐标变换的使用方法，并给出静态、动态以及多坐标变换等诸多情况下的调用，更加完善的掌握TF功能包。
在这里插入图片描述

一、TF功能包介绍

1. 概念

  • TF：即TransForm Frame，指坐标变换；
  • 坐标系：即物体的位置标定系统，以右手坐标系为标准；

在这里插入图片描述

  • 作用：实现不同坐标系之间的点或者向量的转换

2. TF2与TF

  • TF2：在现在的ROS系统中，TF2已经替换了TF使用，并且包含原有功能；
  • 区别：
  1. TF2功能包对应tf2、tf2_ros；TF功能包对应tf包，TF2的功能包增强了内聚性，且内部的不同API完成了分包处理；
  2. TF2实现效率更高，这里以静态坐标变换演示；
  • 静态坐标变换演示：由于信息固定，故TF2更加高效
  1. TF2：
    变换指令：rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 /base_link /laser
    查看话题消息（rostopic）：
    rostopic echo /tf（循环输出）
  2. TF：
    变换指令：rosrun tf static_transform_publisher 0 0 0 0 0 0 /base_link /laser 100
    TF启动参数更多，存在发布频率
    查看话题消息（rostopic）：
    rostopic echo /tf_static（单次输出）
  • 常见功能包：
  1. tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
  2. tf2: 封装了坐标变换的常用消息。
  3. tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。

3. 坐标系查看方法

  • 对应功能包：tf2_tools，安装指令如下
sudo apt install ros-noetic-tf2-tools
  • 工具使用：生成对应pdf文件，图谱呈树形结构
  1. 启动广播坐标系程序
  2. 运行工具包指令：rosrun tf2_tools view_frames.py
  3. 生成日志如下：
[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...
  1. 打开目录下的pdf文件：evince frames.pdf
    在这里插入图片描述

4. 坐标信息

  • 常用msg信息：
  1. geometry_msgs/TransformStamped：用于传输坐标系相关位置信息
std_msgs/Header header                     #头信息
 uint32 seq                                #|-- 序列号
 time stamp                                #|-- 时间戳
 string frame_id                            #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
 geometry_msgs/Vector3 translation        #偏移量
   float64 x                                #|-- X 方向的偏移量
   float64 y                                #|-- Y 方向的偏移量
   float64 z                                #|-- Z 方向上的偏移量
 geometry_msgs/Quaternion rotation        #四元数
   float64 x                                
   float64 y                                
   float64 z                                
   float64 w
  1. geometry_msgs/PointStamped：用于传输某个坐标系中坐标点的信息
std_msgs/Header header                    #头
 uint32 seq                                #|-- 序号
 time stamp                                #|-- 时间戳
 string frame_id                            #|-- 所属坐标系的 id
geometry_msgs/Point point                #点坐标
 float64 x                                    #|-- x y z 坐标
 float64 y
 float64 z
  • 查看msg信息指令：rosmsg info

二、静态坐标变换

1. 情景假设

  • 问题展示：假定一个机器人模型，躯干主体和雷达分别固接两个坐标系，二坐标系原点位于对应物理中心，雷达原点与躯干原点位移关系为：x：0.2 y：0.0 z：0.5，在雷达坐标系中有一物体坐标为（2.0 3.0 5.0），求此物体在躯干坐标系下的坐标？

  • 3d下的演示：图中使用了rviz组件，具体使用方法见后续博客
    在这里插入图片描述

2. 编程实现

  • 基本思路：
  1. 创建功能包：需要包含tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs等
  2. 创建发布者：发布躯干坐标系与雷达坐标之间的相对关系
  3. 创建订阅者：订阅已发布信息，根据物体信息，借助TF功能包实现坐标变换并输出；

注：ROS系统也提供了静态坐标转换的节点：

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
  • 发布者代码：
/* 
    静态坐标变换发布方:
        发布关于 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.生成一个坐标点(相对于子级坐标系)
        5.转换坐标点(相对于父级坐标系)
        6.spin()
*/
//1.包含头文件
#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" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 1;
        point_laser.point.y = 2;
        point_laser.point.z = 7.3;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"base_link");
            ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常.....");
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

三、动态坐标变换

1. 情景假设

  • 问题展示：键盘控制海龟运动，动态发布海龟坐标系与世界坐标系之间的关系；

  • 3d演示（rviz）：
    在这里插入图片描述

2. 编程实现

  • 基本思路：
  1. 创建功能包：依赖项包含tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim；
  2. 创建发布者：订阅turtle1/pose，获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度，将 pose 信息转换成坐标系相对信息并发布
  3. 创建订阅者：订阅相对信息并输出；
  • 发布者代码：
/*  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅对象
        5.回调函数处理订阅到的数据(实现TF广播)
            5-1.创建 TF 广播器
            5-2.创建 广播的数据(通过 pose 设置)
            5-3.广播器发布数据
        6.spin
*/
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  5-1.创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.创建 广播的数据(通过 pose 设置)
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";

    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
    //  |--------- 四元数设置
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    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-3.广播器发布数据
    broadcaster.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    //     5.回调函数处理订阅到的数据(实现TF广播)
    //        
    // 6.spin
    ros::spin();
    return 0;
}
  • 订阅者代码：
//1.包含头文件
#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" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time();
        point_laser.point.x = 1;
        point_laser.point.y = 1;
        point_laser.point.z = 0;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常:%s",e.what());
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

注：关于动态坐标变换之间的进阶编程实际上在入门篇的介绍中就展示了海龟跟踪的编程实现即演示，请翻阅入门篇教程

四、多坐标变换

1. 情景假设

  • 问题展示：假设存在三个坐标系，其中一个坐标系为父坐标系world，其余两个为子坐标系，父子相对关系已知，求两个子坐标系之间的关系；

在这里插入图片描述

2. 编程实现

  • 基本思路：
  1. 创建功能包：依赖项包含tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
  2. 创建发布者：发布父子坐标系之间的坐标消息
  3. 创建订阅者：订阅相对关系，通过TF完成两坐标系转换
  • 发布者代码：利用静态坐标变换发布
<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>
  • 订阅者代码：
/*

实现流程:
    1.包含头文件
    2.初始化 ros 节点
    3.创建 ros 句柄
    4.创建 TF 订阅对象
    5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
      解析 son1 中的点相对于 son2 的坐标
    6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"

int main(int argc, char *argv[])
{   setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_frames");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer; 
    tf2_ros::TransformListener listener(buffer);
    // 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
    ros::Rate r(1);
    while (ros::ok())
    {
        try
        {
        //   解析 son1 中的点相对于 son2 的坐标
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
                    tfs.transform.translation.x,
                    tfs.transform.translation.y,
                    tfs.transform.translation.z
                    );

            // 坐标点解析
            geometry_msgs::PointStamped ps;
            ps.header.frame_id = "son1";
            ps.header.stamp = ros::Time::now();
            ps.point.x = 1.0;
            ps.point.y = 2.0;
            ps.point.z = 3.0;

            geometry_msgs::PointStamped psAtSon2;
            psAtSon2 = buffer.transform(ps,"son2");
            ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.point.z
                    );
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("异常信息:%s",e.what());
        }


        r.sleep();
        // 6.spin
        ros::spinOnce();
    }
    return 0;
}

总结

  • 声明：本节博客部分参考了CSDN用户赵虚左的ROS教程，从下篇博客起ROS进阶篇的教程将会步入ROSBAG的基础学习上，掌握这个强大的ROS记录工具的使用方法，敬请期待。

在这里插入图片描述

生如昭诩
关注
  • 2
    点赞
  • 踩
  • 12
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
专栏目录
ROS-9 ROS常用组件之TF坐标变换
m0_46093829的博客
01-03 1323
一、概念 坐标变换简单来说就是归一化，即把测得的其他物体和机器人身上传感器之间的位置信息都换为物体和机器人的原点之间的信息。 根据我们高中学习的知识，只要明确了不同坐标系之间的的相对关系，就可以实现任何坐标点在不同坐标系之间的转换，但是该计算实现是较为常用的，且算法也有点复杂，因此在 ROS 中直接封装了相关的模块: 坐标变换(TF)。TF坐标变换用于在ROS中实现不同坐标系之间的点或向量的转换。 TF坐标变换通过广播TF变换和监听TF变换来实现的 二、查看系统中当前各个坐标系是否连通 ro
ROS常用组件（二）-----TF坐标变换
dawenzideboke的博客
02-26 835
坐标变换是机器人系统中常用的基础功能，ROS中坐标变换由TF功能包维护。它可以根据时间缓冲并维护多个坐标系之间的坐标变换关系，目前 tf2 比 tf 用的多。 1. 坐标变换常用msg 1.1 geometry_msgs/TransformStamped–>用于传输坐标系相关位置信息 命令行键入:rosmsg info geometry_msgs/TransformStamped std_msgs/Header header #头信息 uint32 .
ROS TF坐标变换
m0_73640344的博客
06-21 900
TF坐标变换在ROS（Robot Operating System）中，TF（Transform）是一个非常重要的概念，它用于处理不同坐标系之间的空间关系。在机器人和自动化领域，理解和管理不同部分或传感器之间的相对位置和方向是至关重要的。TF提供了一种有效和一致的方式来处理这些问题。
ROS——TF坐标变换
LYY_WJL的博客
09-12 1104
ROS——tf坐标系广播器与监听器编程实现
ROS：TF坐标变换
立于皓月之边，不弱星光之势
07-07 496
ROS：TF坐标变换
《ROS机器人开发实践》学习：TF坐标变换
他山之石 可以攻玉
12-07 568
turtle_tf_broadcaster.cpp #include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <turtlesim/Pose.h> std::string turtle_name; void poseCallback(const turtlesim::PoseConstPtr& msg) { // tf广播器 static tf::Transfor
自动驾驶ROS中的TF坐标变换（一）：静态坐标变换
最新发布
08-15
是博文对应的源码资料，里面的工程内含一个静态坐标及发布的使用示例，可以用c++进行编译调用，详细内容请看博文
ROS学习笔记10：TF坐标变换（ROS常用组件）
qq_71734878的博客
11-22 2859
旋转参数的第一种命令格式使用以弧度为单位的yaw、pitch、roll角度（yaw是围绕z轴旋转的偏航角，pitch是围绕y轴旋转的俯仰角，roll是围绕x轴旋转的翻滚角），第二种命令格式使用四元数表达旋转角度。机器人本体和机器人的工作环境中往往存在大量的组件元素，在机器人设计和应用中会涉及不同组件的位置和姿态，这就需要引入坐标系和坐标变换的概念。可视化系统当中的所有tf的关系，监听5s，把5s之内所有坐标系之间的关系保存下来，并生成一个pdf。（2）机器人夹取的物体相对于机器人中心坐标系的位置；
ROS中TF(坐标系转换)原理与使用
sinat_28924869的博客
01-21 4443
官网建议新工作直接使用tf2，因为它有一个更清洁的界面，和更好的使用体验。（自ROS Hydro以来，tf第一代已被“弃用”，转而支持tf2）。 TF介绍 TF(TransForm)，就是坐标转换，包括了位置和姿态两个方面的变换。注意区分坐标转换和坐标系转换。 坐标转换是一个坐标在不同坐标系下的表示，而坐标系转换不同坐标系的相对位姿关系。 ROS中机器人模型包含大量的部件，每一个部件统称之为link（比如手部、头部、某个关节、某个连杆）,每一个link上面对应着一个frame（坐标系）, 用frame表示该
[ROS 系列学习教程] ROS TF坐标变换 - 动态坐标变换
淋曦的进击手记
01-01 2675
所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。比如机械臂末端执行器与 base_link 之间，移动机器人base_link与world之间。可以理解动态坐标关系是随时间变化的静态坐标关系（即静态是动态对时间的微分）。订阅动态坐标转换关系，并利用该关系将小乌龟坐标系下的坐标转换到。目录，在这里存储脚本（如python脚本），我们创建。可以看到转换后的坐标，以及所属父坐标系。以及转换后的坐标都在同步动态变化。发布它相对世界坐标系的坐标。按钮，在弹出的窗口中选择。目录的同级，创建一个。
ROS高效进阶第一章 -- ROS高级组件之 TF坐标变换 其一
cy1641395022的博客
07-11 965
ROS高效进阶第一章 -- ROS高级组件之 TF坐标变换 其一
ROS中的TF（坐标变换）
RTFSC的博客
06-27 1065
文章目录tf常用tf工具写一个 tf broadcaster (C++)：src/turtle_tf_broadcaster.cpp写一个tf listener (C++)：src/turtle_tf_listener.cpp编译节点参考 tf 一个机器人通常有很多坐标系，tf能够跟踪所有定义的坐标系随时间的变化，以及他们之间的相对位置。 常用tf工具 使用view_frames或rqt_tf_tree工具查看发布的tf坐标系信息： ＃　creates a diagram of the frames b
ros入门（八）---TF坐标转换
weixin_51157015的博客
07-07 2551
TF(TransForm)，就是坐标转换，包括了位置和姿态两个方面的变换。
ros tf坐标变换
qinshanjy的博客
04-16 773
启动aster roscore 输入 roslaunch turtle_tf turtle_tf_demo.launch 可以看到两只海龟，其中一只海龟会追着另一只海龟跑。 用键盘控制移动 rosrun turtlesim turtle_teleop_key 输入以下命令，打印一张pdf可以清晰看到坐标 rosrun tf view_frames 监听查看两者的坐标变换 rosrun...
ROS之tf坐标变换
sannianyihoushuma的博客
04-22 366
https://blog.csdn.net/ZXQHBD/article/details/69569051
【ROS】中级操作学习整理-TF坐标变换
qq_52785580的博客
05-09 4454
本文介绍了TF坐标变换的基本概念以及TF在ROS中的表示形式（TF树），通过古月居的乌龟跟随的例子，分析了TF树的广播器和监听器最基本的书写形式，从中展示了如何提取和应用TF变换的信息，并介绍了5种最常用的TF树及TF信息的提取工具，最后简要说明了SLAM中最为重要的两种TF坐标变换。
ros之TF坐标转换
苏凯的博客
07-10 862
ros之TF坐标转换 1.c++ demo01_turtle1.cpp #include <iostream> #include <ros/ros.h> #include <turtlesim/Pose.h> #include <tf/transform_broadcaster.h> using namespace std; void callBack(const tur...
使用tf库将点云转换到机器人基础帧中原理
05-11
点云转换到机器人基础帧的过程需要几个步骤： 1. 计算点云在世界坐标系中的位置和姿态 2. 根据机器人基础帧在世界坐标系中的位置和姿态，计算机器人基础帧在世界坐标系中的变换矩阵 3. 计算点云在机器人基础帧坐标系中的位置和姿态，即将点云从世界坐标系转换到机器人基础帧坐标系中 下面是使用tf库实现上述过程的代码： ```python import tf import numpy as np # 计算点云在世界坐标系中的位置和姿态 pcl_pose = tf.transformations.quaternion_matrix([0,0,0,1]) # 点云的位置和姿态 pcl_pose[0, 3] = 1.0 pcl_pose[1, 3] = 2.0 pcl_pose[2, 3] = 3.0 # 计算机器人基础帧在世界坐标系中的位置和姿态 robot_base_pose = tf.transformations.quaternion_matrix([0,0,0,1]) # 机器人基础帧的位置和姿态 robot_base_pose[0, 3] = 4.0 robot_base_pose[1, 3] = 5.0 robot_base_pose[2, 3] = 6.0 # 计算机器人基础帧在世界坐标系中的变换矩阵 robot_base_tf = tf.transformations.concatenate_matrices(tf.transformations.inverse(robot_base_pose), pcl_pose) # 计算点云在机器人基础帧坐标系中的位置和姿态 pcl_pose_in_base = tf.transformations.concatenate_matrices(robot_base_tf, robot_base_pose) # 将点云从世界坐标系转换到机器人基础帧坐标系中 pcl_in_base = np.array([[pcl_pose_in_base[0, 3]], [pcl_pose_in_base[1, 3]], [pcl_pose_in_base[2, 3]]]) ``` 在上述代码中，我们首先计算了点云在世界坐标系中的位置和姿态，以及机器人基础帧在世界坐标系中的位置和姿态。然后，我们通过tf库中的`tf.transformations.quaternion_matrix`函数将它们转换为变换矩阵。接着，我们计算了机器人基础帧在世界坐标系中的变换矩阵，并使用`tf.transformations.concatenate_matrices`函数将其与点云的变换矩阵相乘，得到点云在机器人基础帧坐标系中的位置和姿态。最后，我们将其转换为数组形式，即为点云在机器人基础帧坐标系中的位置。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

生如昭诩

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值