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

本文主要是介绍实现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; // 更新全局变量odom0if (!has_new_odom0_received) has_new_odom0_received = true; // 标记为已接收新数据
}// odom1的回调函数，当接收到odom1的里程计数据时调用
void odometry1_callback(const nav_msgs::Odometry::ConstPtr& odom_msg)
{odom1 = *odom_msg; // 更新全局变量odom1if (!has_new_odom1_received) has_new_odom1_received = true; // 标记为已接收新数据
}int main(int argc, char *argv[])
{ros::init(argc, argv, "odometry_fusion_correction"); // 初始化ROS节点ros::NodeHandle n("~"); // 创建私有节点句柄// 从参数服务器获取里程计话题名称和修正话题的后缀std::string odom0_topic = "/odom0";n.getParam("odom0_topic", odom0_topic);std::string odom1_topic = "/odom1";n.getParam("odom1_topic", odom1_topic);std::string correction_suffix = "/corrected";n.getParam("correction_suffix", correction_suffix);// 订阅odom0和odom1话题ros::Subscriber sub_0 = n.subscribe(odom0_topic, 1, odometry0_callback);ros::Subscriber sub_1 = n.subscribe(odom1_topic, 1, odometry1_callback);// 创建修正后的odom0和odom1话题的发布者ros::Publisher pub_0 = n.advertise<nav_msgs::Odometry>(odom0_topic + correction_suffix, 1);ros::Publisher pub_1 = n.advertise<nav_msgs::Odometry>(odom1_topic + correction_suffix, 1);// 从参数服务器获取里程计和子坐标系的名称std::string odom_frame_id = "odom";n.getParam("odom_frame_id", odom_frame_id);std::string child_frame_id = "base_link";n.getParam("child_frame_id", child_frame_id);// 获取协方差的对角线值，用于里程计的姿态不确定性float odom0_covariance_diag_value = -1;n.getParam("odom0_covariance_diag_value", odom0_covariance_diag_value);float odom1_covariance_diag_value = -1;n.getParam("odom1_covariance_diag_value", odom1_covariance_diag_value);bool initialization_done = false; // 初始化标志ros::Rate loop_rate(10); // 循环频率设为10Hztf::TransformBroadcaster br; // 创建TF广播器tf::Transform transform_correction; // 修正变换while (ros::ok()){nav_msgs::Odometry odom_corrected; // 用于存储修正后的里程计消息// 当接收到足够的数据后，计算初始的修正变换if (!initialization_done && has_new_odom0_received && has_new_odom1_received){// 将odom0的四元数消息转换为TF四元数tf::Quaternion odom0_orientation;tf::quaternionMsgToTF(odom0.pose.pose.orientation, odom0_orientation);// 基于odom0的姿态和位置，计算修正变换transform_correction = tf::Transform(odom0_orientation, tf::Vector3(odom0.pose.pose.position.x, odom0.pose.pose.position.y, odom0.pose.pose.position.z));initialization_done = true; // 设置初始化完成标志}if (initialization_done){if (has_new_odom1_received){// 将odom1的四元数消息转换为TF四元数tf::Quaternion odom1_orientation;tf::quaternionMsgToTF(odom1.pose.pose.orientation, odom1_orientation);// 基于odom1的姿态和位置，构造TF变换对象tf::Transform odom1_transform(odom1_orientation, tf::Vector3(odom1.pose.pose.position.x, odom1.pose.pose.position.y, odom1.pose.pose.position.z));// 应用修正变换，将odom1的坐标转换到odom0的坐标系下odom1_transform = transform_correction * odom1_transform;// 修正后的odom1数据odom_corrected = odom1;odom_corrected.pose.pose.position.x = odom1_transform.getOrigin().getX();odom_corrected.pose.pose.position.y = odom1_transform.getOrigin().getY();odom_corrected.pose.pose.position.z = odom1_transform.getOrigin().getZ();tf::quaternionTFToMsg(odom1_transform.getRotation(), odom_corrected.pose.pose.orientation); // 将修正后的姿态转换为ROS消息格式// 发布修正后的odom1消息odom_corrected.header.frame_id = odom_frame_id;odom_corrected.child_frame_id = child_frame_id;if (odom1_covariance_diag_value != -1){// 设置修正后的协方差矩阵（对角线值）odom_corrected.pose.covariance = {odom1_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom1_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom1_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom1_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom1_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom1_covariance_diag_value};}pub_1.publish(odom_corrected); // 发布修正后的odom1消息}if (has_new_odom0_received){// 设置修正后的odom0数据odom0.header.frame_id = odom_frame_id;odom0.child_frame_id = child_frame_id;if (odom0_covariance_diag_value != -1){// 设置修正后的协方差矩阵（对角线值）odom0.pose.covariance = {odom0_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom0_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom0_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom0_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom0_covariance_diag_value, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odom0_covariance_diag_value};}pub_0.publish(odom0); // 发布修正后的odom0消息}}has_new_odom0_received = false; // 重置odom0接收标志has_new_odom1_received = false; // 重置odom1接收标志ros::spinOnce(); // 处理回调函数的未处理消息loop_rate.sleep(); // 控制循环频率}return 0;
}

修正变换的计算与应用示例

        让我们假设 odom0 和 odom1 的初始位置和姿态如下：

• odom0 的位置为 (1, 2, 0)，姿态为四元数 (0, 0, 0, 1)。
• odom1 的位置为 (2, 3, 0)，姿态为四元数 (0, 0, 0.7071, 0.7071)（旋转90度）。

        当 transform_correction 被计算时，它会将 odom1 的坐标转换到 odom0 的坐标系下。

        在计算了 transform_correction 之后，如果 odom1 的新数据为位置 (2, 4, 0)，姿态为四元数 (0, 0, 0.7071, 0.7071)，应用 transform_correction 后，转换到 odom0 的坐标系中可能会得到修正后的数据，确保 odom1 的数据在 odom0 的坐标系下是对齐和一致的。

        这个转换过程会调整 odom1 的位置和姿态，使得它们在 odom0 的坐标系下具有一致的表示。通过这种方式，可以实现不同来源的里程计数据的对齐和融合。

这篇关于实现ROS中两个里程计数据的转换到同一坐标系下的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---