实现ROS中两个里程计数据的转换到同一坐标系下

        在多传感器融合的场景中，不同传感器可能会提供不同的位置信息。这段代码的目标是将来自两个不同来源的里程计数据转换到同一个参考坐标系（在这里，选择 odom0 的坐标系作为参考）下进行对齐，以便于后续的融合和处理。

核心步骤解析

1. 读取和订阅里程计数据：

   • 代码首先从ROS参数服务器读取里程计数据的订阅话题名称。
   • 然后，订阅来自两个不同来源的里程计数据，并通过回调函数来更新全局变量 odom0 和 odom1。

2. 计算修正变换：

   • 当第一次接收到 odom0 和 odom1 数据时，计算一个初始的修正变换 transform_correction。
   • 这个修正变换用于将 odom1 的数据转换到 odom0 的坐标系下。

3. 应用修正变换：

   • 对于后续接收到的 odom1 数据，代码会应用这个修正变换，将 odom1 的姿态和位置转换到 odom0 的坐标系中。
   • 转换后的数据会被发布到带有后缀的修正话题上。

详细的代码注释

下面是原代码，加上了更详细的中文注释，帮助理解每一步的目的和操作：

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include "nav_msgs/Odometry.h"

// 定义全局变量来存储里程计消息
nav_msgs::Odometry odom0;
nav_msgs::Odometry odom1;
bool has_new_odom0_received = false;
bool has_new_odom1_received = false;

// odom0的回调函数，当接收到odom0的里程计数据时调用
void odometry0_callback(const nav_msgs::Odometry::ConstPtr& odom_msg)
{
    odom0 = *odom_msg; // 更新全局变量odom0
    if (!has_new_odom0_received) has_new_odom0_received = true; // 标记为已接收新数据
}

// odom1的回调函数，当接收到odom1的里