Skip to content

Commit 43c16f8

Browse files
committed
refactor(demos/manymove_industrial): drop redundant manifest entries
The gateway runs in hybrid discovery mode, so pure ROS-side components (xarm7-arm, ufactory-driver, move-group, object-manager, action-server, gateway, plus their apps) are picked up at runtime from topic/node introspection - no manifest needed. Keep only what the runtime cannot infer: - PLC-side components (no ROS topics: openplc, photoeyes, conveyor-motor) - script-bound logical components (conveyor-line, manymove-planning) - apps the smoke test asserts on (bt-client-xarm7, fault-manager-app) - 'line' / 'planning' / 'diagnostics' areas for namespace routing Drop the manipulation area, the entire functions: section, the inject-soft-fault script (no smoke coverage, narrative overlap with inject-collision). Skip the docker logs probe gracefully when the host docker socket is not reachable. Manifest goes from 217 to 98 lines. Local smoke 11/11 pass against the slim manifest.
1 parent 1ce6015 commit 43c16f8

90 files changed

Lines changed: 4269 additions & 170 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

CLAUDE.md

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
# selfpatch_demos
2+
3+
Docker-based demo scenarios showcasing ros2_medkit in action. 3 demos with increasing complexity.
4+
5+
## Demos
6+
7+
| Demo | Complexity | GPU | Startup | Key Tech |
8+
|------|-----------|-----|---------|----------|
9+
| `sensor_diagnostics` | Low | No | ~5s | Simulated sensors, dual fault reporting |
10+
| `turtlebot3_integration` | Medium | Optional | ~60s | Nav2, Gazebo Harmonic, SOVD manifest, rosbag |
11+
| `moveit_pick_place` | High | Recommended | ~90s | MoveIt 2 Panda arm, Gazebo, fault snapshots |
12+
13+
## Common Pattern
14+
15+
All demos expose:
16+
- REST gateway at `:8080`
17+
- Web UI at `:3000`
18+
- Fault injection scripts (`inject-*.sh`, `restore-normal.sh`)
19+
- Run/stop scripts (`run-demo.sh`, `stop-demo.sh`, `check-demo.sh`)
20+
21+
## Running
22+
23+
```bash
24+
cd demos/<demo_name>
25+
./run-demo.sh # docker compose up
26+
./check-demo.sh # verify services
27+
./inject-*.sh # trigger faults
28+
./restore-normal.sh # clear faults
29+
./stop-demo.sh # docker compose down
30+
```
31+
32+
## Relations
33+
34+
- **Depends on**: ros2_medkit (core), sovd_web_ui (web frontend)
35+
- **Monorepo**: submodule in platform/
Lines changed: 199 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,199 @@
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()

demos/manymove_industrial/README.md

Lines changed: 0 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -69,11 +69,6 @@ at the well-known gateway scripts dir and runnable from the web UI or via
6969
The next `MoveManipulatorAction::onStart` tick observes it, emits
7070
`MANYMOVE_PLANNER_COLLISION_DETECTED` (ERROR) and returns FAILURE.
7171
This is a real BT fault, not a synthesised report.
72-
- `inject-soft-fault` - drops a thin collision wall in the planning scene
73-
near the pick zone; planning fails ~30% of attempts and the BT emits
74-
`MANYMOVE_PLANNER_RETRY_ATTEMPT` (WARN). The reporter's LocalFilter
75-
(threshold=3, window=10s) throttles those locally and only forwards once
76-
the threshold is crossed - this is the soft-fault narrative.
7772
- `restore-normal` - clears `collision_detected`, `stop_execution` and
7873
triggers `reset` + `start` on the blackboard so the BT picks up cleanly.
7974

0 commit comments

Comments
 (0)