当前位置：
  1. 首页
  2. 互联网
  3. 正文
本文介绍: ROS的TF坐标变换（二）：动态坐标变换、多坐标变换代码讲解（以LIO-SAM为例）

目录

1 ROS的动态坐标变换及代码解释

1.1 什么是ROS的动态坐标变换

1.2  CMakeLists.txt、package.xml基础配置

1.3 发布方代码实现

1.4 接收方代码实现  

2 ROS的多坐标变换及代码解释 

2.1 什么是ROS的多坐标变换

2.2 发布方代码实现

2.3 接收方代码实现       

3 LIOSAM中所有的TF坐标变换代码解析

3.1 后端每预测出一帧激光里程计发送TF信息

3.2 IMU前端获取lidar坐标系(/velodyne)到世界坐标系原点(/base_link)的坐标变化

3.3  IMU前端关联/map、/odom坐标系

3.4 IMU前端发布/odom /base_link的TF信息（里程计、 /base_link是原点相当于位姿）

1 ROS的动态坐标变换及代码解释

1.1 什么是ROS的动态坐标变换

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

        我们在这节中，启动 turtlesim_node，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

        

实现分析:

  1. 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点

  2. 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度

  3. 将 pose 信息转换成 坐标系相对信息并发布

实现流程:

  1. 新建功能包，添加依赖（CLION自己创建相关文件）

  2. 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)

  3. 创建坐标相对关系订阅方

  4. 执行

1.2  CMakeLists.txt、package.xml基础配置

        接上节，我们延续上节的CMake配置，不过我们要进行乌龟节点的演示，因此要加入小乌龟的功能包。

cmake_minimum_required(VERSION 2.8.3)
project(test)

######################
### Cmake flags
######################
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")

find_package(catkin REQUIRED COMPONENTS
    roscpp
    rospy
    roslib
    # msg
    std_msgs
    sensor_msgs
    tf2
    tf2_ros
    tf2_geometry_msgs
    geometry_msgs
    turtlesim
)

catkin_package()

include_directories(${catkin_INCLUDE_DIRS})

add_executable(testing main.cpp)
add_executable(static_pub static_pub.cpp)
add_executable(static_sub static_sub.cpp)


target_link_libraries(testing ${catkin_LIBRARIES})
target_link_libraries(static_pub ${catkin_LIBRARIES})
target_link_libraries(static_sub ${catkin_LIBRARIES})

        package.xml配置也是一样，添加小乌龟节点：

<?xml version="1.0"?>
<package>
  <name>test</name>
  <version>0.0.0</version>
  <description>A test</description>
  <maintainer email="haha@nefu.com">haha</maintainer>
  <author>HITLHW</author>
  <license>BSD-3</license>


  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <run_depend>roscpp</run_depend>
  <build_depend>rospy</build_depend>
  <run_depend>rospy</run_depend>

  <build_depend>pcl_conversions</build_depend>
  <run_depend>pcl_conversions</run_depend>

  <build_depend>std_msgs</build_depend>
  <run_depend>std_msgs</run_depend>
  <build_depend>sensor_msgs</build_depend>
  <run_depend>sensor_msgs</run_depend>
  <build_depend>geometry_msgs</build_depend>
  <run_depend>geometry_msgs</run_depend>

  <build_depend>tf2</build_depend>
  <run_depend>tf2</run_depend>
  <build_depend>tf2_ros</build_depend>
  <run_depend>tf2_ros</run_depend>
  <build_depend>tf2_geometry_msgs</build_depend>
  <run_depend>tf2_geometry_msgs</run_depend>

  <build_depend>turtlesim</build_depend>
  <run_depend>turtlesim</run_depend>
</package>

1.3 发布方代码实现

        添加cpp文件，dyna_pub.cpp：

        修改CMakeLists.txt

add_executable(testing main.cpp)
add_executable(static_pub static_pub.cpp)
add_executable(static_sub static_sub.cpp)
add_executable(dyna_pub dyna_pub.cpp)


