Add waybionic_sensors IMU package with correct raw sensor semantics and diagnostics

robostack.yaml

@@ -8,6 +8,7 @@ channels:
 dependencies:
   - ros-jazzy-ros-base
   - ros-jazzy-rviz2
+  - ros-jazzy-rviz-imu-plugin
   - ros-jazzy-xacro
   - ros-jazzy-joint-state-publisher-gui
   - colcon-common-extensions
@@ -16,4 +17,7 @@ dependencies:
   - pkg-config
   - make
   - ninja
-  - pytest <9
+  - pytest <9
+  # colcon still relies on the legacy `develop` and `test` commands removed by
+  # newer setuptools. Keep Python ROS packages editable and testable on macOS.
+  - setuptools <72

waybionic_sensors/README.md

@@ -0,0 +1,221 @@
+# waybionic_sensors
+
+IMU publishing and sensor health for the WayBionic ground station.
+
+The package publishes `sensor_msgs/msg/Imu` and reports IMU health on
+`/diagnostics` so the merged `waybionic_rviz_plugins` DiagnosticsPanel can show a
+live `imu.heartbeat`. It runs entirely on a mock source today, and defines the
+boundary a real driver will plug into once electrical confirms the sensor.
+
+## Quickstart
+
+```bash
+source /opt/ros/jazzy/setup.bash
+cd <workspace>
+rosdep install --from-paths src --ignore-src -y
+colcon build --packages-select waybionic_sensors --symlink-install
+source install/setup.bash
+
+ros2 launch waybionic_sensors imu_publisher.launch.py
+```
+
+Check the output:
+
+```bash
+ros2 topic hz /waybionic/imu/data_raw
+ros2 topic echo /waybionic/imu/data_raw --once
+ros2 topic echo /diagnostics --once
+```
+
+## RViz walkthrough
+
+```bash
+ros2 launch waybionic_sensors imu_demo.launch.py
+```
+
+This enables the synthetic orientation and the rotating demo TF so there is
+something to look at. Both are off in `imu_publisher.launch.py`.
+
+The demo config uses `rviz_imu_plugin/Imu` (Jazzy does not ship
+`rviz_default_plugins/Imu`) and points the orientation display at
+`/waybionic/imu/data_demo`, not `/waybionic/imu/data_raw`. `rosdep install`
+from this package pulls `rviz_imu_plugin` automatically.
+
+## Heartbeat in the diagnostics panel
+
+```bash
+# Terminal 1
+ros2 launch waybionic_sensors imu_publisher.launch.py
+
+# Terminal 2
+ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false
+```
+
+The panel shows `imu.heartbeat` as OK with an age in seconds. To watch it go
+stale without unplugging anything:
+
+```bash
+ros2 launch waybionic_sensors imu_publisher.launch.py mock_stall_after_sec:=5.0
+```
+
+The mock stops after five seconds. Once the sample age passes
+`stale_timeout_sec`, `imu.heartbeat`, `imu.rate`, `imu.angular_velocity`, and
+`imu.linear_acceleration` all report STALE. The last gyro and accel magnitudes
+remain visible so the panel does not look like the sensor is still healthy.
+
+## Raw vs demo data, in plain English
+
+An IMU is a small sensor that measures two things:
+
+- **How fast it is spinning** (angular velocity, rad/s)
+- **How it is accelerating**, including gravity (linear acceleration, m/s^2)
+
+It does **not** automatically know which way the robot is facing. Estimating
+that facing direction is a separate step called fusion. Until a real fusion
+source exists, this package keeps the two kinds of data on different topics so
+nobody mixes them up:
+
+| Topic | What it is | Default | Who should use it |
+|-------|------------|---------|-------------------|
+| `/waybionic/imu/data_raw` | Gyro + accelerometer measurements only. No facing direction. | Always on | Downstream code, diagnostics, a future fusion node |
+| `/waybionic/imu/data_demo` | The same measurements **plus a made-up facing direction** so RViz can show a spinning box | Off, unless you run `imu_demo.launch.py` | Humans looking at RViz. Never control or localisation |
+
+The RViz IMU display subscribes to `data_demo`, because that is the only topic
+with an orientation to draw. `data_raw` marks orientation as unavailable
+(`orientation_covariance[0] = -1`).
+
+If we do not yet know how noisy the sensor is, raw gyro and accel covariance
+stays all zeros. In ROS that means **unknown**, not "perfectly certain." Fake
+noise numbers stay on the demo topic only, until electrical supplies a
+datasheet or calibration value.
+
+Full details, units, covariance conventions, and the parameter list are in
+`docs/IMU_CONTRACT.md`.
+
+## Package layout
+
+```text
+waybionic_sensors/
+  waybionic_sensors/
+    imu_reading.py         # Hardware-independent sample type: the boundary contract
+    mock_source.py         # Synthetic sample generation, no ROS types
+    hardware_reader.py     # Driver interface plus an unimplemented stub
+    imu_messages.py        # sensor_msgs/Imu and TF construction, covariance rules
+    imu_diagnostics.py     # DiagnosticArray construction, heartbeat and freshness
+    imu_publisher_node.py  # ROS node that only wires the above together
+  launch/
+    imu_publisher.launch.py
+    imu_demo.launch.py
+  config/
+    imu_demo.rviz
+  docs/
+    IMU_CONTRACT.md
+    HARDWARE_INTERFACE.md
+    PR_NOTES.md
+  test/
+```
+
+Each stage is separately testable: sample generation, message construction,
+diagnostics, and the hardware boundary have no dependency on one another.
+
+## Hardware status
+
+No physical IMU driver exists yet. `hardware_reader.py` defines the interface
+and deliberately implements no serial protocol. Sensor model, transport,
+mounting, calibration, and noise values stay pending until Electrical answers
+the questions in `docs/HARDWARE_INTERFACE.md`.
+
+Running with `use_mock:=false` is still meaningful: no samples are published and
+`imu.heartbeat` reports STALE, which is what a missing sensor should look like.
+
+**There is currently no physical IMU driver.** Do not treat mock or demo output
+as a real sensor.
+
+## Runtime handoff
+
+For the integration runner (Malik). Source the workspace overlay first.
+There is no physical IMU driver.
+
+Last full verification of this closeout:
+
+- Commit: `e317df4`
+- Environment: Ubuntu 24.04.4 LTS / ROS 2 Jazzy / Python 3.12.3 / WSL2
+- Install: `rosdep install --from-paths . --ignore-src -y` (no `-r`, no skip keys) → all required rosdeps installed; `ros-jazzy-rviz-imu-plugin` present
+- Tests: `colcon test --packages-select waybionic_sensors` → 96 passed at `e317df4` (runtime also verified there). Docs-guard tests on this closeout raise the IMU suite to 98.
+- Shutdown: Ctrl+C on the launch process. The node calls `stop()` on the reader, then destroys itself. Mock and unconfigured live mode have no extra processes.
+
+### Raw
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+ros2 launch waybionic_sensors imu_publisher.launch.py
+```
+
+| | |
+|--|--|
+| Topics | `/waybionic/imu/data_raw` (`sensor_msgs/msg/Imu`), `/diagnostics` (`diagnostic_msgs/msg/DiagnosticArray`). `/waybionic/imu/data_demo` is absent. |
+| Frames | `header.frame_id` = `imu_link`. No demo TF. |
+| Orientation | Unavailable: identity quaternion placeholder, `orientation_covariance[0] = -1`. |
+| Covariance | Gyro and accel 3x3 all zeros (ROS unknown). No datasheet stddev is configured. |
+| Diagnostics | `imu.heartbeat`, `imu.rate`, `imu.angular_velocity`, `imu.linear_acceleration` all OK while streaming. |
+| Check | `ros2 topic echo /waybionic/imu/data_raw --once` |
+
+### Demo
+
+```bash
+ros2 launch waybionic_sensors imu_demo.launch.py
+```
+
+Headless (no GUI): add `launch_rviz:=false`.
+
+| | |
+|--|--|
+| Topics | Raw as above, plus `/waybionic/imu/data_demo` (`sensor_msgs/msg/Imu`) and `/tf`. |
+| Frames | IMU messages: `imu_link`. Demo TF parent: `base_link`. RViz fixed frame: `base_link`. |
+| RViz | Same launch starts `rviz2 -d` `share/waybionic_sensors/config/imu_demo.rviz`. Display class `rviz_imu_plugin/Imu` named "IMU orientation (demo)", topic `/waybionic/imu/data_demo`. Expect a red box / axes wobbling on the grid. Raw semantics stay unchanged (`orientation_covariance[0] = -1` on `data_raw`). |
+| Diagnostics | Same four rows, OK while the mock streams. |
+
+### Stall / mock
+
+```bash
+ros2 launch waybionic_sensors imu_publisher.launch.py mock_stall_after_sec:=5.0
+```
+
+Faster bench check: `mock_stall_after_sec:=1.0 stale_timeout_sec:=0.5`.
+
+| | |
+|--|--|
+| Publication | Mock stops producing samples after the delay and stays stopped (latched). Last gyro/accel magnitudes remain on the diagnostic rows. |
+| Diagnostics | After `stale_timeout_sec`, all four rows go STALE (level 3). Heartbeat age is `now - last sample stamp` (source freshness, not a rewritten clock). |
+| Recovery | Stop the launch (Ctrl+C) and start the default publisher again. All four rows return to OK and `data_raw` resumes. Restart is required; the latch does not un-stall in-process. |
+
+Unconfigured live mode (no fake samples):
+
+```bash
+ros2 launch waybionic_sensors imu_publisher.launch.py use_mock:=false
+```
+
+Zero IMU samples. `imu.heartbeat` is STALE.
+
+## Tests
+
+```bash
+colcon test --packages-select waybionic_sensors
+colcon test-result --all --verbose
+```
+
+96 tests, 0 failures on Ubuntu 24.04 / ROS 2 Jazzy at `e317df4`. This closeout
+adds two documentation-guard tests (expected 98). Coverage spans message
+semantics and covariance, mock generation and stalling, diagnostics levels and
+units, the hardware boundary, package structure, and a runtime suite that spins
+the node to check timestamps, frame IDs, rate, demo defaults, and the heartbeat
+transitioning from OK to STALE.
+
+## Related docs
+
+- `docs/IMU_CONTRACT.md` — topics, units, covariance, timestamps, and parameters
+- `docs/HARDWARE_INTERFACE.md` — questions for electrical (owner / OPEN status) and how to add a driver
+- `docs/PR_NOTES.md` — review notes, design rationale, and runtime evidence
+- README **Runtime handoff** — commands for Malik to run raw / demo / stall without reading the code
+- `waybionic_rviz_plugins/docs/DIAGNOSTICS_BACKEND_INTEGRATION.md` — the diagnostics contract this package publishes against

waybionic_sensors/config/imu_demo.rviz

@@ -0,0 +1,105 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+  - Class: rviz_common/Views
+    Name: Views
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 0.3
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: true
+    - Acceleration properties:
+        Acc. vector alpha: 1
+        Acc. vector color: 204; 51; 204
+        Acc. vector scale: 0.05
+        Derotate acceleration: true
+        Enable acceleration: true
+      Axes properties:
+        Axes scale: 0.2
+        Enable axes: true
+      Box properties:
+        Box alpha: 0.4
+        Box color: 255; 0; 0
+        Enable box: true
+        x_scale: 0.15
+        y_scale: 0.08
+        z_scale: 0.04
+      Class: rviz_imu_plugin/Imu
+      Enabled: true
+      Name: IMU orientation (demo)
+      Topic:
+        Depth: 10
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /waybionic/imu/data_demo
+      Value: true
+      fixed_frame_orientation: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.0
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.5
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.8
+    Saved: ~