ros开发之动态变换位姿发布详解

上一篇介绍了动态变换与静态变换，其中动态变换需要的动态获取节点的位姿信息，然后发布，使用了ros开发的topic通信-发布与订阅模式，上文代码中，只订阅了一次，在主循环中不断的调用订阅的回调函数，这里说明一下原因，顺便复习以下topic通讯

在ROS中，订阅（Subscribe） 是一种事件驱动的机制，类似于Qt的信号与槽。一旦订阅了一个话题，ROS底层会自动监听该话题的消息，每当有新消息发布到该话题时，回调函数就会被自动触发。你不需要显式地多次订阅，也无需手动轮询话题是否有新消息。

---

1. ROS订阅机制的工作原理

(1) 订阅的注册

• 当调用 ros::Subscriber 的 subscribe 方法时，ROS会注册一个回调函数，并开始监听指定话题。
• 例如：

  ros::Subscriber odom_sub = nh.subscribe("odom", 10, odomCallback);
  

  • 这行代码告诉ROS：订阅odom话题，当有新消息时，调用odomCallback函数。
  • 订阅只需一次，后续所有新消息都会自动触发回调函数。

(2) 消息队列与回调触发

• ROS内部维护一个消息队列，所有发布到该话题的消息都会被暂存。
• 当调用 ros::spin() 或 ros::spinOnce() 时，ROS会从队列中取出消息，并触发回调函数。
  • ros::spin()：阻塞当前线程，持续处理回调。
  • ros::spinOnce()：处理一次回调，然后返回。

(3) 事件驱动模型

• 整个过程是异步的：
  1. 新消息发布到odom话题。
  2. ROS将消息存入队列。
  3. 调用 ros::spinOnce() 时，从队列中取出消息，并触发回调函数。
  4. 回调函数处理消息。

---

2. 代码示例与流程分析

以下是一个简化的代码示例，展示订阅和回调的工作流程：

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>

// 回调函数
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    ROS_INFO("Received odom message: x=%f", msg->pose.pose.position.x);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "subscriber_example");
    ros::NodeHandle nh;

    // 1. 订阅话题（只需一次）
    ros::Subscriber sub = nh.subscribe("odom", 10, odomCallback);

    // 2. 进入循环，处理回调
    ros::Rate rate(10);  // 10Hz
    while (ros::ok()) {
        ros::spinOnce();  // 处理一次回调（如果有新消息）
        rate.sleep();
    }

    return 0;
}

流程分析

1. 订阅注册：nh.subscribe 注册了回调函数 odomCallback。
2. 消息发布：假设其他节点不断向odom话题发布消息。
3. 消息接收：ROS将新消息存入队列。
4. 回调触发：每次调用 ros::spinOnce() 时，从队列中取出消息，并调用 odomCallback。
5. 循环处理：通过 while (ros::ok()) 循环，持续处理新消息。

---

3. 与Qt信号槽的类比

ROS的订阅机制与Qt的信号槽机制非常相似：

机制	ROS订阅回调	Qt信号槽
注册方式	subscribe() 注册回调函数	connect() 连接信号与槽
触发方式	新消息到达时自动触发回调	信号发射时自动触发槽函数
队列管理	ROS内部维护消息队列	Qt事件循环维护事件队列
线程模型	单线程（默认）或多线程	单线程（默认）或多线程

---

4. 关键注意事项

(1) 回调函数的执行线程

• 默认情况下，所有回调函数都在 ros::spin() 或 ros::spinOnce() 的调用线程中执行。
• 如果需要多线程处理回调，可以使用 ros::AsyncSpinner。

(2) 消息队列长度

• 在 subscribe() 函数中，第二个参数是队列长度（例如 10）：

  nh.subscribe("odom", 10, odomCallback);
  

  • 如果消息到达的速度快于回调处理的速度，队列会暂存最多10条消息，超出部分将被丢弃。

(3) 必须调用 ros::spin() 或 ros::spinOnce()

• 如果没有调用 ros::spin() 或 ros::spinOnce()，回调函数永远不会被触发！
• 常见的错误是忘记调用 ros::spin()，导致程序看似订阅了话题，但收不到消息。

---

5. 总结

• 订阅只需一次：通过 subscribe() 注册回调函数后，ROS会自动监听话题。
• 事件驱动：新消息到达时，ROS自动触发回调函数。
• 队列处理：通过 ros::spinOnce() 或 ros::spin() 处理消息队列。
• 类比Qt信号槽：ROS的订阅机制与Qt的信号槽机制在异步事件处理上高度相似。

通过这种机制，ROS可以实现高效、异步的消息处理，非常适合机器人系统中多传感器、多节点的复杂场景。