target_link_libraries(testing ${catkin_LIBRARIES})
target_link_libraries(static_pub ${catkin_LIBRARIES})
target_link_libraries(static_sub ${catkin_LIBRARIES})
target_link_libraries(dyna_pub ${catkin_LIBRARIES})
动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅对象
        5.回调函数处理订阅到的数据(实现TF广播)
            5-1.创建 TF 广播器
            5-2.创建 广播的数据(通过 pose 设置)
            5-3.广播器发布数据
        6.spin

        我们先来看看他们的信息格式吧！先打开乌龟的节点。

        rosrun turtlesim turtlesim_node

        查看话题：rostopic list

        查看乌龟位姿的信息格式：rosmsg info /turtle1/pose

liuhongwei@liuhongwei-Legion-Y9000P-IRX8H:~$ rostopic info /turtle1/pose
Type: turtlesim/Pose

Publishers: 
 * /turtlesim (http://liuhongwei-Legion-Y9000P-IRX8H:37701/)

Subscribers: None

        查看其位姿的信息格式：rosmsg info turtlesim/Pose

        OK，我们准备工作完成，准备写代码！

        我们先要明白订阅方要做什么，肯定是接收乌龟的位姿信息，因此需要一个回调函数。这个回调函数里面是处理乌龟的位姿将其转化为世界坐标系。

        我们回顾上次课程，我们做静态坐标变换时引用的头文件是

        我们在做动态坐标变换时，需要引入头文件

        创建广播数据：

    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();

        也就是说，乌龟的位姿是相当于世界坐标系的。

        发布广播数据：完整代码如下：

//
// Created by liuhongwei on 23-12-2.
//
// 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&amp; 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;
}

        我们执行节点：

        看看现在所有的话题：

        查看话题里面包含什么信息吧！ rostopic echo /tf

        可以看到它是一成不变的，我们控制小乌龟运动：

        现在可以看到tf里面是乌龟相对于world坐标系的变化。我们打开rviz查看：

        更改参考系为世界坐标系，订阅TF话题，可以看到他们的相对位姿变化。

1.4 接收方代码实现  

        建立cpp文件dyna_sub.cpp。

        我们来看要做什么，我们地图上有一个点，乌龟能看到，但是它是基于乌龟的，我们要这个点在世界坐标系下的观测。

//
// Created by liuhongwei on 23-12-2.
//
//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&amp; e)
        {
            // std::cerr << e.what() << 'n';
            ROS_INFO("程序异常:%s",e.what());
        }


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


    return 0;
}

        我们先执行发布节点，再执行接收节点。

        成功！！我们移动乌龟

        这里有个问题：

        如果我们把这个点的时间戳改成ros现在的时间，我们请求的TF变换一定是比这个点早的，这时会报错（两个时间戳差异较大无法进行TF变换）。

        我们可以设置一个等待时间，拿LIOSAM代码举例子：

                tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
                tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);

        这段代码很好！它使用了 `waitForTransform()` 函数来等待指定的时间范围内获取从 `lidarFrame` 到 `baselinkFrame` 的变换信息。然后，使用 `lookupTransform()` 函数来获取这个变换信息并将结果存储在 `lidar2Baselink` 变量中。

        这种结合使用 `waitForTransform()` 和 `lookupTransform()` 的方式是一种良好的实践。它确保了在获取坐标系变换信息之前等待了一定时间，以避免时间推断错误，并在数据准备好后获取变换信息。

        我们加入我们的代码中：

        当你在使用ROS中的TF（Transform Library）时，你经常需要从一个坐标系转换到另一个坐标系。这段代码中的两个函数 `waitForTransform()` 和 `lookupTransform()` 正是用于这个目的的。

        1. `waitForTransform()` 函数：
   – 这个函数是用来等待系统中两个特定坐标系之间的转换关系被建立或者更新。
   – 参数 `lidarFrame` 和 `baselinkFrame` 表示你想要从 `lidarFrame` 到 `baselinkFrame` 进行坐标转换。
   – `ros::Time(0)` 表示你要获取最新的可用转换信息。这里使用 `ros::Time(0)` 表示获取最新的变换信息，即时的转换关系。
   – `ros::Duration(3.0)` 指定了最长等待的时间，这里设置为3秒。如果在这个时间段内没有找到有效的坐标系转换关系，程序会继续执行，但这可能意味着后续的坐标转换可能会出现问题或者失败。

        2. `lookupTransform()` 函数：
   – 一旦 `waitForTransform()` 等待到了需要的坐标系转换关系，`lookupTransform()` 函数就可以用来实际获取这个转换。
   – 它会把从 `lidarFrame` 到 `baselinkFrame` 的最新变换信息存储在 `lidar2Baselink` 变量中。

