8.ROS的TF坐标变换(二):动态坐标变换、多坐标变换代码讲解(以LIO-SAM为例)

2023-12-02 13:12

本文主要是介绍8.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 COMPONENTSroscpprospyroslib# msgstd_msgssensor_msgstf2tf2_rostf2_geometry_msgsgeometry_msgsturtlesim
)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/PosePublishers: * /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();//  |----坐标系 IDtfs.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& 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();//  |----坐标系 IDtfs.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.spinros::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& 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.spinros::spinOnce();}return 0;
}

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

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

        // Publish TFstatic 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 tfstatic 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();elsebreak;}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 odometrynav_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 tfstatic 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之间的信息。

这篇关于8.ROS的TF坐标变换(二):动态坐标变换、多坐标变换代码讲解(以LIO-SAM为例)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/445470

相关文章

第10章 中断和动态时钟显示

第10章 中断和动态时钟显示 从本章开始,按照书籍的划分,第10章开始就进入保护模式(Protected Mode)部分了,感觉从这里开始难度突然就增加了。 书中介绍了为什么有中断(Interrupt)的设计,中断的几种方式:外部硬件中断、内部中断和软中断。通过中断做了一个会走的时钟和屏幕上输入字符的程序。 我自己理解中断的一些作用: 为了更好的利用处理器的性能。协同快速和慢速设备一起工作

动态规划---打家劫舍

题目: 你是一个专业的小偷,计划偷窃沿街的房屋。每间房内都藏有一定的现金,影响你偷窃的唯一制约因素就是相邻的房屋装有相互连通的防盗系统,如果两间相邻的房屋在同一晚上被小偷闯入,系统会自动报警。 给定一个代表每个房屋存放金额的非负整数数组,计算你 不触动警报装置的情况下 ,一夜之内能够偷窃到的最高金额。 思路: 动态规划五部曲: 1.确定dp数组及含义 dp数组是一维数组,dp[i]代表

活用c4d官方开发文档查询代码

当你问AI助手比如豆包,如何用python禁止掉xpresso标签时候,它会提示到 这时候要用到两个东西。https://developers.maxon.net/论坛搜索和开发文档 比如这里我就在官方找到正确的id描述 然后我就把参数标签换过来

poj 1258 Agri-Net(最小生成树模板代码)

感觉用这题来当模板更适合。 题意就是给你邻接矩阵求最小生成树啦。~ prim代码:效率很高。172k...0ms。 #include<stdio.h>#include<algorithm>using namespace std;const int MaxN = 101;const int INF = 0x3f3f3f3f;int g[MaxN][MaxN];int n

计算机毕业设计 大学志愿填报系统 Java+SpringBoot+Vue 前后端分离 文档报告 代码讲解 安装调试

🍊作者:计算机编程-吉哥 🍊简介:专业从事JavaWeb程序开发,微信小程序开发,定制化项目、 源码、代码讲解、文档撰写、ppt制作。做自己喜欢的事,生活就是快乐的。 🍊心愿:点赞 👍 收藏 ⭐评论 📝 🍅 文末获取源码联系 👇🏻 精彩专栏推荐订阅 👇🏻 不然下次找不到哟~Java毕业设计项目~热门选题推荐《1000套》 目录 1.技术选型 2.开发工具 3.功能

代码随想录冲冲冲 Day39 动态规划Part7

198. 打家劫舍 dp数组的意义是在第i位的时候偷的最大钱数是多少 如果nums的size为0 总价值当然就是0 如果nums的size为1 总价值是nums[0] 遍历顺序就是从小到大遍历 之后是递推公式 对于dp[i]的最大价值来说有两种可能 1.偷第i个 那么最大价值就是dp[i-2]+nums[i] 2.不偷第i个 那么价值就是dp[i-1] 之后取这两个的最大值就是d

pip-tools:打造可重复、可控的 Python 开发环境,解决依赖关系,让代码更稳定

在 Python 开发中,管理依赖关系是一项繁琐且容易出错的任务。手动更新依赖版本、处理冲突、确保一致性等等,都可能让开发者感到头疼。而 pip-tools 为开发者提供了一套稳定可靠的解决方案。 什么是 pip-tools? pip-tools 是一组命令行工具,旨在简化 Python 依赖关系的管理,确保项目环境的稳定性和可重复性。它主要包含两个核心工具:pip-compile 和 pip

D4代码AC集

贪心问题解决的步骤: (局部贪心能导致全局贪心)    1.确定贪心策略    2.验证贪心策略是否正确 排队接水 #include<bits/stdc++.h>using namespace std;int main(){int w,n,a[32000];cin>>w>>n;for(int i=1;i<=n;i++){cin>>a[i];}sort(a+1,a+n+1);int i=1

html css jquery选项卡 代码练习小项目

在学习 html 和 css jquery 结合使用的时候 做好是能尝试做一些简单的小功能,来提高自己的 逻辑能力,熟悉代码的编写语法 下面分享一段代码 使用html css jquery选项卡 代码练习 <div class="box"><dl class="tab"><dd class="active">手机</dd><dd>家电</dd><dd>服装</dd><dd>数码</dd><dd

生信代码入门:从零开始掌握生物信息学编程技能

少走弯路,高效分析;了解生信云,访问 【生信圆桌x生信专用云服务器】 : www.tebteb.cc 介绍 生物信息学是一个高度跨学科的领域,结合了生物学、计算机科学和统计学。随着高通量测序技术的发展,海量的生物数据需要通过编程来进行处理和分析。因此,掌握生信编程技能,成为每一个生物信息学研究者的必备能力。 生信代码入门,旨在帮助初学者从零开始学习生物信息学中的编程基础。通过学习常用