|
| 1 | +#!/usr/bin/env python3 |
| 2 | +"""M3 Pro Diagnostic Node - monitors robot sensors and publishes /diagnostics. |
| 3 | +
|
| 4 | +Read-only node: subscribes to robot topics, evaluates health, publishes |
| 5 | +diagnostic_msgs/DiagnosticArray. Does NOT publish to any control topics. |
| 6 | +
|
| 7 | +Used with ros2_medkit diagnostic_bridge to generate SOVD faults from |
| 8 | +ROS 2 diagnostic messages. |
| 9 | +
|
| 10 | +Usage: |
| 11 | + source /opt/ros/humble/setup.bash |
| 12 | + export ROS_DOMAIN_ID=30 |
| 13 | + python3 m3pro_diagnostics.py |
| 14 | +""" |
| 15 | + |
| 16 | +import math |
| 17 | +import time |
| 18 | + |
| 19 | +import rclpy |
| 20 | +from rclpy.node import Node |
| 21 | +from rclpy.qos import qos_profile_sensor_data |
| 22 | + |
| 23 | +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue |
| 24 | +from sensor_msgs.msg import Imu, LaserScan |
| 25 | +from std_msgs.msg import Float32 |
| 26 | + |
| 27 | + |
| 28 | +class M3ProDiagnostics(Node): |
| 29 | + """Diagnostic monitor for ROSMASTER M3 Pro robot.""" |
| 30 | + |
| 31 | + # Thresholds |
| 32 | + BATTERY_WARN_V = 10.5 |
| 33 | + BATTERY_ERROR_V = 9.9 |
| 34 | + LIDAR_MIN_VALID_RANGES = 10 |
| 35 | + IMU_GRAVITY_MIN = 8.0 |
| 36 | + IMU_GRAVITY_MAX = 12.0 |
| 37 | + |
| 38 | + def __init__(self): |
| 39 | + super().__init__('m3pro_diagnostics') |
| 40 | + |
| 41 | + # Latest values |
| 42 | + self._battery_v = None |
| 43 | + self._imu_accel_z = None |
| 44 | + self._scan0_valid_count = None |
| 45 | + self._scan1_valid_count = None |
| 46 | + self._last_battery_time = 0.0 |
| 47 | + self._last_imu_time = 0.0 |
| 48 | + self._last_scan0_time = 0.0 |
| 49 | + self._last_scan1_time = 0.0 |
| 50 | + |
| 51 | + # Test fault control |
| 52 | + self._test_fault_active = False |
| 53 | + self._test_fault_sent = False |
| 54 | + self._test_fault_cleared = False |
| 55 | + self._startup_time = time.time() |
| 56 | + |
| 57 | + # Subscribers (read-only, sensor QoS) |
| 58 | + self.create_subscription( |
| 59 | + Float32, '/battery', self._battery_cb, qos_profile_sensor_data) |
| 60 | + self.create_subscription( |
| 61 | + Imu, '/imu/data_raw', self._imu_cb, qos_profile_sensor_data) |
| 62 | + self.create_subscription( |
| 63 | + LaserScan, '/scan0', self._scan0_cb, qos_profile_sensor_data) |
| 64 | + self.create_subscription( |
| 65 | + LaserScan, '/scan1', self._scan1_cb, qos_profile_sensor_data) |
| 66 | + |
| 67 | + # Publisher |
| 68 | + self._diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics', 10) |
| 69 | + |
| 70 | + # Publish diagnostics at 1 Hz |
| 71 | + self.create_timer(1.0, self._publish_diagnostics) |
| 72 | + |
| 73 | + self.get_logger().info('M3 Pro Diagnostics started') |
| 74 | + self.get_logger().info( |
| 75 | + f'Thresholds: battery WARN<{self.BATTERY_WARN_V}V ERROR<{self.BATTERY_ERROR_V}V, ' |
| 76 | + f'LiDAR min_ranges={self.LIDAR_MIN_VALID_RANGES}, ' |
| 77 | + f'IMU gravity=[{self.IMU_GRAVITY_MIN}, {self.IMU_GRAVITY_MAX}] m/s^2') |
| 78 | + |
| 79 | + # -- Callbacks (read-only) -- |
| 80 | + |
| 81 | + def _battery_cb(self, msg): |
| 82 | + self._battery_v = msg.data |
| 83 | + self._last_battery_time = time.time() |
| 84 | + |
| 85 | + def _imu_cb(self, msg): |
| 86 | + self._imu_accel_z = msg.linear_acceleration.z |
| 87 | + self._last_imu_time = time.time() |
| 88 | + |
| 89 | + def _scan0_cb(self, msg): |
| 90 | + self._scan0_valid_count = sum( |
| 91 | + 1 for r in msg.ranges |
| 92 | + if msg.range_min <= r <= msg.range_max and math.isfinite(r)) |
| 93 | + self._last_scan0_time = time.time() |
| 94 | + |
| 95 | + def _scan1_cb(self, msg): |
| 96 | + self._scan1_valid_count = sum( |
| 97 | + 1 for r in msg.ranges |
| 98 | + if msg.range_min <= r <= msg.range_max and math.isfinite(r)) |
| 99 | + self._last_scan1_time = time.time() |
| 100 | + |
| 101 | + # -- Diagnostic evaluation -- |
| 102 | + |
| 103 | + def _publish_diagnostics(self): |
| 104 | + now = time.time() |
| 105 | + msg = DiagnosticArray() |
| 106 | + msg.header.stamp = self.get_clock().now().to_msg() |
| 107 | + |
| 108 | + msg.status.append(self._check_battery(now)) |
| 109 | + msg.status.append(self._check_imu(now)) |
| 110 | + msg.status.append(self._check_lidar( |
| 111 | + now, 'scan0', self._scan0_valid_count, self._last_scan0_time)) |
| 112 | + msg.status.append(self._check_lidar( |
| 113 | + now, 'scan1', self._scan1_valid_count, self._last_scan1_time)) |
| 114 | + |
| 115 | + self._diag_pub.publish(msg) |
| 116 | + |
| 117 | + def _check_battery(self, now): |
| 118 | + status = DiagnosticStatus() |
| 119 | + status.name = 'Battery' |
| 120 | + status.hardware_id = 'M3Pro/battery' |
| 121 | + |
| 122 | + if self._battery_v is None or (now - self._last_battery_time) > 5.0: |
| 123 | + status.level = DiagnosticStatus.STALE |
| 124 | + status.message = 'No battery data received' |
| 125 | + return status |
| 126 | + |
| 127 | + v = self._battery_v |
| 128 | + status.values.append(KeyValue(key='voltage', value=f'{v:.2f}')) |
| 129 | + |
| 130 | + if v < self.BATTERY_ERROR_V: |
| 131 | + status.level = DiagnosticStatus.ERROR |
| 132 | + status.message = f'CRITICAL: {v:.1f}V (below {self.BATTERY_ERROR_V}V)' |
| 133 | + elif v < self.BATTERY_WARN_V: |
| 134 | + status.level = DiagnosticStatus.WARN |
| 135 | + status.message = f'Low battery: {v:.1f}V (below {self.BATTERY_WARN_V}V)' |
| 136 | + else: |
| 137 | + status.level = DiagnosticStatus.OK |
| 138 | + status.message = f'{v:.1f}V' |
| 139 | + |
| 140 | + return status |
| 141 | + |
| 142 | + def _check_imu(self, now): |
| 143 | + status = DiagnosticStatus() |
| 144 | + status.name = 'IMU' |
| 145 | + status.hardware_id = 'M3Pro/imu' |
| 146 | + |
| 147 | + if self._imu_accel_z is None or (now - self._last_imu_time) > 5.0: |
| 148 | + status.level = DiagnosticStatus.STALE |
| 149 | + status.message = 'No IMU data received' |
| 150 | + return status |
| 151 | + |
| 152 | + az = self._imu_accel_z |
| 153 | + status.values.append(KeyValue(key='accel_z', value=f'{az:.3f}')) |
| 154 | + |
| 155 | + if az < self.IMU_GRAVITY_MIN or az > self.IMU_GRAVITY_MAX: |
| 156 | + status.level = DiagnosticStatus.ERROR |
| 157 | + status.message = f'Abnormal gravity: {az:.2f} m/s^2' |
| 158 | + else: |
| 159 | + status.level = DiagnosticStatus.OK |
| 160 | + status.message = f'accel_z={az:.2f} m/s^2' |
| 161 | + |
| 162 | + return status |
| 163 | + |
| 164 | + def _check_lidar(self, now, name, valid_count, last_time): |
| 165 | + status = DiagnosticStatus() |
| 166 | + status.name = f'LiDAR {name}' |
| 167 | + status.hardware_id = f'M3Pro/{name}' |
| 168 | + |
| 169 | + if valid_count is None or (now - last_time) > 5.0: |
| 170 | + status.level = DiagnosticStatus.STALE |
| 171 | + status.message = f'No {name} data received' |
| 172 | + return status |
| 173 | + |
| 174 | + status.values.append(KeyValue(key='valid_ranges', value=str(valid_count))) |
| 175 | + |
| 176 | + if valid_count < self.LIDAR_MIN_VALID_RANGES: |
| 177 | + status.level = DiagnosticStatus.ERROR |
| 178 | + status.message = f'LiDAR blocked or failed: {valid_count} valid ranges' |
| 179 | + else: |
| 180 | + status.level = DiagnosticStatus.OK |
| 181 | + status.message = f'{valid_count} valid ranges' |
| 182 | + |
| 183 | + return status |
| 184 | + |
| 185 | + |
| 186 | +def main(): |
| 187 | + rclpy.init() |
| 188 | + node = M3ProDiagnostics() |
| 189 | + try: |
| 190 | + rclpy.spin(node) |
| 191 | + except KeyboardInterrupt: |
| 192 | + pass |
| 193 | + finally: |
| 194 | + node.destroy_node() |
| 195 | + rclpy.shutdown() |
| 196 | + |
| 197 | + |
| 198 | +if __name__ == '__main__': |
| 199 | + main() |
0 commit comments