这两个函数一起使用，确保了在获取坐标系转换信息之前，等待了一定时间，以避免因为时间推断问题而导致的转换错误。

        代码如下：

//
// Created by liuhongwei on 23-12-2.
//
//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::now();
        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);

            geometry_msgs::TransformStamped transformStamped;
            transformStamped = buffer.lookupTransform("world", point_laser.header.frame_id, ros::Time(0));

            // 应用坐标变换到 point_laser 中
            geometry_msgs::PointStamped point_base;
            tf2::doTransform(point_laser, point_base, transformStamped);
            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;
}

        正常啦！

2 ROS的多坐标变换及代码解释 

2.1 什么是ROS的多坐标变换

        现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标。

        换言之，我们在做多传感器融合，有LIDAR、RADAR、CAM三个传感器，我们通过标定知道相机、雷达在机器人坐标系base_link下的关系（相机、雷达安装在机器人上），由我们从雷达看到一个点，怎么转换到相机坐标系下，怎么转换到机器人坐标系下。

实现分析:

  1. 首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
  2. 然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
  3. 最后，还要实现坐标点的转换

实现流程:

  1. 新建功能包，添加依赖

  2. 创建坐标相对关系发布方(需要发布两个坐标相对关系)

  3. 创建坐标相对关系订阅方

  4. 执行

2.2 发布方代码实现

        创建一个launch文件发布 son1 相对于 world和son2相对于world的关系。

        （没有学过的可以看我上一篇博客）

<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>

        即创建一个静态发布器。发布坐标变换。

        启动节点：

        我们通过RVIZ来查看坐标变换关系：

2.3 接收方代码实现       

需求:
    现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
    son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
    求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现流程:
    1.包含头文件
    2.初始化 ros 节点
    3.创建 ros 句柄
    4.创建 TF 订阅对象
    5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
      解析 son1 中的点相对于 son2 的坐标
    6.spin

        我们先看一个API：

tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));

        lookupTransform可以获取son2相对于son1的坐标变换，前提是他们已知的。

        我们设置一个在son1下的坐标点：

            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;

        son1中一点的坐标，要求求出该点在 son2 中的坐标：这个用我们上篇博客的API既可：

            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
            );

        运行：

        完整代码如下：