上文中位姿信息与获取更新详解

1. 位姿信息的获取与更新

(1) 回调函数的作用

• 在ROS中，回调函数是异步执行的。每当订阅的话题（如odom）有新消息时，回调函数会自动触发。
• 在代码中，odomCallback函数会在每次收到odom话题的新消息时被调用，从而更新latest_transform中的位姿信息。

(2) 主循环的作用

• 主循环的作用是周期性发布最新的位姿信息。
• 在每次循环中，代码会检查latest_transform是否有新的位姿信息（通过时间戳判断），如果有则发布。

(3) 为什么看起来只获取了一次？

• 代码中并没有只获取一次位姿信息。实际上，回调函数会不断被调用，每次收到odom话题的新消息时都会更新latest_transform。
• 主循环只是负责将最新的位姿信息发布出去，而不是负责获取位姿信息。

---

2. 里程计节点的来源

(1) 里程计节点的作用

• 里程计节点负责计算机器人的位姿信息（位置和方向），并将其发布到odom话题中。
• 位姿信息通常基于轮式编码器、IMU（惯性测量单元）或视觉里程计等传感器的数据。

(2) 里程计节点的实现

• ROS自带的里程计节点：
  • ROS提供了一些默认的里程计实现，例如robot_pose_ekf（基于扩展卡尔曼滤波的里程计）和gmapping（基于激光雷达的里程计）。
  • 这些节点可以直接使用，但需要配置相应的传感器数据输入。
• 自定义里程计节点：
  • 如果没有现成的里程计节点，或者需要根据特定需求实现里程计，可以自己编写里程计节点。
  • 例如，基于轮式编码器的里程计节点可以根据编码器数据计算机器人的位移和旋转。

(3) 里程计节点的选择

• 如果你的机器人已经配置了传感器（如编码器、IMU、激光雷达等），并且有现成的里程计节点，可以直接使用。
• 如果没有现成的里程计节点，或者需要更高的精度和定制化功能，可以自己实现里程计节点。

---

3. 自定义里程计节点的示例

以下是一个简单的自定义里程计节点示例，基于轮式编码器数据计算机器人的位姿信息，并发布到odom话题中。

(1) 代码实现

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>

class OdometryNode {
public:
    OdometryNode() {
        // 初始化ROS节点
        ros::NodeHandle nh;

        // 发布odom话题
        odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);

        // 初始化位姿信息
        x = 0.0;
        y = 0.0;
        theta = 0.0;

        // 设置循环频率
        ros::Rate rate(10.0);

        while (ros::ok()) {
            // 模拟获取编码器数据（假设机器人以0.1m/s的速度沿x轴移动）
            double delta_x = 0.1 * 0.1;  // 0.1m/s * 0.1s
            double delta_y = 0.0;
            double delta_theta = 0.0;

            // 更新位姿信息
            x += delta_x;
            y += delta_y;
            theta += delta_theta;

            // 发布odom消息
            publishOdometry();

            // 按照设定的频率循环
            rate.sleep();
        }
    }

    void publishOdometry() {
        // 创建Odometry消息
        nav_msgs::Odometry odom;
        odom.header.stamp = ros::Time::now();
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_link";

        // 设置位姿信息
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta);

        // 设置速度信息（假设机器人以0.1m/s的速度沿x轴移动）
        odom.twist.twist.linear.x = 0.1;
        odom.twist.twist.linear.y = 0.0;
        odom.twist.twist.angular.z = 0.0;

        // 发布消息
        odom_pub.publish(odom);
    }

private:
    ros::Publisher odom_pub;
    double x, y, theta;  // 机器人的位姿信息
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "odometry_node");
    OdometryNode odometry_node;
    return 0;
}

(2) 代码说明

• 模拟编码器数据：
  • 假设机器人以0.1m/s的速度沿x轴移动，每0.1秒更新一次位姿信息。
• 发布odom消息：
  • 将计算机器人的位姿信息发布到odom话题中。
• 使用tf库：
  • 如果需要发布odom -> base_link的变换关系，可以在publishOdometry函数中添加TransformBroadcaster。

---

4. 总结

• 位姿信息的获取：
  • 通过回调函数不断更新位姿信息，主循环只是周期性发布这些信息。
• 里程计节点的来源：
  • 可以使用ROS自带的里程计节点，也可以根据需求自定义里程计节点。
• 自定义里程计节点：
  • 根据传感器数据（如编码器、IMU）计算机器人的位姿信息，并发布到odom话题中。

通过以上代码示例，你可以在实际开发中实现动态变换的广播，并根据需求选择或实现里程计节点。