Response:

这是一篇为您量身定制的 ROS 2 Diagnostics（诊断系统）实战博客文章。结合您之前对底层数据结构和运行机制的深刻理解，这篇教程将带您从零开始，亲手用 C++ 写出一个标准、优雅的“机器人体检节点”。

🚀 从零玩转 ROS 2 诊断系统 (Diagnostics)：给你的机器人建一个“体检中心”

在 ROS 2 开发中，随着机器人身上的传感器越来越多（雷达、相机、IMU、电机），如何统一监控它们的健康状态成了一个大问题。

有的人写日志（RCLCPP_ERROR），有的人发自定义消息。但最优雅、最符合行业标准的方法，是使用 ROS 2 自带的 Diagnostics 诊断系统。

今天，我们将把理论化为实践，从零编写一个带有完整诊断功能的传感器节点。读完本文，您将彻底弄懂 diagnostic_updater 的黑盒机制。

🛠️ 第一步：创建功能包

首先，我们需要创建一个新的 ROS 2 C++ 功能包，并声明依赖项。注意这里必须要依赖 diagnostic_updater。

打开终端，在您的 workspace 的 src 目录下运行：

ros2 pkg create --build-type ament_cmake my_sensor_diag --dependencies rclcpp diagnostic_updater

💻 第二步：编写核心代码

我们在 my_sensor_diag/src 目录下创建一个名为 dummy_sensor_node.cpp 的文件。

这个例子中，我们将模拟一个会“发烧”的温度传感器。它的温度会在正常和超标之间波动，我们要用诊断系统把这个状态实时汇报给 /diagnostics 话题。

#include <rclcpp/rclcpp.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <random>

class DummySensorNode : public rclcpp::Node
{
public:
    DummySensorNode() : Node("dummy_sensor_node")
    {
        // 1. 初始化诊断更新器 (Updater)，并将当前节点传入
        diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(this);
        
        // 2. 设置硬件ID (防重名，通常写设备序列号或端口号)
        diagnostic_updater_->setHardwareID("TempSensor-001");
        
        // 3. 注册“体检医生”：告诉 Updater 用哪个函数来检查设备状态
        diagnostic_updater_->add("Sensor Health", this, &DummySensorNode::check_sensor_status);

        // 为了模拟真实环境，我们创建一个定时器，每 0.5 秒改变一次温度
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500), 
            std::bind(&DummySensorNode::simulate_temperature, this));
            
        RCLCPP_INFO(this->get_logger(), "虚拟传感器已启动，诊断系统开始后台运行...");
    }

private:
    // 诊断更新器指针
    std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;
    rclcpp::TimerBase::SharedPtr timer_;
    double current_temp_ = 30.0;

    // 模拟温度变化的业务逻辑
    void simulate_temperature()
    {
        // 产生一个随机温度波动 (30度 到 90度)
        current_temp_ = 30.0 + (rand() % 60); 
    }

    // =========================================================
    // 🔥 核心回调函数：填写“体检报告单”
    // =========================================================
    void check_sensor_status(diagnostic_updater::DiagnosticStatusWrapper & stat)
    {
        // 就像我们之前讨论的，传进来的 stat 是一张全新的、空白的“体检表”。
        // 底层定时器（默认 1Hz）会自动调用这个函数。

        // 1. 业务逻辑判断
        if (current_temp_ > 80.0) {
            // 亮红灯 (ERROR)，并给出概要说明
            stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "温度过高！危险！");
        } else if (current_temp_ > 60.0) {
            // 亮黄灯 (WARN)
            stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "温度偏高，请注意。");
        } else {
            // 亮绿灯 (OK)
            stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "传感器运行正常。");
        }

        // 2. 填写附加详细指标（KeyValue数组）
        // 这些是给开发人员排查问题用的具体数据
        stat.add("Current Temperature (C)", std::to_string(current_temp_));
        stat.add("Max Limit (C)", "80.0");
        stat.add("Working Voltage (V)", "5.0"); // 可以塞入任何想监控的键值对

        // 函数执行到最后 (return) 时，Updater 会接管这个 stat，
        // 自动打包成 DiagnosticArray 并发布到 /diagnostics 话题。
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DummySensorNode>());
    rclcpp::shutdown();
    return 0;
}

🔍 第三步：代码底层逻辑拆解（回顾你的感悟）

如果您仔细看上面的代码，会发现它完美印证了我们之前探讨的生命周期与控制反转：

1. 谁在发话题？ 我们全程没有写一行 publisher->publish()。当执行 diagnostic_updater_->add(...) 时，Updater 就在后台悄悄建好了一个发布者和一个 1Hz 的定时器。
2. stat 是积累的还是覆盖的？ 由于发送动作是 Updater 控制的，每 1 秒钟，Updater 会拿出一张新表调用 check_sensor_status。执行完里面的 stat.summary() 和 stat.add() 后，表就被拿走发掉并销毁。下一次进来又是空表，所以 add 绝对不会造成数据越攒越多。

⚙️ 第四步：编译与运行

修改 my_sensor_diag/CMakeLists.txt，在文件末尾添加编译规则：

add_executable(dummy_sensor_node src/dummy_sensor_node.cpp)
ament_target_dependencies(dummy_sensor_node rclcpp diagnostic_updater)

install(TARGETS
  dummy_sensor_node
  DESTINATION lib/${PROJECT_NAME}
)

然后在工作空间根目录编译并运行：

colcon build --packages-select my_sensor_diag
source install/setup.bash
ros2 run my_sensor_diag dummy_sensor_node

📊 第五步：查看“体检报告”

节点跑起来后，我们在另一个终端看看它究竟往外发了什么。

方法 1：查看底层原始数据 (终端)

运行命令：

ros2 topic echo /diagnostics

您会看到终端里疯狂刷出类似这样的信封（结构体）：

header:
  stamp:
    sec: 1690000000
    nanosec: 123456789
  frame_id: ''
status:
- level: 2  # 这里 2 代表 ERROR, 0 代表 OK
  name: 'dummy_sensor_node: Sensor Health'
  message: '温度过高！危险！'
  hardware_id: 'TempSensor-001'
  values:
  - key: Current Temperature (C)
    value: '85.000000'
  - key: Max Limit (C)
    value: '80.0'
  - key: Working Voltage (V)
    value: '5.0'

这就是统一强制标准后的结果，不论是相机、IMU还是您自己写的温度传感器，最终输出的数据格式完全一致！

方法 2：上帝视角的监控面板 (rqt)

为了方便工程师，ROS 提供了一个专门看体检报告的图形化界面。打开终端输入：

ros2 run rqt_robot_monitor rqt_robot_monitor

此时弹出的窗口中，您会看到一个树状图。随着温度变化，您的节点会自动在 🟢（绿色，正常）、🟡（黄色，警告）和 🔴（红色，错误）之间切换，右侧还会显示您 add 进去的具体温度数值。

总结

ROS 2 Diagnostics 的极简之美就在于：开发者只需负责填表（定义规则、提供数据），框架负责收发（定时调用、消息组装）。 这种设计彻底解耦了“硬件业务逻辑”与“ROS 通信底层”，是编写工业级、车规级机器人代码的必修课。