如何从ROS2的/diagnostics话题中获取并处理多个DiagnosticArray？

首先要明确：你看到的---是ROS消息的分隔符，代表独立的DiagnosticArray消息实例，而非单个消息内包含多个数组。每个节点会单独向/diagnostics话题发布自己的DiagnosticArray，话题上的消息流是多个独立的DiagnosticArray依次推送，而非一个聚合后的大数组。

下面是具体实现思路：

1. 订阅话题并处理单条DiagnosticArray消息 #

在C++节点中，直接订阅diagnostic_msgs::msg::DiagnosticArray类型的/diagnostics话题。每次回调函数触发时，参数就是某一个节点发来的完整诊断数组，你可以直接访问该数组的header（用来区分不同节点，比如frame_id字段）和status（该节点的所有诊断状态列表）。

示例代码框架：

#include "rclcpp/rclcpp.hpp"
#include "diagnostic_msgs/msg/diagnostic_array.hpp"
#include <map>
#include <vector>

class ErrorCollector : public rclcpp::Node
{
public:
  ErrorCollector() : Node("error_collector")
  {
    // 订阅/diagnostics话题
    subscription_ = this->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
      "/diagnostics", 10, std::bind(&ErrorCollector::diagnostic_callback, this, std::placeholders::_1));
    
    // 定时器：定期将收集的错误通过CAN发送
    timer_ = this->create_wall_timer(
      std::chrono::seconds(1), std::bind(&ErrorCollector::send_can_data, this));
  }

private:
  void diagnostic_callback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg)
  {
    // 用frame_id区分不同的发布节点
    std::string node_id = msg->header.frame_id;
    // 更新该节点的最新诊断状态
    node_diagnostics_[node_id] = msg->status;
  }

  void send_can_data()
  {
    // 遍历所有节点的诊断状态，解析错误码
    for (const auto& [node_id, status_list] : node_diagnostics_) {
      for (const auto& status : status_list) {
        // 仅处理错误级别（level=2对应ERROR）
        if (status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) {
          // 根据message或name映射到对应的CAN错误码
          uint16_t error_code = map_to_can_code(status.name, status.message);
          // 替换为你的CAN发送逻辑
          // send_can_frame(error_code, node_id);
          RCLCPP_INFO(this->get_logger(), "Node: %s, Error: %s, Code: %d", node_id.c_str(), status.message.c_str(), error_code);
        }
      }
    }
  }

  // 自定义函数：将诊断信息映射为CAN错误码
  uint16_t map_to_can_code(const std::string& status_name, const std::string& status_msg)
  {
    // 根据需求编写映射逻辑
    if (status_name == "System" && status_msg == "Invalid Config") {
      return 0x0001;
    } else if (status_name == "Front Sensor" && status_msg == "Screen polluted") {
      return 0x0002;
    }
    return 0xFFFF; // 默认未知错误码
  }

  rclcpp::Subscription<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr subscription_;
  rclcpp::TimerBase::SharedPtr timer_;
  // 存储每个节点的最新诊断状态，key为节点标识（frame_id）
  std::map<std::string, std::vector<diagnostic_msgs::msg::DiagnosticStatus>> node_diagnostics_;
};

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

2. 持久化管理多节点诊断数据 #

通过std::map（或其他容器）将每个节点的诊断状态持久化存储，键可以用header.frame_id（示例中用来区分节点的字段），值为该节点的status列表。每次回调时更新对应节点的状态，确保存储的是最新的诊断信息。

3. 错误解析与CAN发送 #

在定时器回调（或指定的发送时机）中，遍历存储的所有节点诊断数据，筛选出错误级别（level=2）的状态，根据name和message映射为所需的CAN错误码，再执行CAN总线发送逻辑。

内容的提问来源于stack exchange，提问作者Boris