一键启动系统

一键启动系统一、系统概述1.1 架构说明┌─────────────────────────────────────────────────────────────────┐ │ 一键启动系统架构 │ ├─────────────────────────────────────────────────────────────────┤ │ │ │ ┌────────────────────┐ │ │ │ diagnostic_system │ │ │ │ (系统监控器) │ │ │ │ │ │ │ │ • fork 启动模块 │ roslaunch │ │ │ • 监控进程状态 │ ──────────────────────────────────────┼─ Lidar Driver │ │ • 汇总诊断信息 │ │ │ │ • 显示状态表格 │ ──────────────────────────────────────┼─ Localization │ │ │ │ │ │ • 监听 /diagnostics │ ─────────────────────────────────────┼─ Perception │ │ • 心跳超时检测 │ (1Hz DiagnosticArray) │ │ └────────────────────┘ │ │ │ └─────────────────────────────────────────────────────────────────┘1.2 数据流业务模块 system_monitor ────────────────────────────────────────────────────────────── ▲ │ /diagnostics (1Hz) │ ┌───────────────────────┐ │ │ 业务回调 (任意频率) │ │ │ 只写 health_ 状态 │──────────┘ │ 不发 Diagnostic │ └───────────────────────┘ ┌───────────────────────┐ │ │ 1Hz Timer │──────────▶ 汇总 → 显示 → 故障处理 │ 发布 DiagnosticArray │ └───────────────────────┘二、Diagnostic 数据规范2.1 Topic 规范属性值Topic/diagnostics消息类型diagnostic_msgs/DiagnosticArray发布频率1Hz全系统统一2.2 消息字段diagnostic_msgs::DiagnosticStatus st;st.name模块名;// 必须与 modules.yaml 一致st.hardware_id硬件标识;// 具体硬件实例st.level0;// 0OK, 1WARN, 2ERRORst.message状态描述;// 人类可读st.values[{key,value}];// 结构化信息2.3 状态级别定义level名称含义0OK正常运行1WARN警告有异常但可工作2ERROR错误功能受损2.4 错误代码规范格式: XYYY X - 模块类别 YYY - 具体错误编号 类别映射: 1XXX - 驱动层 (Lidar, Camera, Radar) 2XXX - 通信层 (CAN, DataLink) 3XXX - 定位层 (Localization) 4XXX - 感知层 (Perception) 5XXX - 导航层 (Navigation, ObstacleAvoidance) 6XXX - 控制层 (Control) 7XXX - 工作装置 (Shovel, WorkingDevice) 8XXX - 计算层 (VolumeCalculation) 9XXX - 状态机 (StateMachine)三、模块改造指南3.1 改造流程图开始改造 │ ▼ ┌───────────────────────────────────────┐ │ Step 1: 添加健康状态缓存 │ │ struct ModuleHealth { │ │ atomicint level; │ │ string message; │ │ string error_code; │ │ ros::Time last_update; │ │ } health_; │ └───────────────────────────────────────┘ │ ▼ ┌───────────────────────────────────────┐ │ Step 2: 初始化 Diagnostic │ │ • advertise DiagnosticArray │ │ • createTimer 1Hz │ │ • init last_update │ └───────────────────────────────────────┘ │ ▼ ┌───────────────────────────────────────┐ │ Step 3: 修改业务回调 │ │ • 原有逻辑保持不变 │ │ • 末尾只写 health_ │ │ • 不发 Diagnostic │ └───────────────────────────────────────┘ │ ▼ ┌───────────────────────────────────────┐ │ Step 4: 实现 publishDiagnostic │ │ • 读取 health_ │ │ • 超时检测 │ │ • 发布 DiagnosticArray │ └───────────────────────────────────────┘ │ ▼ ┌───────────────────────────────────────┐ │ Step 5: 更新 modules.yaml │ │ • 添加 name │ │ • 添加 launch_cmd │ │ • 添加 missing_error_code │ └───────────────────────────────────────┘ │ ▼ 完成3.2 代码模板3.2.1 头文件声明#includeros/ros.h#includediagnostic_msgs/DiagnosticStatus.h#includediagnostic_msgs/DiagnosticArray.h#includeatomicclassYourModule{private:// 健康状态缓存 // 唯一允许业务回调修改的结构体structModuleHealth{std::atomicintlevel{0};// 0OK, 1WARN, 2ERRORstd::string message{OK};// 状态描述std::string error_code{0};// 错误代码ros::Time last_update;// 心跳时间戳}health_;// ROS 成员ros::NodeHandle nh_;ros::Subscriber sub_;// 业务订阅ros::Publisher diag_pub_;// Diagnostic 发布ros::Timer diag_timer_;// 1Hz Timer// 回调函数voidcallback(constYourMsgType::ConstPtrmsg);voidpublishDiagnostic(constros::TimerEvent);public:YourModule();};3.2.2 构造函数YourModule::YourModule(){// 1. 订阅业务 topic频率任意sub_nh_.subscribe(/your_topic,10,YourModule::callback,this);// 2. 发布 DiagnosticArray1Hz 由 Timer 控制diag_pub_nh_.advertisediagnostic_msgs::DiagnosticArray(/diagnostics,10);// 3. 创建 1Hz Timerdiag_timer_nh_.createTimer(ros::Duration(1.0),YourModule::publishDiagnostic,this);// 4. 初始化 last_update防止启动时误报超时health_.last_updateros::Time::now();}3.2.3 业务回调核心改动voidYourModule::callback(constYourMsgType::ConstPtrmsg){// 原有业务逻辑保持不动boolhas_errorfalse;// ... 你的原有判断逻辑 ...if(msg-ranges.empty()){has_errortrue;}// ... 更多业务判断 ...// 增量改动只写健康状态 if(has_error){health_.level.store(2);// ERROR 级别health_.messageData invalid;// 给用户看的描述health_.error_code3XXX;// 按规范填写}else{health_.level.store(0);// OKhealth_.messageOK;health_.error_code0;}health_.last_updateros::Time::now();// ⚠️ 禁止在这里发布 Diagnostic// ⚠️ 禁止有任何诊断相关的发布}3.2.4 诊断发布Timer 回调voidYourModule::publishDiagnostic(constros::TimerEvent){diagnostic_msgs::DiagnosticStatus st;// 必须与 modules.yaml 中的 name 完全一致st.nameYourModuleName;st.hardware_idHardware_ID_01;// 心跳超时检测核心功能ros::Duration dtros::Time::now()-health_.last_update;if(dt.toSec()2.0){// 超时 → 模块可能崩溃或卡死st.leveldiagnostic_msgs::DiagnosticStatus::ERROR;st.messageNo data timeout (2s);st.values.clear();st.values.push_back({make_pair(error_code,TIMEOUT)});}else{// 正常 → 使用业务回调写入的状态st.levelhealth_.level.load();st.messagehealth_.message;st.values.clear();st.values.push_back({make_pair(error_code,health_.error_code)});}// 发布 DiagnosticArraydiagnostic_msgs::DiagnosticArray arr;arr.header.stampros::Time::now();arr.status.push_back(st);diag_pub_.publish(arr);}3.3 modules.yaml 配置在src/diagnostic_system/config/modules.yaml中添加modules:# 你的模块 -name:YourModuleName# 必须与 st.name 完全一致launch_cmd:roslaunch your_package your_launch.launchmissing_error_code:3XXX# 错误代码见规范四、完整示例 - Lidar 模块#includeros/ros.h#includesensor_msgs/LaserScan.h#includediagnostic_msgs/DiagnosticStatus.h#includediagnostic_msgs/DiagnosticArray.h#includeatomicclassLidarModule{private:structModuleHealth{std::atomicintlevel{0};std::string message{OK};std::string error_code{0};ros::Time last_update;}health_;ros::NodeHandle nh_;ros::Subscriber scan_sub_;ros::Publisher diag_pub_;ros::Timer diag_timer_;voidscanCallback(constsensor_msgs::LaserScan::ConstPtrmsg){boolhas_errorfalse;if(msg-ranges.empty()){has_errortrue;}if(msg-angle_increment0){has_errortrue;}if(has_error){health_.level.store(2);health_.messageInvalid scan data;health_.error_code1011;}else{health_.level.store(0);health_.messageOK;health_.error_code0;}health_.last_updateros::Time::now();}voidpublishDiagnostic(constros::TimerEvent){diagnostic_msgs::DiagnosticStatus st;st.nameLidar Driver;st.hardware_idHokuyo_UTM_01;ros::Duration dtros::Time::now()-health_.last_update;if(dt.toSec()2.0){st.leveldiagnostic_msgs::DiagnosticStatus::ERROR;st.messageNo data timeout (2s);st.values.clear();st.values.push_back({make_pair(error_code,TIMEOUT)});}else{st.levelhealth_.level.load();st.messagehealth_.message;st.values.clear();st.values.push_back({make_pair(error_code,health_.error_code)});}diagnostic_msgs::DiagnosticArray arr;arr.header.stampros::Time::now();arr.status.push_back(st);diag_pub_.publish(arr);}public:LidarModule(){scan_sub_nh_.subscribe(/scan,10,LidarModule::scanCallback,this);diag_pub_nh_.advertisediagnostic_msgs::DiagnosticArray(/diagnostics,10);diag_timer_nh_.createTimer(ros::Duration(1.0),LidarModule::publishDiagnostic,this);health_.last_updateros::Time::now();}};intmain(intargc,char**argv){ros::init(argc,argv,lidar_node);LidarModule node;ros::spin();return0;}五、错误代码速查模块类别码错误代码示例含义Lidar Driver1XXX1010进程缺失Lidar Driver1XXX1011数据无效Lidar Driver1XXX1012雷达掉线Camera Driver1XXX1020进程缺失Camera Driver1XXX1021图像采集失败Radar Driver1XXX1030进程缺失CAN to ROS2XXX2010进程缺失CAN to ROS2XXX2011CAN 总线错误DataLink2XXX2020进程缺失DataLink2XXX2021通信超时Localization3XXX3010进程缺失Localization3XXX3011定位丢失Perception4XXX4010进程缺失Perception4XXX4011障碍物检测失败Navigation5XXX5010进程缺失Navigation5XXX5011路径规划失败Obstacle Avoidance5XXX5020进程缺失Obstacle Avoidance5XXX5021避障失效Control6XXX6010进程缺失Control6XXX6011控制指令超时Shovel7XXX7010进程缺失Shovel7XXX7011铲料异常Working Device7XXX7020进程缺失Working Device7XXX7021装置故障Volume Calculation8XXX8010进程缺失State Machine9XXX9010进程缺失State Machine9XXX9011状态转换异常系统XXXXTIMEOUT心跳超时所有模块通用六、禁止事项┌────────────────────────────────────────────────────────┐ │ 禁止行为清单 │ ├────────────────────────────────────────────────────────┤ │ │ │ ❌ 在业务回调中直接发布 Diagnostic │ │ → 回调只能写 health_不能 publish │ │ │ │ ❌ 在多个地方发布同一个模块的 Diagnostic │ │ → 只能在一个 Timer 回调中发布 │ │ │ │ ❌ Diagnostic 发布频率超过 1Hz │ │ → 必须使用 1Hz Timer 控制 │ │ │ │ ❌ 模块 name 与 modules.yaml 不一致 │ │ → 必须严格匹配否则 system_monitor 无法识别 │ │ │ │ ❌ 错误代码不符合规范 │ │ → 必须使用 XYYY 格式按类别填写 │ │ │ └────────────────────────────────────────────────────────┘七、调试命令# 查看诊断消息rostopicecho/diagnostics# 查看诊断消息频率应为 1Hzrostopic hz /diagnostics# 可视化监控工具rosrun rqt_runtime_monitor rqt_runtime_monitor# 查看所有诊断节点rosnode list|grepdiagnostic八、改造检查清单添加ModuleHealth结构体添加diag_pub_和diag_timer_成员在构造函数中初始化 Diagnostic Publisher在构造函数中创建 1Hz Timer在构造函数中初始化last_update修改业务回调只写health_实现publishDiagnostic函数在modules.yaml中添加模块配置测试验证 Diagnostic 消息正常发布九、常见问题Q1: 为什么不在回调中直接发布 DiagnosticA: 高频回调会导致总线拥塞。统一 1Hz Timer 可以减少负载、便于管理。Q2: 超时时间为什么是 2 秒A: 2 秒是 1Hz 的 2 倍用于容忍抖动。超过 2 秒基本可确认模块崩溃。Q3: 如果模块有多个 topic 怎么办A: 取所有 topic 的最小last_update或分别监控各 topic 状态。Q4: 如何区分模块崩溃和只是没数据A: 心跳超时统一报告TIMEOUT具体错误由业务回调的状态决定。