//
// Created by liuhongwei on 23-12-2.
//
/*

需求:
    现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
    son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
    求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现流程:
    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;
}

3 LIOSAM中所有的TF坐标变换代码解析

3.1 后端每预测出一帧激光里程计发送TF信息

        // Publish TF
        static tf::TransformBroadcaster br;
        tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
                                                      tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
        tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link");
        br.sendTransform(trans_odom_to_lidar);

        transformTobeMapped是当前帧最佳的位姿估计。

1. `tf::TransformBroadcaster br;`：这行代码创建了一个名为`br`的静态变量，它是`TransformBroadcaster`类的一个实例。这个类用于发布坐标变换信息。

2. `tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]), tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));`：这一行代码创建了一个表示从odom坐标系到lidar_link坐标系的变换。它使用了`tf::Transform`类，其中`createQuaternionFromRPY()`函数根据给定的Roll、Pitch和Yaw创建了一个四元数（Quaternion），`tf::Vector3()`创建了一个3D向量，用来表示平移。

3. `tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, “lidar_link”);`：这行代码创建了一个时间戳标记的变换 `trans_odom_to_lidar`。它基于刚刚创建的`tf::Transform`对象`t_odom_to_lidar`，并指定了时间戳`timeLaserInfoStamp`，坐标系为`odometryFrame（odom）`到`lidar_link`。

4. `br.sendTransform(trans_odom_to_lidar);`：这一行代码使用之前创建的`TransformBroadcaster`对象`br`，将刚刚创建的带有时间戳的坐标变换`trans_odom_to_lidar`发布出去，使其他节点可以接收到这个坐标变换信息。

        总体来说，这段代码的作用是创建一个从odom坐标系到lidar_link坐标系的坐标变换，并将它发布到ROS系统中，以便其他节点可以使用这个变换信息进行坐标转换。

3.2 IMU前端获取lidar坐标系(/velodyne)到世界坐标系原点(/base_link)的坐标变化

    tf::TransformListener tfListener;
    tf::StampedTransform lidar2Baselink;

........
            try
            {
                tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
                tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
            }
            catch (tf::TransformException ex)
            {
                ROS_ERROR("%s",ex.what());
            }

        这段代码涉及ROS中TF库的使用，主要用于监听和查询两个坐标系之间的变换关系。

1. `tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));`：这一行代码等待系统中存在从`lidarFrame`到`baselinkFrame`之间的坐标变换。它会等待最多3秒钟，直到这个坐标变换可用或超时。

ros::Time(0)表示的是当前的时间，也就是就等3秒钟取三秒钟最好的。

2. `tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);`：一旦变换可用，这行代码将查询`lidarFrame`到`baselinkFrame`之间的变换关系，并将结果存储在`lidar2Baselink`变量中。

3. `catch (tf::TransformException ex)`：如果在等待或查询过程中出现了异常，比如找不到变换关系，会触发`catch`块中的处理。`ex.what()`会输出异常的详细信息，通过`ROS_ERROR`打印错误消息。

总体来说，这段代码的目的是在ROS系统中等待并查询`lidarFrame`到`baselinkFrame`之间的坐标变换关系，并在获取失败时输出错误消息。

3.3  IMU前端关联/map、/odom坐标系

        static tf::TransformBroadcaster tfMap2Odom;
        static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
        tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));

        这段代码涉及使用ROS中的TF库来发布一个名为`map_to_odom`的静态坐标变换，将地图坐标系（`mapFrame`）与里程计坐标系（`odometryFrame`）进行关联。

1. `static tf::TransformBroadcaster tfMap2Odom;`：创建了一个名为`tfMap2Odom`的静态`TransformBroadcaster`对象，用于发布坐标变换。

2. `static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));`：定义了一个静态的`tf::Transform`变量`map_to_odom`，表示一个从地图坐标系到里程计坐标系的变换。这个变换在这里被初始化为没有旋转（RPY为0）和没有平移（坐标为0,0,0）的初始变换。

3. `tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));`：通过`tfMap2Odom`的`sendTransform`函数，发布了一个时间戳标记的坐标变换。这个变换基于`map_to_odom`，时间戳为`odomMsg->header.stamp`（里程计消息的时间戳），并将这个变换从`mapFrame`（地图坐标系）发送到`odometryFrame`（里程计坐标系）。

        这段代码的作用是在ROS系统中发布一个固定的、从地图坐标系到里程计坐标系的初始坐标变换，可以为后续节点提供这两个坐标系之间的初始关联信息。

        可以看到/map坐标系和/odom是重合的。

3.4 IMU前端发布/odom /base_link的TF信息（里程计、 /base_link是原点相当于位姿）

        // static tf
        static tf::TransformBroadcaster tfMap2Odom;
        static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
        tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));

        std::lock_guard<std::mutex> lock(mtx);

        imuOdomQueue.push_back(*odomMsg);

        // get latest odometry (at current IMU stamp)
        if (lidarOdomTime == -1)
            return;
        while (!imuOdomQueue.empty())
        {
            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
                imuOdomQueue.pop_front();
            else
                break;
        }
        Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
        Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
        
        // publish latest odometry
        nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
        laserOdometry.pose.pose.position.x = x;
        laserOdometry.pose.pose.position.y = y;
        laserOdometry.pose.pose.position.z = z;
        laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
        pubImuOdometry.publish(laserOdometry);

        // publish tf
        static tf::TransformBroadcaster tfOdom2BaseLink;
        tf::Transform tCur;
        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
        if(lidarFrame != baselinkFrame)
            tCur = tCur * lidar2Baselink;
        tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
        tfOdom2BaseLink.sendTransform(odom_2_baselink);

        发布了base_link和odom之间的信息。

原文地址:https://blog.csdn.net/qq_41694024/article/details/134747660

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。

如若转载，请注明出处：http://www.7code.cn/show_25630.html

如若内容造成侵权/违法违规/事实不符，请联系代码007邮箱：suwngjj01@126.com进行投诉反馈，一经查实，立即删除！

上一篇 将Linux 标准输出，错误输出重定向到文件
下一篇 Linux swapon命令教程：如何在Linux中启用和禁用交换空间（附实例教程和注意事项）

相关文章

发表回复

您的邮箱地址不会被公开。 必填项已用 * 标注

插入图片
浏览器会保存昵称、邮箱和网站cookies信息，下次评论时使用。