From e559cdc2108f1cc3ed0f20a6d1a51a4c2ee4bbfc Mon Sep 17 00:00:00 2001 From: Khuzaymah Bin Haris Date: Tue, 4 Aug 2026 04:59:11 +0300 Subject: [PATCH 1/7] Add waybionic_sensors IMU package with correct raw sensor semantics Adds an IMU publisher built from current main, carrying over only the waybionic_sensors directory from the earlier IMU branch. That branch predated the merged foundation, so replaying it would have reverted CI and other files that landed since. The publisher no longer presents generated data as measurement. An accelerometer and a gyroscope cannot observe absolute heading, so the raw topic sets orientation_covariance[0] = -1 and the synthetic orientation moved to its own data_demo topic, off by default. The rotating TF became opt-in for the same reason. Covariances are populated from parameterised standard deviations rather than left at zero, which a consumer would read as perfect certainty. Sensor health now reaches the merged diagnostics panel: imu.heartbeat publishes at 2 Hz and reports STALE past a configurable sample age, including when live mode runs with no hardware attached. The node is split into a hardware-independent reading type, a mock source, a driver interface, a message builder, and a diagnostics builder, so adding a real sensor means implementing one interface rather than editing the publisher. No serial protocol is invented; docs/HARDWARE_INTERFACE.md records the open questions for electrical. Co-authored-by: Cursor --- .gitignore | 4 + waybionic_sensors/README.md | 123 ++++++++ waybionic_sensors/config/imu_demo.rviz | 93 ++++++ waybionic_sensors/docs/HARDWARE_INTERFACE.md | 85 ++++++ waybionic_sensors/docs/IMU_CONTRACT.md | 104 +++++++ waybionic_sensors/docs/PR_NOTES.md | 169 +++++++++++ waybionic_sensors/launch/imu_demo.launch.py | 87 ++++++ .../launch/imu_publisher.launch.py | 65 +++++ waybionic_sensors/package.xml | 31 ++ waybionic_sensors/resource/waybionic_sensors | 0 waybionic_sensors/setup.cfg | 4 + waybionic_sensors/setup.py | 34 +++ waybionic_sensors/test/test_flake8.py | 11 + .../test/test_hardware_reader.py | 73 +++++ .../test/test_imu_diagnostics.py | 144 +++++++++ waybionic_sensors/test/test_imu_messages.py | 157 ++++++++++ .../test/test_imu_publisher_node.py | 224 ++++++++++++++ waybionic_sensors/test/test_mock_source.py | 91 ++++++ .../test/test_package_metadata.py | 98 +++++++ waybionic_sensors/test/test_pep257.py | 11 + .../waybionic_sensors/__init__.py | 0 .../waybionic_sensors/hardware_reader.py | 84 ++++++ .../waybionic_sensors/imu_diagnostics.py | 175 +++++++++++ .../waybionic_sensors/imu_messages.py | 144 +++++++++ .../waybionic_sensors/imu_publisher_node.py | 273 ++++++++++++++++++ .../waybionic_sensors/imu_reading.py | 51 ++++ .../waybionic_sensors/mock_source.py | 115 ++++++++ 27 files changed, 2450 insertions(+) create mode 100644 waybionic_sensors/README.md create mode 100644 waybionic_sensors/config/imu_demo.rviz create mode 100644 waybionic_sensors/docs/HARDWARE_INTERFACE.md create mode 100644 waybionic_sensors/docs/IMU_CONTRACT.md create mode 100644 waybionic_sensors/docs/PR_NOTES.md create mode 100644 waybionic_sensors/launch/imu_demo.launch.py create mode 100644 waybionic_sensors/launch/imu_publisher.launch.py create mode 100644 waybionic_sensors/package.xml create mode 100644 waybionic_sensors/resource/waybionic_sensors create mode 100644 waybionic_sensors/setup.cfg create mode 100644 waybionic_sensors/setup.py create mode 100644 waybionic_sensors/test/test_flake8.py create mode 100644 waybionic_sensors/test/test_hardware_reader.py create mode 100644 waybionic_sensors/test/test_imu_diagnostics.py create mode 100644 waybionic_sensors/test/test_imu_messages.py create mode 100644 waybionic_sensors/test/test_imu_publisher_node.py create mode 100644 waybionic_sensors/test/test_mock_source.py create mode 100644 waybionic_sensors/test/test_package_metadata.py create mode 100644 waybionic_sensors/test/test_pep257.py create mode 100644 waybionic_sensors/waybionic_sensors/__init__.py create mode 100644 waybionic_sensors/waybionic_sensors/hardware_reader.py create mode 100644 waybionic_sensors/waybionic_sensors/imu_diagnostics.py create mode 100644 waybionic_sensors/waybionic_sensors/imu_messages.py create mode 100644 waybionic_sensors/waybionic_sensors/imu_publisher_node.py create mode 100644 waybionic_sensors/waybionic_sensors/imu_reading.py create mode 100644 waybionic_sensors/waybionic_sensors/mock_source.py diff --git a/.gitignore b/.gitignore index 28b0c9f..de74c75 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,10 @@ build/ install/ log/ +# Python caches +__pycache__/ +*.py[cod] + # imported ROS projects easy_handeye2/ ros2_aruco/ diff --git a/waybionic_sensors/README.md b/waybionic_sensors/README.md new file mode 100644 index 0000000..36b7113 --- /dev/null +++ b/waybionic_sensors/README.md @@ -0,0 +1,123 @@ +# 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 +rosdep install --from-paths src --ignore-src -r -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`. + +## 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, and the row turns STALE once the sample age +passes `stale_timeout_sec`. + +## What is measured and what is generated + +The raw topic carries gyroscope and accelerometer data only. It marks +orientation unavailable with `orientation_covariance[0] = -1`, because an +accelerometer and a gyroscope cannot observe absolute heading. The synthetic +orientation lives on its own `data_demo` topic and is off by default, so nothing +can mistake it for a measurement. + +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, because the sensor model, +transport, and packet format are unconfirmed. The open questions for electrical +are tracked 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. + +## Tests + +```bash +colcon test --packages-select waybionic_sensors +colcon test-result --all --verbose +``` + +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, and parameters +- `docs/HARDWARE_INTERFACE.md` — questions for electrical and how to add a driver +- `docs/PR_NOTES.md` — review notes, design rationale, and runtime evidence +- `waybionic_rviz_plugins/docs/DIAGNOSTICS_BACKEND_INTEGRATION.md` — the diagnostics contract this package publishes against diff --git a/waybionic_sensors/config/imu_demo.rviz b/waybionic_sensors/config/imu_demo.rviz new file mode 100644 index 0000000..e558e15 --- /dev/null +++ b/waybionic_sensors/config/imu_demo.rviz @@ -0,0 +1,93 @@ +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: + 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 + - Class: rviz_default_plugins/Imu + Enabled: true + Name: IMU acceleration (raw) + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waybionic/imu/data_raw + Value: true + alpha: 1 + color: 204; 51; 204 + history_length: 1 + queue_size: 10 + scale: 0.1 + 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: + Value: Orbit (rviz) + Yaw: 0.8 + Saved: ~ diff --git a/waybionic_sensors/docs/HARDWARE_INTERFACE.md b/waybionic_sensors/docs/HARDWARE_INTERFACE.md new file mode 100644 index 0000000..aeeaf7b --- /dev/null +++ b/waybionic_sensors/docs/HARDWARE_INTERFACE.md @@ -0,0 +1,85 @@ +# IMU Hardware Interface + +Everything the software side needs from electrical before a real IMU driver can +be written, plus what is deliberately left unimplemented until those answers +arrive. + +**Status: awaiting answers from electrical.** No value below is confirmed. The +package ships a mock source and an unimplemented driver stub rather than a +guessed serial protocol. + +## Questions for electrical + +### Sensor + +| # | Question | Answer | +|---|----------|--------| +| 1 | Exact sensor model and manufacturer part number? | _unanswered_ | +| 2 | Link to the datasheet used for the selection? | _unanswered_ | +| 3 | Does the device output a fused quaternion, or only raw accelerometer and gyroscope? | _unanswered_ | +| 4 | Is there a magnetometer, and will it be usable near the motors? | _unanswered_ | +| 5 | Is on-chip filtering enabled, and at what cutoff? | _unanswered_ | + +### Transport + +| # | Question | Answer | +|---|----------|--------| +| 6 | How does the IMU reach the ground-station computer: direct USB, USB-serial bridge, microcontroller relay, or I2C/SPI on a board? | _unanswered_ | +| 7 | If a microcontroller sits in between, what is its packet format: framing bytes, field order, endianness, checksum? | _unanswered_ | +| 8 | Baud rate or bus clock, and is it configurable? | _unanswered_ | +| 9 | Device path or enumeration rule on Linux, and should a udev rule be provided? | _unanswered_ | +| 10 | Does the device timestamp its own samples, or must the host stamp on receipt? | _unanswered_ | + +### Data format + +| # | Question | Answer | +|---|----------|--------| +| 11 | Units as transmitted: g or m/s^2, deg/s or rad/s, raw counts with a scale factor? | _unanswered_ | +| 12 | Axis convention and mounting orientation relative to `base_link`? | _unanswered_ | +| 13 | Sample rate, and is it fixed or configurable? | _unanswered_ | +| 14 | Per-axis noise density or RMS noise for covariance values? | _unanswered_ | +| 15 | Is bias/scale calibration applied on-device, or expected from the host? | _unanswered_ | + +### Integration + +| # | Question | Answer | +|---|----------|--------| +| 16 | Where is the IMU physically mounted, and what is its transform from `base_link`? | _unanswered_ | +| 17 | Is there a status or fault line worth surfacing in `/diagnostics`? | _unanswered_ | +| 18 | Expected behaviour on cable disconnect: silence, error frames, or reconnect? | _unanswered_ | + +## Known unknowns + +Until the answers arrive, these stay open and are not guessed anywhere in code: + +- Wire protocol and framing. `hardware_reader.py` defines an interface only. +- Real covariance values. The mock uses placeholder standard deviations exposed + as parameters (`angular_velocity_stddev`, `linear_acceleration_stddev`). +- Whether a fused orientation will ever be available. Until it is, the raw topic + marks orientation unavailable. +- The static transform from `base_link` to `imu_link`. The demo TF is a + visualisation aid with a placeholder 0.1 m offset, not a mounting claim. + +## Adding the driver later + +The node reads samples through +`waybionic_sensors.hardware_reader.ImuHardwareReader`. Adding hardware means +implementing that interface in a new module and constructing it instead of +`UnconfiguredImuReader`: + +```python +class MyImuReader(ImuHardwareReader): + def open(self): ... + def read(self, stamp_ns) -> Optional[ImuReading]: ... + def close(self): ... + def describe(self) -> str: ... +``` + +The driver owns transport and parsing, and converts to the REP-103 units of +`ImuReading`. Message construction, covariance, diagnostics, and TF need no +changes. Parser tests should be added at that point using recorded packets from +the real device. + +Live mode already works end to end with the stub: the node publishes no samples +and `imu.heartbeat` reports STALE, which is the correct depiction of a missing +sensor. diff --git a/waybionic_sensors/docs/IMU_CONTRACT.md b/waybionic_sensors/docs/IMU_CONTRACT.md new file mode 100644 index 0000000..a4e1ca0 --- /dev/null +++ b/waybionic_sensors/docs/IMU_CONTRACT.md @@ -0,0 +1,104 @@ +# IMU Message Contract + +What `waybionic_sensors` publishes, in what units, and which parts are measured +versus generated for display. + +## Topics + +| Topic | Type | Default | Meaning | +|-------|------|---------|---------| +| `/waybionic/imu/data_raw` | `sensor_msgs/msg/Imu` | always on | Gyroscope and accelerometer only. Orientation marked unavailable. | +| `/waybionic/imu/data_demo` | `sensor_msgs/msg/Imu` | off | Synthetic orientation for visualisation. Not a measurement. | +| `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | always on | Sensor health including `imu.heartbeat`. | +| `/tf` | `tf2_msgs/msg/TFMessage` | off | Rotating demo transform for `imu_link`. | + +## Units and axes + +REP-103 throughout: right-handed, x forward, y left, z up. + +| Field | Unit | +|-------|------| +| `angular_velocity` | rad/s | +| `linear_acceleration` | m/s^2, including gravity | +| `orientation` | unit quaternion, demo topic only | +| Covariance diagonals | squared units of the field above | + +A level, stationary sensor reports approximately `+9.80665` m/s^2 on +`linear_acceleration.z`, matching the `sensor_msgs/msg/Imu` convention that +acceleration is not gravity compensated. + +## Raw versus fused orientation + +An accelerometer and a gyroscope cannot observe absolute heading. Publishing a +generated quaternion on the raw topic would let any consumer, including a future +localisation node, treat invented data as a measurement. + +The raw topic therefore always sets: + +``` +orientation = (0, 0, 0, 1) # placeholder, not a measurement +orientation_covariance[0] = -1.0 # "orientation not available" +``` + +`orientation_covariance[0] = -1` is the standard `sensor_msgs/msg/Imu` signal for +absent orientation, and well-behaved consumers check it before using the field. + +When a real fusion source exists, either on-chip or from a filter node, it should +publish on a separate fused topic with a genuine covariance. Only then should +`imu.roll`, `imu.pitch`, and `imu.yaw` appear in `/diagnostics`. + +The demo topic exists so RViz has something to show. It is off by default, named +`data_demo` rather than `data`, and documented here as generated. + +## Covariance + +Each covariance is a row-major 3x3 matrix carrying `stddev^2` on the diagonal. +Off-diagonal terms are zero because the mock models the axes as uncorrelated, +which is a stated assumption rather than a missing value. + +| Parameter | Default | Meaning | +|-----------|---------|---------| +| `angular_velocity_stddev` | `0.01` | Assumed gyroscope noise, rad/s | +| `linear_acceleration_stddev` | `0.05` | Assumed accelerometer noise, m/s^2 | +| `orientation_stddev` | `0.05` | Assumed demo orientation noise, rad | + +These are placeholders chosen to be plausible for a consumer MEMS IMU. They are +parameters precisely so they can be replaced with datasheet or bench values +without touching code. See `HARDWARE_INTERFACE.md` question 14. + +## Diagnostics + +Names, `value`/`unit` keys, and level mapping follow +`waybionic_rviz_plugins/docs/DIAGNOSTICS_BACKEND_INTEGRATION.md`, so the merged +DiagnosticsPanel renders these without IMU-specific code. + +| Signal | Unit | Levels | Meaning | +|--------|------|--------|---------| +| `imu.heartbeat` | `s` | OK, STALE | Age of the newest sample. STALE past `stale_timeout_sec`, or when none ever arrived. | +| `imu.rate` | `Hz` | OK, WARN, STALE | Measured publish rate. WARN below 80% of the configured rate. | +| `imu.angular_velocity` | `rad/s` | OK | Gyroscope vector magnitude. | +| `imu.linear_acceleration` | `m/s^2` | OK | Accelerometer vector magnitude, including gravity. | + +Published at `diagnostics_rate_hz`, default 2 Hz, which satisfies the +at-least-1-Hz requirement in issue #4. + +## Parameters + +| Parameter | Default | Purpose | +|-----------|---------|---------| +| `use_mock` | `true` | Mock samples instead of hardware | +| `topic` | `/waybionic/imu/data_raw` | Raw output topic | +| `demo_orientation_topic` | `/waybionic/imu/data_demo` | Demo output topic | +| `diagnostics_topic` | `/diagnostics` | Health output topic | +| `frame_id` | `imu_link` | Measurement frame | +| `parent_frame_id` | `base_link` | Parent for the demo TF | +| `publish_rate_hz` | `50.0` | Sample rate | +| `diagnostics_rate_hz` | `2.0` | Health report rate | +| `stale_timeout_sec` | `1.0` | Heartbeat staleness threshold | +| `publish_demo_orientation` | `false` | Enable the demo topic | +| `publish_demo_tf` | `false` | Enable the demo TF | +| `angular_velocity_stddev` | `0.01` | Gyroscope noise assumption | +| `linear_acceleration_stddev` | `0.05` | Accelerometer noise assumption | +| `orientation_stddev` | `0.05` | Demo orientation noise assumption | +| `mock_stall_after_sec` | `0.0` | Stop the mock to demonstrate stale, 0 disables | +| `serial_port` | `''` | Reserved for the future driver | diff --git a/waybionic_sensors/docs/PR_NOTES.md b/waybionic_sensors/docs/PR_NOTES.md new file mode 100644 index 0000000..9d11fba --- /dev/null +++ b/waybionic_sensors/docs/PR_NOTES.md @@ -0,0 +1,169 @@ +# IMU PR Notes + +Review notes for the standalone `waybionic_sensors` pull request. + +## Scope + +This branch adds one package, `waybionic_sensors`, on top of current `main`. It +touches nothing else: no diagnostics panel changes, no CI or foundation changes. + +The earlier `feature/imu-rviz-integration` branch was not rebased or merged +forward. It predated the merged foundation, so replaying it would have reverted +CI, `CONTRIBUTING.md`, and other files that landed in the meantime. Only the +`waybionic_sensors` directory was carried across, and the publisher was then +rewritten against the review feedback. + +## What changed relative to the old IMU branch + +| Old behaviour | Problem | Now | +|---------------|---------|-----| +| Synthetic quaternion published on `data_raw` | Presented generated data as a measurement | Raw topic sets `orientation_covariance[0] = -1`; synthetic orientation moved to `/waybionic/imu/data_demo`, off by default | +| Rotating TF always broadcast | Implied the sensor knows its own attitude | `publish_demo_tf`, default false; enabled only by `imu_demo.launch.py` | +| All covariances left at zero | Zero means "perfectly certain" to a consumer | Diagonal covariances from parameterised standard deviations, documented as placeholders | +| No `/diagnostics` output | Panel could not show IMU health | `imu.heartbeat` plus rate and telemetry rows at 2 Hz | +| One 120-line node doing everything | Serial work would have to be bolted into the publisher | Six modules: reading type, mock source, hardware boundary, message builder, diagnostics builder, node | +| Three metadata tests | No behavioural coverage | 83 tests including a runtime suite that spins the node | +| `serial_port` parameter with no reader | Suggested a driver existed | Documented boundary plus a stub that makes the missing driver visible in diagnostics | + +## Raw versus fused orientation + +An accelerometer and a gyroscope cannot observe absolute heading. Publishing a +generated quaternion on the raw topic would let a future localisation or fusion +node consume invented data as if it were measured. + +`/waybionic/imu/data_raw` therefore always sets `orientation_covariance[0] = -1`, +the standard `sensor_msgs/msg/Imu` marker for absent orientation, and leaves the +quaternion at identity as a placeholder. The synthetic orientation lives on +`/waybionic/imu/data_demo`, is off by default, and is named so it cannot be +mistaken for the real thing. + +`imu.roll`, `imu.pitch`, and `imu.yaw` from the backend integration doc are +deliberately **not** published yet, for the same reason. They should appear when +a real fusion source exists. + +## Covariance + +Row-major 3x3 with `stddev^2` on the diagonal. Off-diagonals are zero because +the mock models the axes as uncorrelated, which is a stated assumption rather +than an unknown left blank. + +| Parameter | Default | Meaning | +|-----------|---------|---------| +| `angular_velocity_stddev` | `0.01` | rad/s | +| `linear_acceleration_stddev` | `0.05` | m/s^2 | +| `orientation_stddev` | `0.05` | rad, demo topic only | + +Plausible consumer-MEMS placeholders, exposed as parameters so datasheet values +can replace them without a code change. Tracked as question 14 in +`docs/HARDWARE_INTERFACE.md`. + +## Module boundaries + +```text +mock_source.py ─┐ + ├─> ImuReading ─┬─> imu_messages.py -> sensor_msgs/Imu, TF +hardware_reader.py ─┘ └─> imu_diagnostics.py -> DiagnosticArray + imu_publisher_node.py wires them +``` + +`imu_reading.py` is the contract between sample producers and consumers. A real +driver implements `ImuHardwareReader` and returns `ImuReading` values; message +construction, covariance, diagnostics, and TF need no changes. + +Two structural tests enforce this: the node must not construct `Imu()` or +`DiagnosticStatus` itself. + +## Hardware handoff + +No serial protocol is implemented, because the sensor model, transport, and +packet format are unconfirmed. `docs/HARDWARE_INTERFACE.md` holds 18 numbered +questions for electrical across sensor, transport, data format, and integration, +plus the list of known unknowns and the recipe for adding the driver later. + +A structural test asserts no invented protocol crept in (`import serial`, +`baudrate`, `struct.unpack`). + +Live mode is still meaningful today: with `use_mock:=false` the node publishes no +samples and `imu.heartbeat` reports STALE, which is the correct depiction of an +absent sensor. + +## Runtime evidence (Ubuntu 24.04 / ROS 2 Jazzy / WSL2) + +`ros2 topic hz /waybionic/imu/data_raw`: + +``` +average rate: 49.988 + min: 0.019s max: 0.021s std dev: 0.00030s window: 51 +``` + +`ros2 topic echo /waybionic/imu/data_raw --once`: + +```yaml +header: + stamp: {sec: 1785808547, nanosec: 917613821} + frame_id: imu_link +orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} +orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +angular_velocity: {x: -0.0736..., y: -0.0383..., z: -0.1473...} +angular_velocity_covariance: [0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001] +linear_acceleration: {x: -0.0368..., y: -0.0153..., z: 9.80665} +linear_acceleration_covariance: [0.0025, ...] +``` + +`ros2 topic hz /diagnostics` gives `average rate: 2.000`, comfortably above the +1 Hz requirement. + +Heartbeat while streaming: + +``` +name: imu.heartbeat +message: IMU streaming from mock generator +values: [{key: value, value: '0.00'}, {key: unit, value: s}] +``` + +Heartbeat after `mock_stall_after_sec:=3.0`: + +``` +name: imu.heartbeat +message: No IMU sample for 6.64 s (timeout 1.00 s) +values: [{key: value, value: '6.64'}, {key: unit, value: s}] +``` + +Heartbeat with `use_mock:=false`: + +``` +name: imu.heartbeat +message: No IMU samples received from unconfigured IMU driver; awaiting sensor + model, transport and packet format from electrical +values: [{key: value, value: never}, {key: unit, value: s}] +``` + +`/waybionic/imu/data_demo` is absent from `ros2 topic list` on a default launch, +confirming the demo output is off unless asked for. + +## Tests + +```bash +colcon test --packages-select waybionic_sensors +colcon test-result --all --verbose +``` + +83 tests, 0 failures. + +| Suite | Count | Covers | +|-------|-------|--------| +| `test_imu_messages.py` | 15 | Frame, timestamp, orientation-unavailable marker, covariance diagonals and cross terms, demo message, demo TF | +| `test_imu_diagnostics.py` | 16 | Heartbeat OK/STALE, custom timeout, never-received, age units, rate WARN, telemetry units, absence of roll/pitch/yaw | +| `test_imu_publisher_node.py` | 14 | Runtime: rate follows the parameter, monotonic timestamps, frame IDs, demo defaults off, demo TF on request, heartbeat OK then STALE, live mode without hardware | +| `test_mock_source.py` | 13 | Determinism, gravity, amplitude bounds, stall behaviour, quaternion normalisation | +| `test_hardware_reader.py` | 9 | Interface surface, stub behaviour, a custom reader satisfying the boundary | +| `test_package_metadata.py` | 14 | Module separation, node delegation, launch defaults, docs, entry point, no invented protocol | +| `test_flake8.py`, `test_pep257.py` | 2 | Style and docstrings | + +## Known limitations + +- No physical IMU driver. Blocked on `docs/HARDWARE_INTERFACE.md`. +- Covariance values are documented placeholders, not measured noise. +- The demo orientation and demo TF are visualisation aids, not estimates. +- The `base_link` to `imu_link` offset in the demo TF is a placeholder 0.1 m, not + a mounting claim. diff --git a/waybionic_sensors/launch/imu_demo.launch.py b/waybionic_sensors/launch/imu_demo.launch.py new file mode 100644 index 0000000..fbba776 --- /dev/null +++ b/waybionic_sensors/launch/imu_demo.launch.py @@ -0,0 +1,87 @@ +""" +Benchtop IMU walkthrough: publisher plus RViz, with demo outputs enabled. + +This launch deliberately turns on the synthetic orientation and the rotating TF +so there is something to look at in RViz. Both are visualisation aids and are +off by default in imu_publisher.launch.py. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + """Build the launch description for the IMU RViz demo.""" + use_mock = LaunchConfiguration('use_mock') + topic = LaunchConfiguration('topic') + publish_rate_hz = LaunchConfiguration('publish_rate_hz') + mock_stall_after_sec = LaunchConfiguration('mock_stall_after_sec') + launch_rviz = LaunchConfiguration('launch_rviz') + + imu_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('waybionic_sensors'), + 'launch', + 'imu_publisher.launch.py', + ]) + ), + launch_arguments={ + 'use_mock': use_mock, + 'topic': topic, + 'publish_rate_hz': publish_rate_hz, + 'mock_stall_after_sec': mock_stall_after_sec, + 'publish_demo_orientation': 'true', + 'publish_demo_tf': 'true', + }.items(), + ) + + rviz_config = PathJoinSubstitution([ + FindPackageShare('waybionic_sensors'), + 'config', + 'imu_demo.rviz', + ]) + + return LaunchDescription([ + DeclareLaunchArgument( + 'use_mock', + default_value='true', + choices=['true', 'false'], + description='Generate mock samples instead of reading hardware.', + ), + DeclareLaunchArgument( + 'topic', + default_value='/waybionic/imu/data_raw', + description='sensor_msgs/msg/Imu topic for raw data.', + ), + DeclareLaunchArgument( + 'publish_rate_hz', + default_value='50.0', + description='Sample publish rate in Hz.', + ), + DeclareLaunchArgument( + 'mock_stall_after_sec', + default_value='0.0', + description='Stop the mock after this many seconds to show the stale heartbeat.', + ), + DeclareLaunchArgument( + 'launch_rviz', + default_value='true', + choices=['true', 'false'], + description='Set false for a headless run in CI or over a plain shell.', + ), + imu_launch, + Node( + package='rviz2', + executable='rviz2', + name='waybionic_imu_rviz', + output='screen', + arguments=['-d', rviz_config], + condition=IfCondition(launch_rviz), + ), + ]) diff --git a/waybionic_sensors/launch/imu_publisher.launch.py b/waybionic_sensors/launch/imu_publisher.launch.py new file mode 100644 index 0000000..edcbd99 --- /dev/null +++ b/waybionic_sensors/launch/imu_publisher.launch.py @@ -0,0 +1,65 @@ +""" +Launch the WayBionic IMU publisher on its own. + +Demo orientation and demo TF default to false here so the raw sensor contract is +what a plain launch produces. Use imu_demo.launch.py for the visual walkthrough. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + +_ARGUMENTS = [ + ('use_mock', 'true', 'Generate mock samples instead of reading hardware.', bool), + ('topic', '/waybionic/imu/data_raw', + 'sensor_msgs/msg/Imu topic for raw gyroscope and accelerometer data.', str), + ('demo_orientation_topic', '/waybionic/imu/data_demo', + 'Topic for the synthetic demo orientation. Never the raw topic.', str), + ('diagnostics_topic', '/diagnostics', + 'DiagnosticArray topic carrying imu.heartbeat.', str), + ('frame_id', 'imu_link', 'Frame of the IMU measurements.', str), + ('parent_frame_id', 'base_link', 'Parent frame used by the optional demo TF.', str), + ('publish_rate_hz', '50.0', 'Sample publish rate in Hz.', float), + ('diagnostics_rate_hz', '2.0', 'Diagnostics publish rate in Hz, at least 1 Hz.', float), + ('stale_timeout_sec', '1.0', 'Sample age after which imu.heartbeat reports STALE.', float), + ('publish_demo_orientation', 'false', + 'Publish a synthetic orientation on the demo topic. Visualisation aid only.', bool), + ('publish_demo_tf', 'false', + 'Broadcast a rotating demo TF for the IMU frame. Visualisation aid only.', bool), + ('angular_velocity_stddev', '0.01', + 'Assumed gyroscope noise standard deviation in rad/s.', float), + ('linear_acceleration_stddev', '0.05', + 'Assumed accelerometer noise standard deviation in m/s^2.', float), + ('orientation_stddev', '0.05', + 'Assumed orientation standard deviation in rad, demo topic only.', float), + ('mock_stall_after_sec', '0.0', + 'Stop the mock after this many seconds to demonstrate the stale heartbeat. ' + '0 disables stalling.', float), + ('serial_port', '', + 'Reserved for the future hardware reader. No driver is implemented yet.', str), +] + + +def generate_launch_description(): + """Build the launch description for the IMU publisher node.""" + declarations = [ + DeclareLaunchArgument(name, default_value=default, description=description) + for name, default, description, _ in _ARGUMENTS + ] + + parameters = { + name: ParameterValue(LaunchConfiguration(name), value_type=value_type) + for name, _, _, value_type in _ARGUMENTS + } + + return LaunchDescription(declarations + [ + Node( + package='waybionic_sensors', + executable='imu_publisher', + name='waybionic_imu_publisher', + output='screen', + parameters=[parameters], + ), + ]) diff --git a/waybionic_sensors/package.xml b/waybionic_sensors/package.xml new file mode 100644 index 0000000..2c17d8e --- /dev/null +++ b/waybionic_sensors/package.xml @@ -0,0 +1,31 @@ + + + + waybionic_sensors + 0.1.0 + WayBionic IMU publishing, sensor health diagnostics, and the hardware driver boundary. + Khuzaymah Bin Haris + Apache-2.0 + + ament_python + + builtin_interfaces + diagnostic_msgs + geometry_msgs + rclpy + sensor_msgs + tf2_ros + + launch + launch_ros + rviz2 + + ament_flake8 + ament_pep257 + python3-pytest + tf2_msgs + + + ament_python + + diff --git a/waybionic_sensors/resource/waybionic_sensors b/waybionic_sensors/resource/waybionic_sensors new file mode 100644 index 0000000..e69de29 diff --git a/waybionic_sensors/setup.cfg b/waybionic_sensors/setup.cfg new file mode 100644 index 0000000..884847a --- /dev/null +++ b/waybionic_sensors/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/waybionic_sensors +[install] +install_scripts=$base/lib/waybionic_sensors diff --git a/waybionic_sensors/setup.py b/waybionic_sensors/setup.py new file mode 100644 index 0000000..8cf145e --- /dev/null +++ b/waybionic_sensors/setup.py @@ -0,0 +1,34 @@ +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'waybionic_sensors' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*')), + (os.path.join('share', package_name, 'docs'), glob('docs/*.md')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Khuzaymah Bin Haris', + maintainer_email='khuzaymahbinharis@gmail.com', + description=( + 'WayBionic IMU publishing, sensor health diagnostics, and the hardware ' + 'driver boundary.' + ), + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'imu_publisher = waybionic_sensors.imu_publisher_node:main', + ], + }, +) diff --git a/waybionic_sensors/test/test_flake8.py b/waybionic_sensors/test/test_flake8.py new file mode 100644 index 0000000..3d3f045 --- /dev/null +++ b/waybionic_sensors/test/test_flake8.py @@ -0,0 +1,11 @@ +"""Style check for the package sources.""" + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, '\n'.join(['Found %d code style errors / warnings:' % len(errors)] + errors) diff --git a/waybionic_sensors/test/test_hardware_reader.py b/waybionic_sensors/test/test_hardware_reader.py new file mode 100644 index 0000000..ebc36f8 --- /dev/null +++ b/waybionic_sensors/test/test_hardware_reader.py @@ -0,0 +1,73 @@ +"""Tests for the future-hardware boundary.""" + +import inspect + +import pytest + +from waybionic_sensors.hardware_reader import ImuHardwareReader, UnconfiguredImuReader +from waybionic_sensors.imu_reading import ImuReading + + +def test_interface_cannot_be_instantiated_directly(): + with pytest.raises(TypeError): + ImuHardwareReader() + + +def test_interface_declares_the_expected_driver_surface(): + for name in ('start', 'read', 'stop', 'describe'): + assert hasattr(ImuHardwareReader, name) + + +def test_read_returns_the_shared_reading_type(): + # The driver boundary is defined in terms of ImuReading so a real sensor can + # be added without touching message construction or diagnostics. + signature = inspect.signature(ImuHardwareReader.read) + assert 'stamp_ns' in signature.parameters + + +def test_unconfigured_reader_satisfies_the_interface(): + assert isinstance(UnconfiguredImuReader(), ImuHardwareReader) + + +def test_unconfigured_reader_produces_no_samples(): + assert UnconfiguredImuReader().read(1_000) is None + + +def test_unconfigured_reader_start_and_stop_are_safe(): + reader = UnconfiguredImuReader() + reader.start() + reader.stop() + reader.stop() + + +def test_unconfigured_reader_description_flags_the_missing_driver(): + description = UnconfiguredImuReader().describe() + assert 'unconfigured' in description + assert 'electrical' in description + + +def test_unconfigured_reader_description_mentions_a_configured_port(): + assert '/dev/ttyUSB0' in UnconfiguredImuReader('/dev/ttyUSB0').describe() + + +def test_a_custom_reader_can_supply_readings(): + class FakeReader(ImuHardwareReader): + def start(self): + pass + + def read(self, stamp_ns): + return ImuReading( + stamp_ns=stamp_ns, + angular_velocity=(0.0, 0.0, 0.0), + linear_acceleration=(0.0, 0.0, 9.80665), + ) + + def stop(self): + pass + + def describe(self): + return 'fake' + + reading = FakeReader().read(42) + assert reading.stamp_ns == 42 + assert reading.linear_acceleration[2] == 9.80665 diff --git a/waybionic_sensors/test/test_imu_diagnostics.py b/waybionic_sensors/test/test_imu_diagnostics.py new file mode 100644 index 0000000..cc4c8c2 --- /dev/null +++ b/waybionic_sensors/test/test_imu_diagnostics.py @@ -0,0 +1,144 @@ +"""Tests for the /diagnostics output, including heartbeat and stale behaviour.""" + +from diagnostic_msgs.msg import DiagnosticStatus + +from waybionic_sensors.imu_diagnostics import ( + ANGULAR_VELOCITY_NAME, + HEARTBEAT_NAME, + ImuDiagnosticsBuilder, + LINEAR_ACCELERATION_NAME, + RATE_NAME, +) +from waybionic_sensors.imu_reading import GRAVITY_M_S2, ImuReading + +NOW_NS = 10_000_000_000 +SECOND_NS = 1_000_000_000 + + +def make_reading(age_sec: float) -> ImuReading: + """Build a reading that is ``age_sec`` old relative to ``NOW_NS``.""" + return ImuReading( + stamp_ns=NOW_NS - int(age_sec * SECOND_NS), + angular_velocity=(0.0, 0.0, 3.0), + linear_acceleration=(0.0, 0.0, GRAVITY_M_S2), + ) + + +def build(last_reading, measured_rate_hz=50.0, stale_timeout_sec=1.0): + """Run the builder with the defaults used across these tests.""" + builder = ImuDiagnosticsBuilder( + stale_timeout_sec=stale_timeout_sec, expected_rate_hz=50.0 + ) + return builder.build( + NOW_NS, last_reading, measured_rate_hz, source_description='mock generator' + ) + + +def status_named(array, name): + """Return the single status with ``name``, or None.""" + matches = [status for status in array.status if status.name == name] + return matches[0] if matches else None + + +def value_of(status): + """Return the ``value`` key of a status.""" + return next(entry.value for entry in status.values if entry.key == 'value') + + +def unit_of(status): + """Return the ``unit`` key of a status.""" + return next(entry.value for entry in status.values if entry.key == 'unit') + + +def test_heartbeat_is_always_published(): + assert status_named(build(make_reading(0.0)), HEARTBEAT_NAME) is not None + assert status_named(build(None), HEARTBEAT_NAME) is not None + + +def test_heartbeat_is_ok_when_samples_are_fresh(): + status = status_named(build(make_reading(0.02)), HEARTBEAT_NAME) + assert status.level == DiagnosticStatus.OK + + +def test_heartbeat_goes_stale_past_the_timeout(): + status = status_named(build(make_reading(2.5)), HEARTBEAT_NAME) + assert status.level == DiagnosticStatus.STALE + assert 'timeout' in status.message + + +def test_heartbeat_respects_a_custom_timeout(): + fresh = status_named(build(make_reading(1.5), stale_timeout_sec=3.0), HEARTBEAT_NAME) + stale = status_named(build(make_reading(1.5), stale_timeout_sec=0.5), HEARTBEAT_NAME) + assert fresh.level == DiagnosticStatus.OK + assert stale.level == DiagnosticStatus.STALE + + +def test_heartbeat_is_stale_when_no_sample_ever_arrived(): + status = status_named(build(None), HEARTBEAT_NAME) + assert status.level == DiagnosticStatus.STALE + assert value_of(status) == 'never' + + +def test_heartbeat_reports_age_in_seconds(): + status = status_named(build(make_reading(0.25)), HEARTBEAT_NAME) + assert unit_of(status) == 's' + assert float(value_of(status)) == 0.25 + + +def test_heartbeat_names_the_active_source(): + status = status_named(build(make_reading(0.0)), HEARTBEAT_NAME) + assert 'mock generator' in status.message + + +def test_statuses_carry_a_hardware_id(): + for status in build(make_reading(0.0)).status: + assert status.hardware_id + + +def test_rate_row_reports_measured_hz(): + status = status_named(build(make_reading(0.0), measured_rate_hz=49.7), RATE_NAME) + assert unit_of(status) == 'Hz' + assert float(value_of(status)) == 49.7 + assert status.level == DiagnosticStatus.OK + + +def test_rate_row_warns_when_well_below_the_configured_rate(): + status = status_named(build(make_reading(0.0), measured_rate_hz=10.0), RATE_NAME) + assert status.level == DiagnosticStatus.WARN + + +def test_rate_row_is_stale_without_samples(): + status = status_named(build(None, measured_rate_hz=0.0), RATE_NAME) + assert status.level == DiagnosticStatus.STALE + + +def test_telemetry_rows_use_unambiguous_units(): + array = build(make_reading(0.0)) + assert unit_of(status_named(array, ANGULAR_VELOCITY_NAME)) == 'rad/s' + assert unit_of(status_named(array, LINEAR_ACCELERATION_NAME)) == 'm/s^2' + + +def test_telemetry_rows_report_vector_magnitudes(): + array = build(make_reading(0.0)) + assert float(value_of(status_named(array, ANGULAR_VELOCITY_NAME))) == 3.0 + assert abs( + float(value_of(status_named(array, LINEAR_ACCELERATION_NAME))) - GRAVITY_M_S2 + ) < 0.001 + + +def test_telemetry_rows_are_omitted_before_any_sample(): + array = build(None) + assert status_named(array, ANGULAR_VELOCITY_NAME) is None + assert status_named(array, LINEAR_ACCELERATION_NAME) is None + + +def test_array_carries_a_header_stamp(): + array = build(make_reading(0.0)) + assert array.header.stamp.sec == NOW_NS // SECOND_NS + + +def test_no_orientation_signals_are_published_from_raw_data(): + # imu.roll/pitch/yaw only become meaningful once a real fusion source + # exists; publishing them from raw data would misrepresent the sensor. + names = {status.name for status in build(make_reading(0.0)).status} + assert not {'imu.roll', 'imu.pitch', 'imu.yaw'} & names diff --git a/waybionic_sensors/test/test_imu_messages.py b/waybionic_sensors/test/test_imu_messages.py new file mode 100644 index 0000000..b556bb9 --- /dev/null +++ b/waybionic_sensors/test/test_imu_messages.py @@ -0,0 +1,157 @@ +"""Tests for sensor_msgs/msg/Imu construction, especially raw/fused semantics.""" + +from waybionic_sensors.imu_messages import ( + build_demo_orientation_message, + build_demo_transform, + build_raw_imu_message, + diagonal_covariance, + ORIENTATION_UNAVAILABLE, + to_time_msg, +) +from waybionic_sensors.imu_reading import GRAVITY_M_S2, ImuReading + +STAMP_NS = 1_234_567_890_123 +ANGULAR_STDDEV = 0.01 +LINEAR_STDDEV = 0.05 +ORIENTATION_STDDEV = 0.5 + + +def make_reading(orientation=None) -> ImuReading: + """Build a reading with distinguishable per-axis values.""" + return ImuReading( + stamp_ns=STAMP_NS, + angular_velocity=(0.1, 0.2, 0.3), + linear_acceleration=(0.4, 0.5, GRAVITY_M_S2), + orientation=orientation, + ) + + +def build_raw(): + """Build the raw message used by most assertions here.""" + return build_raw_imu_message( + make_reading(), + 'imu_link', + angular_velocity_stddev=ANGULAR_STDDEV, + linear_acceleration_stddev=LINEAR_STDDEV, + ) + + +def test_to_time_msg_splits_seconds_and_nanoseconds(): + stamp = to_time_msg(1_500_000_000) + assert stamp.sec == 1 + assert stamp.nanosec == 500_000_000 + + +def test_diagonal_covariance_squares_the_standard_deviation(): + covariance = diagonal_covariance(0.5) + assert covariance[0] == 0.25 + assert covariance[4] == 0.25 + assert covariance[8] == 0.25 + + +def test_diagonal_covariance_leaves_cross_terms_zero(): + covariance = diagonal_covariance(0.5) + off_diagonal = [covariance[i] for i in (1, 2, 3, 5, 6, 7)] + assert off_diagonal == [0.0] * 6 + + +def test_raw_message_uses_configured_frame(): + assert build_raw().header.frame_id == 'imu_link' + + +def test_raw_message_preserves_reading_timestamp(): + header = build_raw().header + assert header.stamp.sec == STAMP_NS // 1_000_000_000 + assert header.stamp.nanosec == STAMP_NS % 1_000_000_000 + + +def test_raw_message_marks_orientation_unavailable(): + message = build_raw() + assert message.orientation_covariance[0] == ORIENTATION_UNAVAILABLE + + +def test_raw_message_uses_identity_orientation_placeholder(): + message = build_raw() + assert ( + message.orientation.x, + message.orientation.y, + message.orientation.z, + message.orientation.w, + ) == (0.0, 0.0, 0.0, 1.0) + + +def test_raw_message_ignores_orientation_present_on_the_reading(): + # Even if a device supplies a fused quaternion, the raw topic must not + # advertise it, otherwise consumers cannot tell the two topics apart. + message = build_raw_imu_message( + make_reading(orientation=(0.1, 0.2, 0.3, 0.9)), + 'imu_link', + angular_velocity_stddev=ANGULAR_STDDEV, + linear_acceleration_stddev=LINEAR_STDDEV, + ) + assert message.orientation_covariance[0] == ORIENTATION_UNAVAILABLE + assert message.orientation.w == 1.0 + + +def test_raw_message_copies_angular_velocity(): + message = build_raw() + assert message.angular_velocity.x == 0.1 + assert message.angular_velocity.y == 0.2 + assert message.angular_velocity.z == 0.3 + + +def test_raw_message_copies_linear_acceleration_including_gravity(): + message = build_raw() + assert message.linear_acceleration.x == 0.4 + assert message.linear_acceleration.y == 0.5 + assert message.linear_acceleration.z == GRAVITY_M_S2 + + +def test_raw_message_populates_measurement_covariances(): + message = build_raw() + assert message.angular_velocity_covariance[0] == ANGULAR_STDDEV ** 2 + assert message.linear_acceleration_covariance[0] == LINEAR_STDDEV ** 2 + + +def test_raw_message_covariances_are_not_left_at_zero(): + message = build_raw() + assert any(value > 0.0 for value in message.angular_velocity_covariance) + assert any(value > 0.0 for value in message.linear_acceleration_covariance) + + +def test_demo_message_carries_a_usable_orientation(): + message = build_demo_orientation_message( + make_reading(), + 'imu_link', + (0.0, 0.0, 0.3826834, 0.9238795), + orientation_stddev=ORIENTATION_STDDEV, + angular_velocity_stddev=ANGULAR_STDDEV, + linear_acceleration_stddev=LINEAR_STDDEV, + ) + assert message.orientation.z == 0.3826834 + assert message.orientation_covariance[0] == ORIENTATION_STDDEV ** 2 + assert message.orientation_covariance[0] != ORIENTATION_UNAVAILABLE + + +def test_demo_message_keeps_the_same_measurement_fields(): + message = build_demo_orientation_message( + make_reading(), + 'imu_link', + (0.0, 0.0, 0.0, 1.0), + orientation_stddev=ORIENTATION_STDDEV, + angular_velocity_stddev=ANGULAR_STDDEV, + linear_acceleration_stddev=LINEAR_STDDEV, + ) + assert message.angular_velocity.z == 0.3 + assert message.linear_acceleration.z == GRAVITY_M_S2 + assert message.header.frame_id == 'imu_link' + + +def test_demo_transform_links_parent_to_imu_frame(): + transform = build_demo_transform( + STAMP_NS, 'base_link', 'imu_link', (0.0, 0.0, 0.0, 1.0) + ) + assert transform.header.frame_id == 'base_link' + assert transform.child_frame_id == 'imu_link' + assert transform.transform.rotation.w == 1.0 + assert transform.header.stamp.sec == STAMP_NS // 1_000_000_000 diff --git a/waybionic_sensors/test/test_imu_publisher_node.py b/waybionic_sensors/test/test_imu_publisher_node.py new file mode 100644 index 0000000..fdef0d9 --- /dev/null +++ b/waybionic_sensors/test/test_imu_publisher_node.py @@ -0,0 +1,224 @@ +""" +Runtime tests that spin the real node and inspect what it publishes. + +These stay hardware independent: everything runs against the mock source. +""" + +import threading +import time + +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +import pytest +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from rclpy.parameter import Parameter +from sensor_msgs.msg import Imu +from tf2_msgs.msg import TFMessage + +from waybionic_sensors.imu_diagnostics import HEARTBEAT_NAME +from waybionic_sensors.imu_messages import ORIENTATION_UNAVAILABLE +from waybionic_sensors.imu_publisher_node import ImuPublisher + +RAW_TOPIC = '/test/imu/data_raw' +DEMO_TOPIC = '/test/imu/data_demo' +DIAGNOSTICS_TOPIC = '/test/diagnostics' + + +class Collector(Node): + """Subscribes to everything the publisher can emit.""" + + def __init__(self) -> None: + """Create one subscription per output under test.""" + super().__init__('imu_test_collector') + self.raw = [] + self.demo = [] + self.diagnostics = [] + self.transforms = [] + self.create_subscription(Imu, RAW_TOPIC, self.raw.append, 20) + self.create_subscription(Imu, DEMO_TOPIC, self.demo.append, 20) + self.create_subscription(DiagnosticArray, DIAGNOSTICS_TOPIC, self.diagnostics.append, 20) + self.create_subscription(TFMessage, '/tf', self.transforms.append, 20) + + +def params(**overrides): + """Build parameter overrides with test topics already applied.""" + values = { + 'topic': RAW_TOPIC, + 'demo_orientation_topic': DEMO_TOPIC, + 'diagnostics_topic': DIAGNOSTICS_TOPIC, + 'publish_rate_hz': 50.0, + 'diagnostics_rate_hz': 10.0, + } + values.update(overrides) + + parameters = [] + for name, value in values.items(): + if isinstance(value, bool): + parameter_type = Parameter.Type.BOOL + elif isinstance(value, float): + parameter_type = Parameter.Type.DOUBLE + else: + parameter_type = Parameter.Type.STRING + parameters.append(Parameter(name, parameter_type, value)) + return parameters + + +class Harness: + """Spins the publisher and a collector together for a fixed duration.""" + + def __init__(self, duration_sec=1.2, **overrides): + """Record how long to spin and which parameters to override.""" + self._duration_sec = duration_sec + self._overrides = overrides + + def __enter__(self): + """Start both nodes on a background executor.""" + self.publisher = ImuPublisher(parameter_overrides=params(**self._overrides)) + self.collector = Collector() + self._executor = SingleThreadedExecutor() + self._executor.add_node(self.publisher) + self._executor.add_node(self.collector) + self._thread = threading.Thread(target=self._executor.spin, daemon=True) + self._thread.start() + time.sleep(self._duration_sec) + return self.collector + + def __exit__(self, *exc_info): + """Tear the executor and nodes down.""" + self._executor.shutdown() + self._thread.join(timeout=5.0) + self.publisher.destroy_node() + self.collector.destroy_node() + return False + + +@pytest.fixture(scope='module', autouse=True) +def ros_context(): + """Initialise rclpy once for this module.""" + rclpy.init() + yield + rclpy.shutdown() + + +def test_publishes_raw_imu_messages(): + with Harness() as collector: + assert len(collector.raw) > 10 + + +def test_raw_messages_use_the_configured_frame(): + with Harness(frame_id='imu_link') as collector: + assert all(message.header.frame_id == 'imu_link' for message in collector.raw) + + +def test_raw_messages_carry_increasing_timestamps(): + with Harness() as collector: + stamps = [ + message.header.stamp.sec * 10**9 + message.header.stamp.nanosec + for message in collector.raw + ] + assert stamps == sorted(stamps) + assert stamps[0] > 0 + + +def test_raw_messages_mark_orientation_unavailable(): + with Harness() as collector: + assert all( + message.orientation_covariance[0] == ORIENTATION_UNAVAILABLE + for message in collector.raw + ) + + +def test_raw_messages_carry_non_zero_measurement_covariance(): + with Harness() as collector: + message = collector.raw[0] + assert message.angular_velocity_covariance[0] > 0.0 + assert message.linear_acceleration_covariance[0] > 0.0 + + +def test_publish_rate_follows_the_parameter(): + duration = 1.5 + with Harness(duration_sec=duration, publish_rate_hz=20.0) as collector: + measured = len(collector.raw) / duration + assert 12.0 < measured < 28.0 + + +def test_demo_topic_is_silent_by_default(): + with Harness() as collector: + assert collector.demo == [] + + +def test_demo_topic_publishes_orientation_when_enabled(): + with Harness(publish_demo_orientation=True) as collector: + assert len(collector.demo) > 10 + assert collector.demo[0].orientation_covariance[0] > 0.0 + + +def test_demo_tf_is_not_broadcast_by_default(): + with Harness() as collector: + children = { + transform.child_frame_id + for message in collector.transforms + for transform in message.transforms + } + assert 'imu_link' not in children + + +def test_demo_tf_is_broadcast_when_enabled(): + with Harness(publish_demo_tf=True, frame_id='imu_link') as collector: + children = { + transform.child_frame_id + for message in collector.transforms + for transform in message.transforms + } + assert 'imu_link' in children + + +def test_heartbeat_is_published_at_least_once_per_second(): + duration = 1.5 + with Harness(duration_sec=duration, diagnostics_rate_hz=2.0) as collector: + heartbeats = [ + status + for array in collector.diagnostics + for status in array.status + if status.name == HEARTBEAT_NAME + ] + assert len(heartbeats) >= int(duration) + + +def test_heartbeat_is_ok_while_the_mock_streams(): + with Harness() as collector: + levels = [ + status.level + for array in collector.diagnostics + for status in array.status + if status.name == HEARTBEAT_NAME + ] + assert DiagnosticStatus.OK in levels + + +def test_heartbeat_goes_stale_after_the_mock_stalls(): + with Harness( + duration_sec=2.5, mock_stall_after_sec=0.4, stale_timeout_sec=0.5 + ) as collector: + levels = [ + status.level + for array in collector.diagnostics + for status in array.status + if status.name == HEARTBEAT_NAME + ] + assert DiagnosticStatus.OK in levels + assert levels[-1] == DiagnosticStatus.STALE + + +def test_live_mode_without_hardware_reports_stale_and_publishes_nothing(): + with Harness(use_mock=False) as collector: + assert collector.raw == [] + heartbeats = [ + status + for array in collector.diagnostics + for status in array.status + if status.name == HEARTBEAT_NAME + ] + assert heartbeats + assert all(status.level == DiagnosticStatus.STALE for status in heartbeats) diff --git a/waybionic_sensors/test/test_mock_source.py b/waybionic_sensors/test/test_mock_source.py new file mode 100644 index 0000000..a0dc057 --- /dev/null +++ b/waybionic_sensors/test/test_mock_source.py @@ -0,0 +1,91 @@ +"""Tests for the synthetic sample generator.""" + +import math + +from waybionic_sensors.imu_reading import GRAVITY_M_S2 +from waybionic_sensors.mock_source import euler_to_quaternion, MockImuSource + +START_NS = 5_000_000_000 +SECOND_NS = 1_000_000_000 + + +def test_first_read_returns_a_sample(): + assert MockImuSource().read(START_NS) is not None + + +def test_reading_uses_the_supplied_timestamp(): + reading = MockImuSource().read(START_NS) + assert reading.stamp_ns == START_NS + + +def test_mock_never_claims_an_orientation(): + # The generator models an accelerometer and gyroscope, which cannot observe + # absolute attitude. Demo orientation is a separate, explicit call. + reading = MockImuSource().read(START_NS) + assert reading.orientation is None + assert reading.has_orientation is False + + +def test_acceleration_includes_gravity_on_z(): + reading = MockImuSource().read(START_NS) + assert reading.linear_acceleration[2] == GRAVITY_M_S2 + + +def test_values_stay_within_the_configured_amplitudes(): + source = MockImuSource(angular_amplitude=0.2, linear_amplitude=0.05) + for step in range(200): + reading = source.read(START_NS + step * SECOND_NS // 10) + assert all(abs(value) <= 0.2 + 1e-9 for value in reading.angular_velocity) + assert abs(reading.linear_acceleration[0]) <= 0.05 + 1e-9 + + +def test_generator_is_deterministic(): + first = MockImuSource().read(START_NS) + second = MockImuSource().read(START_NS) + assert first.angular_velocity == second.angular_velocity + assert first.linear_acceleration == second.linear_acceleration + + +def test_motion_changes_over_time(): + source = MockImuSource() + source.read(START_NS) + later = source.read(START_NS + 2 * SECOND_NS) + assert later.angular_velocity != (0.0, 0.0, 0.0) + + +def test_stall_is_disabled_by_default(): + source = MockImuSource() + source.read(START_NS) + assert source.stalled_deliberately is False + assert source.read(START_NS + 100 * SECOND_NS) is not None + + +def test_stall_stops_samples_after_the_configured_delay(): + source = MockImuSource(stall_after_sec=2.0) + assert source.stalled_deliberately is True + assert source.read(START_NS) is not None + assert source.read(START_NS + 1 * SECOND_NS) is not None + assert source.read(START_NS + 3 * SECOND_NS) is None + + +def test_elapsed_is_measured_from_the_first_sample(): + source = MockImuSource() + source.read(START_NS) + assert source.elapsed_sec(START_NS + 3 * SECOND_NS) == 3.0 + + +def test_demo_orientation_is_a_unit_quaternion(): + source = MockImuSource() + source.read(START_NS) + x, y, z, w = source.demo_orientation(START_NS + SECOND_NS) + assert abs(math.sqrt(x * x + y * y + z * z + w * w) - 1.0) < 1e-9 + + +def test_euler_to_quaternion_returns_identity_for_zero_rotation(): + assert euler_to_quaternion(0.0, 0.0, 0.0) == (0.0, 0.0, 0.0, 1.0) + + +def test_euler_to_quaternion_matches_a_known_yaw(): + _, _, z, w = euler_to_quaternion(0.0, 0.0, math.pi / 2) + assert abs(z - math.sqrt(0.5)) < 1e-9 + assert abs(w - math.sqrt(0.5)) < 1e-9 diff --git a/waybionic_sensors/test/test_package_metadata.py b/waybionic_sensors/test/test_package_metadata.py new file mode 100644 index 0000000..c78fce0 --- /dev/null +++ b/waybionic_sensors/test/test_package_metadata.py @@ -0,0 +1,98 @@ +"""Structural tests: package layout, entry points, and safe launch defaults.""" + +from pathlib import Path + +PACKAGE_ROOT = Path(__file__).resolve().parent.parent +MODULE_ROOT = PACKAGE_ROOT / 'waybionic_sensors' + + +def read(relative_path: str) -> str: + """Return the text of a file inside the package.""" + return (PACKAGE_ROOT / relative_path).read_text(encoding='utf-8') + + +def test_launch_files_exist(): + assert (PACKAGE_ROOT / 'launch' / 'imu_publisher.launch.py').exists() + assert (PACKAGE_ROOT / 'launch' / 'imu_demo.launch.py').exists() + + +def test_demo_rviz_config_exists(): + assert (PACKAGE_ROOT / 'config' / 'imu_demo.rviz').exists() + + +def test_components_are_separated_into_modules(): + # Serial parsing, mock generation, message construction and diagnostics must + # not collapse back into one publisher function. + for module in ( + 'imu_reading.py', + 'mock_source.py', + 'hardware_reader.py', + 'imu_messages.py', + 'imu_diagnostics.py', + 'imu_publisher_node.py', + ): + assert (MODULE_ROOT / module).exists(), module + + +def test_node_module_delegates_message_construction(): + # The node should wire components together, not populate message fields. + node_source = read('waybionic_sensors/imu_publisher_node.py') + assert 'Imu()' not in node_source + assert 'diagonal_covariance' not in node_source + assert '.orientation_covariance' not in node_source + assert 'build_raw_imu_message' in node_source + + +def test_node_module_delegates_diagnostics_construction(): + node_source = read('waybionic_sensors/imu_publisher_node.py') + assert 'DiagnosticStatus' not in node_source + assert 'ImuDiagnosticsBuilder' in node_source + + +def test_hardware_docs_exist(): + assert (PACKAGE_ROOT / 'docs' / 'HARDWARE_INTERFACE.md').exists() + assert (PACKAGE_ROOT / 'docs' / 'IMU_CONTRACT.md').exists() + + +def test_console_entry_point_is_registered(): + assert 'imu_publisher = waybionic_sensors.imu_publisher_node:main' in read('setup.py') + + +def test_setup_installs_launch_config_and_docs(): + setup_source = read('setup.py') + for directory in ('launch', 'config', 'docs'): + assert directory in setup_source + + +def test_package_declares_diagnostics_dependency(): + assert 'diagnostic_msgs' in read('package.xml') + + +def test_demo_outputs_default_to_off(): + # A default-on rotating TF would imply the raw sensor knows its attitude. + launch_source = read('launch/imu_publisher.launch.py') + assert "('publish_demo_orientation', 'false'" in launch_source + assert "('publish_demo_tf', 'false'" in launch_source + + +def test_demo_launch_enables_the_visualisation_aids(): + demo_source = read('launch/imu_demo.launch.py') + assert "'publish_demo_orientation': 'true'" in demo_source + assert "'publish_demo_tf': 'true'" in demo_source + + +def test_demo_launch_supports_a_headless_run(): + assert 'launch_rviz' in read('launch/imu_demo.launch.py') + + +def test_default_topic_and_frame_are_preserved(): + launch_source = read('launch/imu_publisher.launch.py') + assert '/waybionic/imu/data_raw' in launch_source + assert 'imu_link' in launch_source + assert 'base_link' in launch_source + + +def test_no_invented_serial_protocol_is_implemented(): + reader_source = read('waybionic_sensors/hardware_reader.py') + for token in ('import serial', 'baudrate', 'struct.unpack'): + assert token not in reader_source diff --git a/waybionic_sensors/test/test_pep257.py b/waybionic_sensors/test/test_pep257.py new file mode 100644 index 0000000..06e19b7 --- /dev/null +++ b/waybionic_sensors/test/test_pep257.py @@ -0,0 +1,11 @@ +"""Docstring check for the package sources.""" + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/waybionic_sensors/waybionic_sensors/__init__.py b/waybionic_sensors/waybionic_sensors/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/waybionic_sensors/waybionic_sensors/hardware_reader.py b/waybionic_sensors/waybionic_sensors/hardware_reader.py new file mode 100644 index 0000000..9c000f7 --- /dev/null +++ b/waybionic_sensors/waybionic_sensors/hardware_reader.py @@ -0,0 +1,84 @@ +""" +Boundary for a future physical IMU. + +The sensor model, transport and packet format are not confirmed yet, so this +module defines only the interface a driver has to satisfy. Nothing here invents +a wire protocol. See ``docs/HARDWARE_INTERFACE.md`` for the questions that must +be answered by electrical before a real reader is written. + +To add hardware later, implement :class:`ImuHardwareReader` in its own module +and hand an instance to the node. No change to message construction, +diagnostics, or TF publishing is required. +""" + +from abc import ABC, abstractmethod +from typing import Optional + +from waybionic_sensors.imu_reading import ImuReading + + +class ImuHardwareReader(ABC): + """ + Reads samples from a physical IMU. + + Implementations own transport and parsing only. They must return values + already converted into the REP-103 units of :class:`ImuReading`, so the unit + and axis conversion for a specific sensor stays inside its own driver. + """ + + @abstractmethod + def start(self) -> None: + """Acquire the device. Raise if the device cannot be opened.""" + + @abstractmethod + def read(self, stamp_ns: int) -> Optional[ImuReading]: + """ + Return the newest sample, or ``None`` if none is available yet. + + ``stamp_ns`` is the fallback acquisition time to use when the device + does not supply its own timestamp. + """ + + @abstractmethod + def stop(self) -> None: + """Release the device. Must be safe to call when never started.""" + + @abstractmethod + def describe(self) -> str: + """Return a short human-readable description for logs and diagnostics.""" + + +class UnconfiguredImuReader(ImuHardwareReader): + """ + Stands in for the real driver until electrical confirms the sensor. + + Never produces samples. Live mode with this reader is still useful: the + heartbeat goes stale in the diagnostics panel, which is exactly what an + absent or disconnected sensor should look like. + """ + + def __init__(self, serial_port: str = '') -> None: + """Record the configured port, if any, purely for the description.""" + self._serial_port = serial_port + + def start(self) -> None: + """Do nothing. There is no device to acquire.""" + + def read(self, stamp_ns: int) -> Optional[ImuReading]: + """Return ``None`` because no hardware interface exists yet.""" + return None + + def stop(self) -> None: + """Do nothing. There is no device to release.""" + + def describe(self) -> str: + """Return a description that makes the missing driver obvious in logs.""" + if self._serial_port: + return ( + f'unconfigured IMU driver (port {self._serial_port}); ' + 'awaiting sensor model and packet format from electrical' + ) + return ( + 'unconfigured IMU driver; awaiting sensor model, transport and ' + 'packet format from electrical' + ) diff --git a/waybionic_sensors/waybionic_sensors/imu_diagnostics.py b/waybionic_sensors/waybionic_sensors/imu_diagnostics.py new file mode 100644 index 0000000..1ba525f --- /dev/null +++ b/waybionic_sensors/waybionic_sensors/imu_diagnostics.py @@ -0,0 +1,175 @@ +""" +Builds the ``/diagnostics`` array describing IMU health. + +Signal names, ``value``/``unit`` keys and level mapping follow +``waybionic_rviz_plugins/docs/DIAGNOSTICS_BACKEND_INTEGRATION.md`` so the merged +DiagnosticsPanel renders these rows without any camera- or IMU-specific code. + +Separated from the node so the freshness and level logic can be tested without +spinning ROS. +""" + +import math +from typing import Optional + +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue + +from waybionic_sensors.imu_messages import to_time_msg +from waybionic_sensors.imu_reading import ImuReading + +HEARTBEAT_NAME = 'imu.heartbeat' +RATE_NAME = 'imu.rate' +ANGULAR_VELOCITY_NAME = 'imu.angular_velocity' +LINEAR_ACCELERATION_NAME = 'imu.linear_acceleration' + +HARDWARE_ID = 'waybionic_sensors/imu' + + +def _key_values(value: str, unit: str) -> list: + """Return the ``value``/``unit`` pair the panel looks for first.""" + return [KeyValue(key='value', value=value), KeyValue(key='unit', value=unit)] + + +def _magnitude(vector) -> float: + """Return the Euclidean norm of a 3-tuple.""" + return math.sqrt(sum(component * component for component in vector)) + + +class ImuDiagnosticsBuilder: + """ + Turns the publisher's recent activity into diagnostic statuses. + + ``stale_timeout_sec`` is the sample age past which the heartbeat is reported + STALE. It should comfortably exceed one publish period so ordinary jitter + does not flap the status. + + ``expected_rate_hz`` is the configured publish rate, reported alongside the + measured rate so a reviewer can see shortfalls. + """ + + def __init__(self, *, stale_timeout_sec: float = 1.0, expected_rate_hz: float = 50.0) -> None: + """Store the freshness threshold and the configured rate.""" + self._stale_timeout_sec = stale_timeout_sec + self._expected_rate_hz = expected_rate_hz + + @property + def stale_timeout_sec(self) -> float: + """Return the age after which the heartbeat is considered stale.""" + return self._stale_timeout_sec + + def build( + self, + now_ns: int, + last_reading: Optional[ImuReading], + measured_rate_hz: float, + *, + source_description: str, + ) -> DiagnosticArray: + """ + Assemble the diagnostics for the current moment. + + ``now_ns`` is the current time in nanoseconds, ``last_reading`` the most + recent sample or ``None`` if none has arrived, ``measured_rate_hz`` the + publish rate observed since the last report, and ``source_description`` + a short description of the active data source. + """ + array = DiagnosticArray() + array.header.stamp = to_time_msg(now_ns) + array.status.append( + self._heartbeat_status(now_ns, last_reading, source_description) + ) + array.status.append(self._rate_status(measured_rate_hz, last_reading is not None)) + + if last_reading is not None: + array.status.append( + self._value_status( + ANGULAR_VELOCITY_NAME, + _magnitude(last_reading.angular_velocity), + 'rad/s', + 'Gyroscope magnitude', + ) + ) + array.status.append( + self._value_status( + LINEAR_ACCELERATION_NAME, + _magnitude(last_reading.linear_acceleration), + 'm/s^2', + 'Accelerometer magnitude, including gravity', + ) + ) + + return array + + def _heartbeat_status( + self, + now_ns: int, + last_reading: Optional[ImuReading], + source_description: str, + ) -> DiagnosticStatus: + """Build the ``imu.heartbeat`` row required by issue #4.""" + status = DiagnosticStatus() + status.name = HEARTBEAT_NAME + status.hardware_id = HARDWARE_ID + + if last_reading is None: + status.level = DiagnosticStatus.STALE + status.message = f'No IMU samples received from {source_description}' + status.values = _key_values('never', 's') + return status + + age_sec = max(0.0, (now_ns - last_reading.stamp_ns) / 1e9) + status.values = _key_values(f'{age_sec:.2f}', 's') + + if age_sec > self._stale_timeout_sec: + status.level = DiagnosticStatus.STALE + status.message = ( + f'No IMU sample for {age_sec:.2f} s ' + f'(timeout {self._stale_timeout_sec:.2f} s)' + ) + else: + status.level = DiagnosticStatus.OK + status.message = f'IMU streaming from {source_description}' + + return status + + def _rate_status(self, measured_rate_hz: float, has_data: bool) -> DiagnosticStatus: + """Build the ``imu.rate`` row comparing measured against expected rate.""" + status = DiagnosticStatus() + status.name = RATE_NAME + status.hardware_id = HARDWARE_ID + status.values = _key_values(f'{measured_rate_hz:.1f}', 'Hz') + + if not has_data: + status.level = DiagnosticStatus.STALE + status.message = 'No IMU samples to measure' + return status + + # Anything below about 80% of the configured rate is worth surfacing but + # is not a fault on its own, since the publisher is still alive. + if measured_rate_hz < self._expected_rate_hz * 0.8: + status.level = DiagnosticStatus.WARN + status.message = ( + f'Publishing at {measured_rate_hz:.1f} Hz, ' + f'below the configured {self._expected_rate_hz:.1f} Hz' + ) + else: + status.level = DiagnosticStatus.OK + status.message = f'Publishing at {measured_rate_hz:.1f} Hz' + + return status + + def _value_status( + self, + name: str, + value: float, + unit: str, + message: str, + ) -> DiagnosticStatus: + """Build an informational telemetry row with an unambiguous unit.""" + status = DiagnosticStatus() + status.name = name + status.hardware_id = HARDWARE_ID + status.level = DiagnosticStatus.OK + status.message = message + status.values = _key_values(f'{value:.3f}', unit) + return status diff --git a/waybionic_sensors/waybionic_sensors/imu_messages.py b/waybionic_sensors/waybionic_sensors/imu_messages.py new file mode 100644 index 0000000..bc0b802 --- /dev/null +++ b/waybionic_sensors/waybionic_sensors/imu_messages.py @@ -0,0 +1,144 @@ +""" +Builds ``sensor_msgs/msg/Imu`` messages from :class:`ImuReading` values. + +Isolated from the node so the message contract, and especially the raw versus +fused orientation rules, can be asserted directly in unit tests. + +Covariance layout +----------------- +Each covariance field is a row-major 3x3 matrix. Only the diagonal is populated: +the mock axes are modelled as uncorrelated, so the off-diagonal terms are zero +because they are genuinely believed to be zero, not because they are unknown. +Real per-axis values should come from the sensor datasheet or from a bench +characterisation once the hardware is chosen. + +Unavailable orientation +----------------------- +``sensor_msgs/msg/Imu`` defines ``orientation_covariance[0] = -1`` as "this +message does not contain orientation". The raw topic always sets that, because +an accelerometer and a gyroscope alone cannot observe absolute heading. +""" + +from typing import Tuple + +from builtin_interfaces.msg import Time +from geometry_msgs.msg import TransformStamped +from sensor_msgs.msg import Imu + +from waybionic_sensors.imu_reading import ImuReading, Quaternion + +ORIENTATION_UNAVAILABLE = -1.0 +"""Value placed in ``orientation_covariance[0]`` to mark orientation absent.""" + +IDENTITY_QUATERNION: Quaternion = (0.0, 0.0, 0.0, 1.0) + + +def to_time_msg(stamp_ns: int) -> Time: + """Convert integer nanoseconds into a ``builtin_interfaces/msg/Time``.""" + return Time(sec=int(stamp_ns // 1_000_000_000), nanosec=int(stamp_ns % 1_000_000_000)) + + +def diagonal_covariance(stddev: float) -> list: + """Return a row-major 3x3 covariance with ``stddev**2`` on the diagonal.""" + variance = float(stddev) ** 2 + return [ + variance, 0.0, 0.0, + 0.0, variance, 0.0, + 0.0, 0.0, variance, + ] + + +def build_raw_imu_message( + reading: ImuReading, + frame_id: str, + *, + angular_velocity_stddev: float, + linear_acceleration_stddev: float, +) -> Imu: + """ + Build the raw message for ``/waybionic/imu/data_raw``. + + Orientation is always marked unavailable here, even if the reading happens + to carry one, because this topic is defined as un-fused sensor output. + """ + message = Imu() + message.header.stamp = to_time_msg(reading.stamp_ns) + message.header.frame_id = frame_id + + message.orientation.x = IDENTITY_QUATERNION[0] + message.orientation.y = IDENTITY_QUATERNION[1] + message.orientation.z = IDENTITY_QUATERNION[2] + message.orientation.w = IDENTITY_QUATERNION[3] + message.orientation_covariance = [0.0] * 9 + message.orientation_covariance[0] = ORIENTATION_UNAVAILABLE + + message.angular_velocity.x = float(reading.angular_velocity[0]) + message.angular_velocity.y = float(reading.angular_velocity[1]) + message.angular_velocity.z = float(reading.angular_velocity[2]) + message.angular_velocity_covariance = diagonal_covariance(angular_velocity_stddev) + + message.linear_acceleration.x = float(reading.linear_acceleration[0]) + message.linear_acceleration.y = float(reading.linear_acceleration[1]) + message.linear_acceleration.z = float(reading.linear_acceleration[2]) + message.linear_acceleration_covariance = diagonal_covariance(linear_acceleration_stddev) + + return message + + +def build_demo_orientation_message( + reading: ImuReading, + frame_id: str, + orientation: Quaternion, + *, + orientation_stddev: float, + angular_velocity_stddev: float, + linear_acceleration_stddev: float, +) -> Imu: + """ + Build the clearly separated demo/fused message. + + The orientation here is generated for visualisation. It is published on its + own topic so that nothing subscribing to the raw topic can mistake it for a + measured attitude. + """ + message = build_raw_imu_message( + reading, + frame_id, + angular_velocity_stddev=angular_velocity_stddev, + linear_acceleration_stddev=linear_acceleration_stddev, + ) + + message.orientation.x = float(orientation[0]) + message.orientation.y = float(orientation[1]) + message.orientation.z = float(orientation[2]) + message.orientation.w = float(orientation[3]) + message.orientation_covariance = diagonal_covariance(orientation_stddev) + + return message + + +def build_demo_transform( + stamp_ns: int, + parent_frame_id: str, + frame_id: str, + orientation: Quaternion, + translation: Tuple[float, float, float] = (0.0, 0.0, 0.1), +) -> TransformStamped: + """ + Build the optional demo TF that rotates ``frame_id`` for visualisation. + + This is a mock aid. Broadcasting it by default would imply the sensor knows + its own attitude, so the node only sends it when explicitly enabled. + """ + transform = TransformStamped() + transform.header.stamp = to_time_msg(stamp_ns) + transform.header.frame_id = parent_frame_id + transform.child_frame_id = frame_id + transform.transform.translation.x = float(translation[0]) + transform.transform.translation.y = float(translation[1]) + transform.transform.translation.z = float(translation[2]) + transform.transform.rotation.x = float(orientation[0]) + transform.transform.rotation.y = float(orientation[1]) + transform.transform.rotation.z = float(orientation[2]) + transform.transform.rotation.w = float(orientation[3]) + return transform diff --git a/waybionic_sensors/waybionic_sensors/imu_publisher_node.py b/waybionic_sensors/waybionic_sensors/imu_publisher_node.py new file mode 100644 index 0000000..a6c894a --- /dev/null +++ b/waybionic_sensors/waybionic_sensors/imu_publisher_node.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python3 +""" +ROS 2 node publishing WayBionic IMU data and sensor health. + +The node only wires components together: sample generation lives in +:mod:`waybionic_sensors.mock_source` or a future +:class:`~waybionic_sensors.hardware_reader.ImuHardwareReader`, message +construction in :mod:`waybionic_sensors.imu_messages`, and health reporting in +:mod:`waybionic_sensors.imu_diagnostics`. + +Topics +------ +``~topic`` (default ``/waybionic/imu/data_raw``) + ``sensor_msgs/msg/Imu`` with gyroscope and accelerometer data. Orientation is + always marked unavailable here. +``~demo_orientation_topic`` (default ``/waybionic/imu/data_demo``) + Only advertised when ``publish_demo_orientation`` is true. Carries a + synthetic orientation for visualisation. +``/diagnostics`` + ``diagnostic_msgs/msg/DiagnosticArray`` including ``imu.heartbeat``. +""" + +from diagnostic_msgs.msg import DiagnosticArray +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Imu +from tf2_ros import TransformBroadcaster + +from waybionic_sensors.hardware_reader import UnconfiguredImuReader +from waybionic_sensors.imu_diagnostics import ImuDiagnosticsBuilder +from waybionic_sensors.imu_messages import ( + build_demo_orientation_message, + build_demo_transform, + build_raw_imu_message, +) +from waybionic_sensors.mock_source import MockImuSource + + +class ImuPublisher(Node): + """Publishes IMU samples, an optional demo orientation, and health.""" + + def __init__(self, **kwargs) -> None: + """ + Declare parameters, build the data source, and start the timers. + + Extra keyword arguments are forwarded to :class:`rclpy.node.Node`, which + lets tests supply ``parameter_overrides`` without a launch file. + """ + super().__init__('waybionic_imu_publisher', **kwargs) + + self._declare_parameters() + + self._frame_id = self._string_param('frame_id') + self._parent_frame_id = self._string_param('parent_frame_id') + self._publish_demo_orientation = self._bool_param('publish_demo_orientation') + self._publish_demo_tf = self._bool_param('publish_demo_tf') + self._angular_velocity_stddev = self._double_param('angular_velocity_stddev') + self._linear_acceleration_stddev = self._double_param('linear_acceleration_stddev') + self._orientation_stddev = self._double_param('orientation_stddev') + + rate_hz = self._double_param('publish_rate_hz') + self._publish_rate_hz = rate_hz if rate_hz > 0.0 else 50.0 + + use_mock = self._bool_param('use_mock') + self._mock_source = None + self._hardware_reader = None + if use_mock: + self._mock_source = MockImuSource( + stall_after_sec=self._double_param('mock_stall_after_sec'), + ) + self._source_description = 'mock generator' + else: + self._hardware_reader = UnconfiguredImuReader(self._string_param('serial_port')) + self._hardware_reader.start() + self._source_description = self._hardware_reader.describe() + + self._imu_publisher = self.create_publisher(Imu, self._string_param('topic'), 10) + self._demo_publisher = None + if self._publish_demo_orientation: + self._demo_publisher = self.create_publisher( + Imu, self._string_param('demo_orientation_topic'), 10 + ) + + self._tf_broadcaster = TransformBroadcaster(self) if self._publish_demo_tf else None + + self._diagnostics_publisher = self.create_publisher( + DiagnosticArray, self._string_param('diagnostics_topic'), 10 + ) + self._diagnostics_builder = ImuDiagnosticsBuilder( + stale_timeout_sec=self._double_param('stale_timeout_sec'), + expected_rate_hz=self._publish_rate_hz, + ) + + self._last_reading = None + self._samples_since_report = 0 + self._last_report_ns = self._now_ns() + + self._sample_timer = self.create_timer(1.0 / self._publish_rate_hz, self._on_sample_timer) + + diagnostics_period = 1.0 / max(1.0, self._double_param('diagnostics_rate_hz')) + self._diagnostics_timer = self.create_timer( + diagnostics_period, self._on_diagnostics_timer + ) + + self._log_startup(use_mock) + + def _declare_parameters(self) -> None: + """Declare every runtime parameter with its default.""" + self.declare_parameter('use_mock', True) + self.declare_parameter('topic', '/waybionic/imu/data_raw') + self.declare_parameter('demo_orientation_topic', '/waybionic/imu/data_demo') + self.declare_parameter('diagnostics_topic', '/diagnostics') + self.declare_parameter('frame_id', 'imu_link') + self.declare_parameter('parent_frame_id', 'base_link') + self.declare_parameter('publish_rate_hz', 50.0) + self.declare_parameter('diagnostics_rate_hz', 2.0) + self.declare_parameter('stale_timeout_sec', 1.0) + # Both demo outputs default off: they are visualisation aids and would + # otherwise imply the raw sensor knows its own attitude. + self.declare_parameter('publish_demo_orientation', False) + self.declare_parameter('publish_demo_tf', False) + # Placeholder noise figures. Replace with datasheet or bench values once + # the sensor is chosen; see docs/HARDWARE_INTERFACE.md. + self.declare_parameter('angular_velocity_stddev', 0.01) + self.declare_parameter('linear_acceleration_stddev', 0.05) + self.declare_parameter('orientation_stddev', 0.05) + self.declare_parameter('mock_stall_after_sec', 0.0) + self.declare_parameter('serial_port', '') + + def _bool_param(self, name: str) -> bool: + """Read a declared boolean parameter.""" + return self.get_parameter(name).get_parameter_value().bool_value + + def _string_param(self, name: str) -> str: + """Read a declared string parameter.""" + return self.get_parameter(name).get_parameter_value().string_value + + def _double_param(self, name: str) -> float: + """Read a declared double parameter.""" + return self.get_parameter(name).get_parameter_value().double_value + + def _now_ns(self) -> int: + """Return the current node time in nanoseconds.""" + return self.get_clock().now().nanoseconds + + def _log_startup(self, use_mock: bool) -> None: + """Log the active configuration so the mode is obvious in the console.""" + logger = self.get_logger() + logger.info( + f'IMU source: {self._source_description}; ' + f'publishing {self._string_param("topic")} ' + f'at {self._publish_rate_hz:.1f} Hz in frame {self._frame_id}' + ) + logger.info( + 'Raw topic marks orientation unavailable ' + '(orientation_covariance[0] = -1); demo orientation ' + f'{"enabled" if self._publish_demo_orientation else "disabled"}, ' + f'demo TF {"enabled" if self._publish_demo_tf else "disabled"}' + ) + if not use_mock: + logger.warning( + 'Live mode selected but no hardware driver is implemented yet. ' + 'imu.heartbeat will report STALE until a real reader is supplied.' + ) + if self._mock_source is not None and self._mock_source.stalled_deliberately: + logger.warning( + 'mock_stall_after_sec is set; the mock will stop publishing so ' + 'the stale heartbeat path can be demonstrated.' + ) + + def _read(self, stamp_ns: int): + """Return the newest sample from whichever source is configured.""" + if self._mock_source is not None: + return self._mock_source.read(stamp_ns) + return self._hardware_reader.read(stamp_ns) + + def _on_sample_timer(self) -> None: + """Acquire one sample and publish the raw, demo, and TF outputs.""" + stamp_ns = self._now_ns() + reading = self._read(stamp_ns) + if reading is None: + return + + self._last_reading = reading + self._samples_since_report += 1 + + self._imu_publisher.publish( + build_raw_imu_message( + reading, + self._frame_id, + angular_velocity_stddev=self._angular_velocity_stddev, + linear_acceleration_stddev=self._linear_acceleration_stddev, + ) + ) + + if self._demo_publisher is None and self._tf_broadcaster is None: + return + + orientation = self._demo_orientation(reading) + if orientation is None: + return + + if self._demo_publisher is not None: + self._demo_publisher.publish( + build_demo_orientation_message( + reading, + self._frame_id, + orientation, + orientation_stddev=self._orientation_stddev, + angular_velocity_stddev=self._angular_velocity_stddev, + linear_acceleration_stddev=self._linear_acceleration_stddev, + ) + ) + + if self._tf_broadcaster is not None: + self._tf_broadcaster.sendTransform( + build_demo_transform( + reading.stamp_ns, + self._parent_frame_id, + self._frame_id, + orientation, + ) + ) + + def _demo_orientation(self, reading): + """Return the orientation to use for demo outputs, if one is available.""" + if reading.has_orientation: + return reading.orientation + if self._mock_source is not None: + return self._mock_source.demo_orientation(reading.stamp_ns) + return None + + def _on_diagnostics_timer(self) -> None: + """Publish IMU health, including the heartbeat, at a steady rate.""" + now_ns = self._now_ns() + elapsed_sec = max(1e-9, (now_ns - self._last_report_ns) / 1e9) + measured_rate_hz = self._samples_since_report / elapsed_sec + + self._diagnostics_publisher.publish( + self._diagnostics_builder.build( + now_ns, + self._last_reading, + measured_rate_hz, + source_description=self._source_description, + ) + ) + + self._samples_since_report = 0 + self._last_report_ns = now_ns + + def destroy_node(self) -> bool: + """Release the hardware reader before the node goes away.""" + if self._hardware_reader is not None: + self._hardware_reader.stop() + return super().destroy_node() + + +def main() -> None: + """Spin the IMU publisher until interrupted.""" + rclpy.init() + node = ImuPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/waybionic_sensors/waybionic_sensors/imu_reading.py b/waybionic_sensors/waybionic_sensors/imu_reading.py new file mode 100644 index 0000000..5c3b8d6 --- /dev/null +++ b/waybionic_sensors/waybionic_sensors/imu_reading.py @@ -0,0 +1,51 @@ +""" +Hardware-independent IMU sample passed between readers and ROS publishing. + +This is the boundary type of the package. A mock generator or a future hardware +driver produces :class:`ImuReading` values, and everything downstream (ROS +message construction, diagnostics, TF) consumes only this type. Adding a real +sensor therefore means writing a reader that returns these values, not editing +the publisher. + +Units and axes follow REP-103: right-handed, x forward, y left, z up, SI units. +""" + +from dataclasses import dataclass +from typing import Optional, Tuple + +Vector3 = Tuple[float, float, float] +Quaternion = Tuple[float, float, float, float] + +GRAVITY_M_S2 = 9.80665 +"""Standard gravity, the value a level accelerometer reports on its z axis.""" + + +@dataclass(frozen=True) +class ImuReading: + """ + One IMU sample. + + ``stamp_ns`` is the acquisition time in nanoseconds since the epoch of the + clock that produced it. + + ``angular_velocity`` is the gyroscope reading in rad/s as ``(x, y, z)``. + + ``linear_acceleration`` is the accelerometer reading in m/s^2 as + ``(x, y, z)``, including gravity, per the ``sensor_msgs/msg/Imu`` + convention. + + ``orientation`` is an absolute orientation as ``(x, y, z, w)``, or ``None`` + when the device provides no fused orientation. Raw accelerometer and + gyroscope data alone never yield an absolute orientation, so this stays + ``None`` unless a real fusion source fills it in. + """ + + stamp_ns: int + angular_velocity: Vector3 + linear_acceleration: Vector3 + orientation: Optional[Quaternion] = None + + @property + def has_orientation(self) -> bool: + """Return whether this sample carries a fused absolute orientation.""" + return self.orientation is not None diff --git a/waybionic_sensors/waybionic_sensors/mock_source.py b/waybionic_sensors/waybionic_sensors/mock_source.py new file mode 100644 index 0000000..049aca7 --- /dev/null +++ b/waybionic_sensors/waybionic_sensors/mock_source.py @@ -0,0 +1,115 @@ +""" +Synthetic IMU sample generation for benchtop work without hardware. + +Kept free of ROS types so the motion model can be unit tested directly and so +the node does not mix data generation with message construction. +""" + +import math +from typing import Optional + +from waybionic_sensors.imu_reading import GRAVITY_M_S2, ImuReading, Quaternion + + +def euler_to_quaternion(roll: float, pitch: float, yaw: float) -> Quaternion: + """Convert intrinsic roll/pitch/yaw in radians to an ``(x, y, z, w)`` quaternion.""" + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + return ( + sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, + cr * cp * sy - sr * sp * cy, + cr * cp * cy + sr * sp * sy, + ) + + +class MockImuSource: + """ + Generates a smooth, repeatable wobble so RViz and the panel have data. + + The motion is a deterministic function of elapsed time, which keeps tests + stable and makes the output obviously synthetic rather than noise that could + be mistaken for a real sensor. + """ + + def __init__( + self, + *, + angular_amplitude: float = 0.20, + linear_amplitude: float = 0.05, + stall_after_sec: float = 0.0, + ) -> None: + """ + Configure the generator. + + ``angular_amplitude`` is the peak angular velocity in rad/s. + + ``linear_amplitude`` is the peak lateral acceleration in m/s^2, on top + of gravity. + + ``stall_after_sec``, when positive, stops sample production that many + seconds after the first one. It exists to demonstrate the stale + heartbeat path without unplugging anything. + """ + self._angular_amplitude = angular_amplitude + self._linear_amplitude = linear_amplitude + self._stall_after_sec = stall_after_sec + self._start_ns: Optional[int] = None + + @property + def stalled_deliberately(self) -> bool: + """Return whether a stall was configured for stale-path demonstrations.""" + return self._stall_after_sec > 0.0 + + def elapsed_sec(self, stamp_ns: int) -> float: + """Return seconds since the first sample, given the current timestamp.""" + if self._start_ns is None: + return 0.0 + return (stamp_ns - self._start_ns) / 1e9 + + def read(self, stamp_ns: int) -> Optional[ImuReading]: + """Produce the sample for ``stamp_ns``, or ``None`` once stalled.""" + if self._start_ns is None: + self._start_ns = stamp_ns + + elapsed = self.elapsed_sec(stamp_ns) + if self._stall_after_sec > 0.0 and elapsed > self._stall_after_sec: + return None + + slow = math.sin(elapsed * 0.5) + medium = math.cos(elapsed * 0.8) + + angular_velocity = ( + self._angular_amplitude * 0.5 * slow, + self._angular_amplitude * 0.25 * medium, + self._angular_amplitude * slow, + ) + linear_acceleration = ( + self._linear_amplitude * slow, + self._linear_amplitude * 0.4 * medium, + GRAVITY_M_S2, + ) + + return ImuReading( + stamp_ns=stamp_ns, + angular_velocity=angular_velocity, + linear_acceleration=linear_acceleration, + orientation=None, + ) + + def demo_orientation(self, stamp_ns: int) -> Quaternion: + """ + Return a synthetic orientation for the demo topic and demo TF. + + This is a display aid only. It is not derived from the accelerometer or + gyroscope values above and must never be published on the raw topic. + """ + elapsed = self.elapsed_sec(stamp_ns) + roll = 0.10 * math.sin(elapsed * 0.5) + pitch = 0.05 * math.sin(elapsed * 0.33) + yaw = 0.20 * math.sin(elapsed * 0.5) + return euler_to_quaternion(roll, pitch, yaw) From b903bc05b2738f02980a2cf275e5dc2ae66a60c2 Mon Sep 17 00:00:00 2001 From: Yassin Soliman <108886216+yassinsolim@users.noreply.github.com> Date: Sat, 22 Aug 2026 12:27:10 -0600 Subject: [PATCH 2/7] Pin setuptools for macOS ROS Python workflow --- robostack.yaml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/robostack.yaml b/robostack.yaml index fac5030..9c3e368 100644 --- a/robostack.yaml +++ b/robostack.yaml @@ -16,4 +16,7 @@ dependencies: - pkg-config - make - ninja - - pytest <9 \ No newline at end of file + - 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 From 3b4f1c25ddfca11432305a70bedcd3d27d19079a Mon Sep 17 00:00:00 2001 From: Khuzaymah Bin Haris Date: Fri, 28 Aug 2026 17:07:01 -0600 Subject: [PATCH 3/7] Fix PR #11 RViz plugin, stale telemetry, and covariance Jazzy does not ship rviz_default_plugins/Imu, so the demo uses rviz_imu_plugin on /waybionic/imu/data_demo. Stale samples now mark gyro and accel STALE, and raw covariance stays unknown until a datasheet value is supplied. Refs #11 --- robostack.yaml | 1 + waybionic_sensors/README.md | 11 +++- waybionic_sensors/config/imu_demo.rviz | 28 ++++++--- waybionic_sensors/docs/HARDWARE_INTERFACE.md | 7 ++- waybionic_sensors/docs/IMU_CONTRACT.md | 36 +++++++----- waybionic_sensors/docs/PR_NOTES.md | 49 ++++++++++------ waybionic_sensors/launch/imu_demo.launch.py | 3 +- .../launch/imu_publisher.launch.py | 13 +++-- waybionic_sensors/package.xml | 1 + .../test/test_imu_diagnostics.py | 24 ++++++++ waybionic_sensors/test/test_imu_messages.py | 36 ++++++++---- .../test/test_imu_publisher_node.py | 57 +++++++++++++++++-- .../test/test_package_metadata.py | 12 ++++ .../waybionic_sensors/imu_diagnostics.py | 51 +++++++++++++---- .../waybionic_sensors/imu_messages.py | 44 +++++++++++--- .../waybionic_sensors/imu_publisher_node.py | 9 +-- 16 files changed, 293 insertions(+), 89 deletions(-) diff --git a/robostack.yaml b/robostack.yaml index 9c3e368..f4f5a0d 100644 --- a/robostack.yaml +++ b/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 diff --git a/waybionic_sensors/README.md b/waybionic_sensors/README.md index 36b7113..ab328b2 100644 --- a/waybionic_sensors/README.md +++ b/waybionic_sensors/README.md @@ -36,6 +36,11 @@ 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 @@ -53,8 +58,10 @@ stale without unplugging anything: ros2 launch waybionic_sensors imu_publisher.launch.py mock_stall_after_sec:=5.0 ``` -The mock stops after five seconds, and the row turns STALE once the sample age -passes `stale_timeout_sec`. +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. ## What is measured and what is generated diff --git a/waybionic_sensors/config/imu_demo.rviz b/waybionic_sensors/config/imu_demo.rviz index e558e15..772ff40 100644 --- a/waybionic_sensors/config/imu_demo.rviz +++ b/waybionic_sensors/config/imu_demo.rviz @@ -38,22 +38,34 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Class: rviz_default_plugins/Imu + - 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 acceleration (raw) + Name: IMU orientation (demo) Topic: Depth: 10 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /waybionic/imu/data_raw + Value: /waybionic/imu/data_demo Value: true - alpha: 1 - color: 204; 51; 204 - history_length: 1 - queue_size: 10 - scale: 0.1 + fixed_frame_orientation: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/waybionic_sensors/docs/HARDWARE_INTERFACE.md b/waybionic_sensors/docs/HARDWARE_INTERFACE.md index aeeaf7b..904dbbc 100644 --- a/waybionic_sensors/docs/HARDWARE_INTERFACE.md +++ b/waybionic_sensors/docs/HARDWARE_INTERFACE.md @@ -53,8 +53,11 @@ guessed serial protocol. Until the answers arrive, these stay open and are not guessed anywhere in code: - Wire protocol and framing. `hardware_reader.py` defines an interface only. -- Real covariance values. The mock uses placeholder standard deviations exposed - as parameters (`angular_velocity_stddev`, `linear_acceleration_stddev`). +- Real covariance values. Raw/live gyro and accel covariances stay all-zero + (ROS "unknown") until question 14 is answered. Set + `angular_velocity_stddev` / `linear_acceleration_stddev` to a positive value + once a datasheet or calibration result exists. `orientation_stddev` is a + demo-topic-only placeholder and is never applied to `data_raw`. - Whether a fused orientation will ever be available. Until it is, the raw topic marks orientation unavailable. - The static transform from `base_link` to `imu_link`. The demo TF is a diff --git a/waybionic_sensors/docs/IMU_CONTRACT.md b/waybionic_sensors/docs/IMU_CONTRACT.md index a4e1ca0..f5bf5d3 100644 --- a/waybionic_sensors/docs/IMU_CONTRACT.md +++ b/waybionic_sensors/docs/IMU_CONTRACT.md @@ -52,19 +52,25 @@ The demo topic exists so RViz has something to show. It is off by default, named ## Covariance -Each covariance is a row-major 3x3 matrix carrying `stddev^2` on the diagonal. -Off-diagonal terms are zero because the mock models the axes as uncorrelated, -which is a stated assumption rather than a missing value. +`sensor_msgs/Imu` treats an all-zero 3x3 matrix as **unknown**, not as perfect +certainty. A field that is not present at all uses `covariance[0] = -1`. + +Raw gyroscope and accelerometer covariances therefore stay all-zero until +electrical supplies a datasheet or calibration result. Set +`angular_velocity_stddev` or `linear_acceleration_stddev` to a positive value +to populate `stddev^2` on the diagonal. Off-diagonal terms are then zero +because the axes are modelled as uncorrelated. + +`orientation_stddev` is a **demo-topic-only** placeholder. It is never copied +onto `/waybionic/imu/data_raw`. | Parameter | Default | Meaning | |-----------|---------|---------| -| `angular_velocity_stddev` | `0.01` | Assumed gyroscope noise, rad/s | -| `linear_acceleration_stddev` | `0.05` | Assumed accelerometer noise, m/s^2 | -| `orientation_stddev` | `0.05` | Assumed demo orientation noise, rad | +| `angular_velocity_stddev` | `0.0` | Unknown gyroscope noise until a datasheet/calibration value is set, rad/s | +| `linear_acceleration_stddev` | `0.0` | Unknown accelerometer noise until a datasheet/calibration value is set, m/s^2 | +| `orientation_stddev` | `0.05` | Synthetic demo orientation noise, rad. Not applied to the raw topic. | -These are placeholders chosen to be plausible for a consumer MEMS IMU. They are -parameters precisely so they can be replaced with datasheet or bench values -without touching code. See `HARDWARE_INTERFACE.md` question 14. +See `HARDWARE_INTERFACE.md` question 14. ## Diagnostics @@ -75,9 +81,9 @@ DiagnosticsPanel renders these without IMU-specific code. | Signal | Unit | Levels | Meaning | |--------|------|--------|---------| | `imu.heartbeat` | `s` | OK, STALE | Age of the newest sample. STALE past `stale_timeout_sec`, or when none ever arrived. | -| `imu.rate` | `Hz` | OK, WARN, STALE | Measured publish rate. WARN below 80% of the configured rate. | -| `imu.angular_velocity` | `rad/s` | OK | Gyroscope vector magnitude. | -| `imu.linear_acceleration` | `m/s^2` | OK | Accelerometer vector magnitude, including gravity. | +| `imu.rate` | `Hz` | OK, WARN, STALE | Measured publish rate. WARN below 80% of the configured rate while samples are still fresh. STALE when samples have stopped. | +| `imu.angular_velocity` | `rad/s` | OK, STALE | Gyroscope vector magnitude. STALE when the last sample is older than `stale_timeout_sec`; last value is still shown. | +| `imu.linear_acceleration` | `m/s^2` | OK, STALE | Accelerometer vector magnitude, including gravity. STALE with the last value when samples stop. | Published at `diagnostics_rate_hz`, default 2 Hz, which satisfies the at-least-1-Hz requirement in issue #4. @@ -97,8 +103,8 @@ at-least-1-Hz requirement in issue #4. | `stale_timeout_sec` | `1.0` | Heartbeat staleness threshold | | `publish_demo_orientation` | `false` | Enable the demo topic | | `publish_demo_tf` | `false` | Enable the demo TF | -| `angular_velocity_stddev` | `0.01` | Gyroscope noise assumption | -| `linear_acceleration_stddev` | `0.05` | Accelerometer noise assumption | -| `orientation_stddev` | `0.05` | Demo orientation noise assumption | +| `angular_velocity_stddev` | `0.0` | Gyroscope noise; 0 means unknown covariance | +| `linear_acceleration_stddev` | `0.0` | Accelerometer noise; 0 means unknown covariance | +| `orientation_stddev` | `0.05` | Demo-topic-only orientation noise assumption | | `mock_stall_after_sec` | `0.0` | Stop the mock to demonstrate stale, 0 disables | | `serial_port` | `''` | Reserved for the future driver | diff --git a/waybionic_sensors/docs/PR_NOTES.md b/waybionic_sensors/docs/PR_NOTES.md index 9d11fba..6f402c6 100644 --- a/waybionic_sensors/docs/PR_NOTES.md +++ b/waybionic_sensors/docs/PR_NOTES.md @@ -13,16 +13,28 @@ CI, `CONTRIBUTING.md`, and other files that landed in the meantime. Only the `waybionic_sensors` directory was carried across, and the publisher was then rewritten against the review feedback. +## PR #11 review fixes + +Requested changes on the open IMU PR, without expanding scope: + +| Request | Fix | +|---------|-----| +| Jazzy has no `rviz_default_plugins/Imu` | `imu_demo.rviz` uses `rviz_imu_plugin/Imu`; `package.xml` and `robostack.yaml` declare the dependency so `rosdep install` / macOS setup pull it | +| Display subscribed to `data_raw` | Orientation display now subscribes to `/waybionic/imu/data_demo` | +| Stale gyro/accel left OK | When samples age out, `imu.angular_velocity` and `imu.linear_acceleration` report STALE (last values still shown). Rate also goes STALE, not just WARN | +| No regression for the stall path | `test_stale_stall_marks_heartbeat_rate_and_telemetry` plus unit tests on the diagnostics builder | +| Placeholder stddev implied confidence | Raw/live gyro and accel covariances default to all-zero (ROS unknown). Positive `*_stddev` is opt-in for datasheet/calibration. Synthetic orientation covariance stays on the demo topic only | + ## What changed relative to the old IMU branch | Old behaviour | Problem | Now | |---------------|---------|-----| | Synthetic quaternion published on `data_raw` | Presented generated data as a measurement | Raw topic sets `orientation_covariance[0] = -1`; synthetic orientation moved to `/waybionic/imu/data_demo`, off by default | | Rotating TF always broadcast | Implied the sensor knows its own attitude | `publish_demo_tf`, default false; enabled only by `imu_demo.launch.py` | -| All covariances left at zero | Zero means "perfectly certain" to a consumer | Diagonal covariances from parameterised standard deviations, documented as placeholders | +| Covariances all zero | Zero was misread as "perfectly certain" | Raw gyro/accel now stay unknown (all-zero) until a datasheet stddev is supplied; demo orientation covariance is synthetic and demo-only | | No `/diagnostics` output | Panel could not show IMU health | `imu.heartbeat` plus rate and telemetry rows at 2 Hz | | One 120-line node doing everything | Serial work would have to be bolted into the publisher | Six modules: reading type, mock source, hardware boundary, message builder, diagnostics builder, node | -| Three metadata tests | No behavioural coverage | 83 tests including a runtime suite that spins the node | +| Three metadata tests | No behavioural coverage | 92 tests including a runtime suite that spins the node | | `serial_port` parameter with no reader | Suggested a driver existed | Documented boundary plus a stub that makes the missing driver visible in diagnostics | ## Raw versus fused orientation @@ -43,19 +55,19 @@ a real fusion source exists. ## Covariance -Row-major 3x3 with `stddev^2` on the diagonal. Off-diagonals are zero because -the mock models the axes as uncorrelated, which is a stated assumption rather -than an unknown left blank. +`sensor_msgs/Imu` treats an all-zero matrix as unknown, not as perfect +certainty. Raw gyroscope and accelerometer covariances therefore default to +all zeros. A positive `angular_velocity_stddev` or +`linear_acceleration_stddev` fills `stddev^2` on the diagonal once electrical +supplies a datasheet or calibration value. | Parameter | Default | Meaning | |-----------|---------|---------| -| `angular_velocity_stddev` | `0.01` | rad/s | -| `linear_acceleration_stddev` | `0.05` | m/s^2 | +| `angular_velocity_stddev` | `0.0` | rad/s; 0 = unknown | +| `linear_acceleration_stddev` | `0.0` | m/s^2; 0 = unknown | | `orientation_stddev` | `0.05` | rad, demo topic only | -Plausible consumer-MEMS placeholders, exposed as parameters so datasheet values -can replace them without a code change. Tracked as question 14 in -`docs/HARDWARE_INTERFACE.md`. +Tracked as question 14 in `docs/HARDWARE_INTERFACE.md`. ## Module boundaries @@ -105,9 +117,9 @@ header: orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] angular_velocity: {x: -0.0736..., y: -0.0383..., z: -0.1473...} -angular_velocity_covariance: [0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001] +angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] linear_acceleration: {x: -0.0368..., y: -0.0153..., z: 9.80665} -linear_acceleration_covariance: [0.0025, ...] +linear_acceleration_covariance: [0.0, ...] ``` `ros2 topic hz /diagnostics` gives `average rate: 2.000`, comfortably above the @@ -148,22 +160,23 @@ colcon test --packages-select waybionic_sensors colcon test-result --all --verbose ``` -83 tests, 0 failures. +92 tests, 0 failures. | Suite | Count | Covers | |-------|-------|--------| -| `test_imu_messages.py` | 15 | Frame, timestamp, orientation-unavailable marker, covariance diagonals and cross terms, demo message, demo TF | -| `test_imu_diagnostics.py` | 16 | Heartbeat OK/STALE, custom timeout, never-received, age units, rate WARN, telemetry units, absence of roll/pitch/yaw | -| `test_imu_publisher_node.py` | 14 | Runtime: rate follows the parameter, monotonic timestamps, frame IDs, demo defaults off, demo TF on request, heartbeat OK then STALE, live mode without hardware | +| `test_imu_messages.py` | 17 | Frame, timestamp, orientation-unavailable marker, unknown vs datasheet covariance, demo message, demo TF | +| `test_imu_diagnostics.py` | 19 | Heartbeat OK/STALE, custom timeout, never-received, age units, rate WARN/STALE, telemetry OK then STALE, absence of roll/pitch/yaw | +| `test_imu_publisher_node.py` | 16 | Runtime: rate, timestamps, frame IDs, demo defaults, unknown covariance, heartbeat OK then STALE, stall marks all four signals, live mode without hardware | | `test_mock_source.py` | 13 | Determinism, gravity, amplitude bounds, stall behaviour, quaternion normalisation | | `test_hardware_reader.py` | 9 | Interface surface, stub behaviour, a custom reader satisfying the boundary | -| `test_package_metadata.py` | 14 | Module separation, node delegation, launch defaults, docs, entry point, no invented protocol | +| `test_package_metadata.py` | 16 | Module separation, node delegation, launch defaults, `rviz_imu_plugin` on `data_demo`, docs, entry point, no invented protocol | | `test_flake8.py`, `test_pep257.py` | 2 | Style and docstrings | ## Known limitations - No physical IMU driver. Blocked on `docs/HARDWARE_INTERFACE.md`. -- Covariance values are documented placeholders, not measured noise. +- Covariance values on the raw topic stay unknown until electrical answers + question 14. `orientation_stddev` is a demo-topic-only placeholder. - The demo orientation and demo TF are visualisation aids, not estimates. - The `base_link` to `imu_link` offset in the demo TF is a placeholder 0.1 m, not a mounting claim. diff --git a/waybionic_sensors/launch/imu_demo.launch.py b/waybionic_sensors/launch/imu_demo.launch.py index fbba776..9216a73 100644 --- a/waybionic_sensors/launch/imu_demo.launch.py +++ b/waybionic_sensors/launch/imu_demo.launch.py @@ -3,7 +3,8 @@ This launch deliberately turns on the synthetic orientation and the rotating TF so there is something to look at in RViz. Both are visualisation aids and are -off by default in imu_publisher.launch.py. +off by default in imu_publisher.launch.py. The RViz config loads +``rviz_imu_plugin/Imu`` on ``/waybionic/imu/data_demo``. """ from launch import LaunchDescription diff --git a/waybionic_sensors/launch/imu_publisher.launch.py b/waybionic_sensors/launch/imu_publisher.launch.py index edcbd99..b5f431a 100644 --- a/waybionic_sensors/launch/imu_publisher.launch.py +++ b/waybionic_sensors/launch/imu_publisher.launch.py @@ -28,12 +28,15 @@ 'Publish a synthetic orientation on the demo topic. Visualisation aid only.', bool), ('publish_demo_tf', 'false', 'Broadcast a rotating demo TF for the IMU frame. Visualisation aid only.', bool), - ('angular_velocity_stddev', '0.01', - 'Assumed gyroscope noise standard deviation in rad/s.', float), - ('linear_acceleration_stddev', '0.05', - 'Assumed accelerometer noise standard deviation in m/s^2.', float), + ('angular_velocity_stddev', '0.0', + 'Gyroscope noise stddev in rad/s. 0 means unknown (all-zero covariance).', + float), + ('linear_acceleration_stddev', '0.0', + 'Accelerometer noise stddev in m/s^2. 0 means unknown (all-zero covariance).', + float), ('orientation_stddev', '0.05', - 'Assumed orientation standard deviation in rad, demo topic only.', float), + 'Demo-topic-only orientation stddev in rad. Not applied to the raw topic.', + float), ('mock_stall_after_sec', '0.0', 'Stop the mock after this many seconds to demonstrate the stale heartbeat. ' '0 disables stalling.', float), diff --git a/waybionic_sensors/package.xml b/waybionic_sensors/package.xml index 2c17d8e..47a69a0 100644 --- a/waybionic_sensors/package.xml +++ b/waybionic_sensors/package.xml @@ -19,6 +19,7 @@ launch launch_ros rviz2 + rviz_imu_plugin ament_flake8 ament_pep257 diff --git a/waybionic_sensors/test/test_imu_diagnostics.py b/waybionic_sensors/test/test_imu_diagnostics.py index cc4c8c2..c1eecde 100644 --- a/waybionic_sensors/test/test_imu_diagnostics.py +++ b/waybionic_sensors/test/test_imu_diagnostics.py @@ -126,6 +126,30 @@ def test_telemetry_rows_report_vector_magnitudes(): ) < 0.001 +def test_telemetry_rows_are_ok_when_samples_are_fresh(): + array = build(make_reading(0.02)) + assert status_named(array, ANGULAR_VELOCITY_NAME).level == DiagnosticStatus.OK + assert status_named(array, LINEAR_ACCELERATION_NAME).level == DiagnosticStatus.OK + assert status_named(array, RATE_NAME).level == DiagnosticStatus.OK + + +def test_telemetry_rows_go_stale_when_sample_ages_out(): + array = build(make_reading(2.5)) + gyro = status_named(array, ANGULAR_VELOCITY_NAME) + accel = status_named(array, LINEAR_ACCELERATION_NAME) + assert gyro.level == DiagnosticStatus.STALE + assert accel.level == DiagnosticStatus.STALE + assert 'stale' in gyro.message + assert 'stale' in accel.message + assert float(value_of(gyro)) == 3.0 + assert abs(float(value_of(accel)) - GRAVITY_M_S2) < 0.001 + + +def test_rate_row_is_stale_when_sample_is_stale(): + status = status_named(build(make_reading(2.5), measured_rate_hz=0.0), RATE_NAME) + assert status.level == DiagnosticStatus.STALE + + def test_telemetry_rows_are_omitted_before_any_sample(): array = build(None) assert status_named(array, ANGULAR_VELOCITY_NAME) is None diff --git a/waybionic_sensors/test/test_imu_messages.py b/waybionic_sensors/test/test_imu_messages.py index b556bb9..f2759fa 100644 --- a/waybionic_sensors/test/test_imu_messages.py +++ b/waybionic_sensors/test/test_imu_messages.py @@ -4,9 +4,11 @@ build_demo_orientation_message, build_demo_transform, build_raw_imu_message, + covariance_from_stddev, diagonal_covariance, ORIENTATION_UNAVAILABLE, to_time_msg, + UNKNOWN_COVARIANCE, ) from waybionic_sensors.imu_reading import GRAVITY_M_S2, ImuReading @@ -26,13 +28,13 @@ def make_reading(orientation=None) -> ImuReading: ) -def build_raw(): +def build_raw(angular_velocity_stddev=0.0, linear_acceleration_stddev=0.0): """Build the raw message used by most assertions here.""" return build_raw_imu_message( make_reading(), 'imu_link', - angular_velocity_stddev=ANGULAR_STDDEV, - linear_acceleration_stddev=LINEAR_STDDEV, + angular_velocity_stddev=angular_velocity_stddev, + linear_acceleration_stddev=linear_acceleration_stddev, ) @@ -107,16 +109,28 @@ def test_raw_message_copies_linear_acceleration_including_gravity(): assert message.linear_acceleration.z == GRAVITY_M_S2 -def test_raw_message_populates_measurement_covariances(): - message = build_raw() +def test_covariance_from_stddev_zero_is_unknown(): + assert covariance_from_stddev(0.0) == UNKNOWN_COVARIANCE + assert covariance_from_stddev(-1.0) == UNKNOWN_COVARIANCE + + +def test_covariance_from_stddev_positive_is_diagonal(): + assert covariance_from_stddev(0.5) == diagonal_covariance(0.5) + + +def test_raw_message_populates_measurement_covariances_when_stddev_is_set(): + message = build_raw( + angular_velocity_stddev=ANGULAR_STDDEV, + linear_acceleration_stddev=LINEAR_STDDEV, + ) assert message.angular_velocity_covariance[0] == ANGULAR_STDDEV ** 2 assert message.linear_acceleration_covariance[0] == LINEAR_STDDEV ** 2 -def test_raw_message_covariances_are_not_left_at_zero(): +def test_raw_message_uses_unknown_covariance_by_default(): message = build_raw() - assert any(value > 0.0 for value in message.angular_velocity_covariance) - assert any(value > 0.0 for value in message.linear_acceleration_covariance) + assert list(message.angular_velocity_covariance) == UNKNOWN_COVARIANCE + assert list(message.linear_acceleration_covariance) == UNKNOWN_COVARIANCE def test_demo_message_carries_a_usable_orientation(): @@ -125,12 +139,14 @@ def test_demo_message_carries_a_usable_orientation(): 'imu_link', (0.0, 0.0, 0.3826834, 0.9238795), orientation_stddev=ORIENTATION_STDDEV, - angular_velocity_stddev=ANGULAR_STDDEV, - linear_acceleration_stddev=LINEAR_STDDEV, + angular_velocity_stddev=0.0, + linear_acceleration_stddev=0.0, ) assert message.orientation.z == 0.3826834 assert message.orientation_covariance[0] == ORIENTATION_STDDEV ** 2 assert message.orientation_covariance[0] != ORIENTATION_UNAVAILABLE + assert list(message.angular_velocity_covariance) == UNKNOWN_COVARIANCE + assert list(message.linear_acceleration_covariance) == UNKNOWN_COVARIANCE def test_demo_message_keeps_the_same_measurement_fields(): diff --git a/waybionic_sensors/test/test_imu_publisher_node.py b/waybionic_sensors/test/test_imu_publisher_node.py index fdef0d9..b088c79 100644 --- a/waybionic_sensors/test/test_imu_publisher_node.py +++ b/waybionic_sensors/test/test_imu_publisher_node.py @@ -16,8 +16,13 @@ from sensor_msgs.msg import Imu from tf2_msgs.msg import TFMessage -from waybionic_sensors.imu_diagnostics import HEARTBEAT_NAME -from waybionic_sensors.imu_messages import ORIENTATION_UNAVAILABLE +from waybionic_sensors.imu_diagnostics import ( + ANGULAR_VELOCITY_NAME, + HEARTBEAT_NAME, + LINEAR_ACCELERATION_NAME, + RATE_NAME, +) +from waybionic_sensors.imu_messages import ORIENTATION_UNAVAILABLE, UNKNOWN_COVARIANCE from waybionic_sensors.imu_publisher_node import ImuPublisher RAW_TOPIC = '/test/imu/data_raw' @@ -129,11 +134,20 @@ def test_raw_messages_mark_orientation_unavailable(): ) -def test_raw_messages_carry_non_zero_measurement_covariance(): +def test_raw_messages_carry_unknown_measurement_covariance_by_default(): with Harness() as collector: message = collector.raw[0] - assert message.angular_velocity_covariance[0] > 0.0 - assert message.linear_acceleration_covariance[0] > 0.0 + assert list(message.angular_velocity_covariance) == UNKNOWN_COVARIANCE + assert list(message.linear_acceleration_covariance) == UNKNOWN_COVARIANCE + + +def test_raw_messages_use_stddev_when_explicitly_configured(): + with Harness( + angular_velocity_stddev=0.01, linear_acceleration_stddev=0.05 + ) as collector: + message = collector.raw[0] + assert message.angular_velocity_covariance[0] == pytest.approx(0.01 ** 2) + assert message.linear_acceleration_covariance[0] == pytest.approx(0.05 ** 2) def test_publish_rate_follows_the_parameter(): @@ -197,6 +211,15 @@ def test_heartbeat_is_ok_while_the_mock_streams(): assert DiagnosticStatus.OK in levels +def _status_levels_by_name(collector): + """Return the last diagnostic level seen for each IMU signal name.""" + latest = {} + for array in collector.diagnostics: + for status in array.status: + latest[status.name] = status.level + return latest + + def test_heartbeat_goes_stale_after_the_mock_stalls(): with Harness( duration_sec=2.5, mock_stall_after_sec=0.4, stale_timeout_sec=0.5 @@ -211,6 +234,30 @@ def test_heartbeat_goes_stale_after_the_mock_stalls(): assert levels[-1] == DiagnosticStatus.STALE +def test_stale_stall_marks_heartbeat_rate_and_telemetry(): + with Harness( + duration_sec=2.5, mock_stall_after_sec=0.4, stale_timeout_sec=0.5 + ) as collector: + streaming_ok = False + for array in collector.diagnostics: + by_name = {status.name: status.level for status in array.status} + if ( + by_name.get(HEARTBEAT_NAME) == DiagnosticStatus.OK + and by_name.get(RATE_NAME) == DiagnosticStatus.OK + and by_name.get(ANGULAR_VELOCITY_NAME) == DiagnosticStatus.OK + and by_name.get(LINEAR_ACCELERATION_NAME) == DiagnosticStatus.OK + ): + streaming_ok = True + break + assert streaming_ok + + last = _status_levels_by_name(collector) + assert last[HEARTBEAT_NAME] == DiagnosticStatus.STALE + assert last[RATE_NAME] == DiagnosticStatus.STALE + assert last[ANGULAR_VELOCITY_NAME] == DiagnosticStatus.STALE + assert last[LINEAR_ACCELERATION_NAME] == DiagnosticStatus.STALE + + def test_live_mode_without_hardware_reports_stale_and_publishes_nothing(): with Harness(use_mock=False) as collector: assert collector.raw == [] diff --git a/waybionic_sensors/test/test_package_metadata.py b/waybionic_sensors/test/test_package_metadata.py index c78fce0..3b19e2b 100644 --- a/waybionic_sensors/test/test_package_metadata.py +++ b/waybionic_sensors/test/test_package_metadata.py @@ -68,6 +68,18 @@ def test_package_declares_diagnostics_dependency(): assert 'diagnostic_msgs' in read('package.xml') +def test_package_declares_rviz_imu_plugin_dependency(): + assert 'rviz_imu_plugin' in read('package.xml') + + +def test_demo_rviz_uses_rviz_imu_plugin_on_the_demo_topic(): + config = read('config/imu_demo.rviz') + assert 'rviz_imu_plugin/Imu' in config + assert 'rviz_default_plugins/Imu' not in config + assert '/waybionic/imu/data_demo' in config + assert '/waybionic/imu/data_raw' not in config + + def test_demo_outputs_default_to_off(): # A default-on rotating TF would imply the raw sensor knows its attitude. launch_source = read('launch/imu_publisher.launch.py') diff --git a/waybionic_sensors/waybionic_sensors/imu_diagnostics.py b/waybionic_sensors/waybionic_sensors/imu_diagnostics.py index 1ba525f..256eacd 100644 --- a/waybionic_sensors/waybionic_sensors/imu_diagnostics.py +++ b/waybionic_sensors/waybionic_sensors/imu_diagnostics.py @@ -39,9 +39,9 @@ class ImuDiagnosticsBuilder: """ Turns the publisher's recent activity into diagnostic statuses. - ``stale_timeout_sec`` is the sample age past which the heartbeat is reported - STALE. It should comfortably exceed one publish period so ordinary jitter - does not flap the status. + ``stale_timeout_sec`` is the sample age past which heartbeat, rate, and + telemetry rows are reported STALE. It should comfortably exceed one publish + period so ordinary jitter does not flap the status. ``expected_rate_hz`` is the configured publish rate, reported alongside the measured rate so a reviewer can see shortfalls. @@ -73,12 +73,16 @@ def build( publish rate observed since the last report, and ``source_description`` a short description of the active data source. """ + sample_is_stale = self._is_stale(now_ns, last_reading) + array = DiagnosticArray() array.header.stamp = to_time_msg(now_ns) array.status.append( self._heartbeat_status(now_ns, last_reading, source_description) ) - array.status.append(self._rate_status(measured_rate_hz, last_reading is not None)) + array.status.append( + self._rate_status(measured_rate_hz, last_reading is not None, sample_is_stale) + ) if last_reading is not None: array.status.append( @@ -87,6 +91,7 @@ def build( _magnitude(last_reading.angular_velocity), 'rad/s', 'Gyroscope magnitude', + stale=sample_is_stale, ) ) array.status.append( @@ -95,11 +100,19 @@ def build( _magnitude(last_reading.linear_acceleration), 'm/s^2', 'Accelerometer magnitude, including gravity', + stale=sample_is_stale, ) ) return array + def _is_stale(self, now_ns: int, last_reading: Optional[ImuReading]) -> bool: + """Return True when no sample exists or the newest sample is too old.""" + if last_reading is None: + return True + age_sec = max(0.0, (now_ns - last_reading.stamp_ns) / 1e9) + return age_sec > self._stale_timeout_sec + def _heartbeat_status( self, now_ns: int, @@ -132,16 +145,28 @@ def _heartbeat_status( return status - def _rate_status(self, measured_rate_hz: float, has_data: bool) -> DiagnosticStatus: + def _rate_status( + self, + measured_rate_hz: float, + has_data: bool, + sample_is_stale: bool, + ) -> DiagnosticStatus: """Build the ``imu.rate`` row comparing measured against expected rate.""" status = DiagnosticStatus() status.name = RATE_NAME status.hardware_id = HARDWARE_ID status.values = _key_values(f'{measured_rate_hz:.1f}', 'Hz') - if not has_data: + if not has_data or sample_is_stale: status.level = DiagnosticStatus.STALE - status.message = 'No IMU samples to measure' + status.message = ( + 'No IMU samples to measure' + if not has_data + else ( + f'Publishing at {measured_rate_hz:.1f} Hz; ' + 'IMU samples are stale' + ) + ) return status # Anything below about 80% of the configured rate is worth surfacing but @@ -164,12 +189,18 @@ def _value_status( value: float, unit: str, message: str, + *, + stale: bool = False, ) -> DiagnosticStatus: - """Build an informational telemetry row with an unambiguous unit.""" + """Build a telemetry row; mark it STALE when the last sample is old.""" status = DiagnosticStatus() status.name = name status.hardware_id = HARDWARE_ID - status.level = DiagnosticStatus.OK - status.message = message status.values = _key_values(f'{value:.3f}', unit) + if stale: + status.level = DiagnosticStatus.STALE + status.message = f'{message} (stale)' + else: + status.level = DiagnosticStatus.OK + status.message = message return status diff --git a/waybionic_sensors/waybionic_sensors/imu_messages.py b/waybionic_sensors/waybionic_sensors/imu_messages.py index bc0b802..1499add 100644 --- a/waybionic_sensors/waybionic_sensors/imu_messages.py +++ b/waybionic_sensors/waybionic_sensors/imu_messages.py @@ -6,17 +6,23 @@ Covariance layout ----------------- -Each covariance field is a row-major 3x3 matrix. Only the diagonal is populated: -the mock axes are modelled as uncorrelated, so the off-diagonal terms are zero -because they are genuinely believed to be zero, not because they are unknown. -Real per-axis values should come from the sensor datasheet or from a bench -characterisation once the hardware is chosen. +Each covariance field is a row-major 3x3 matrix. + +``sensor_msgs/Imu`` treats an all-zero matrix as *unknown*, not as perfect +certainty. Raw gyroscope and accelerometer covariances therefore stay all-zero +until electrical supplies a datasheet or calibration value (a positive +``*_stddev`` parameter). A positive standard deviation yields ``stddev**2`` on +the diagonal; off-diagonal terms are then zero because the axes are modelled as +uncorrelated, which is a stated assumption rather than a missing value. Unavailable orientation ----------------------- ``sensor_msgs/msg/Imu`` defines ``orientation_covariance[0] = -1`` as "this message does not contain orientation". The raw topic always sets that, because an accelerometer and a gyroscope alone cannot observe absolute heading. + +The demo topic may carry a synthetic orientation covariance. That placeholder +is restricted to demo output and is never copied onto ``data_raw``. """ from typing import Tuple @@ -30,6 +36,9 @@ ORIENTATION_UNAVAILABLE = -1.0 """Value placed in ``orientation_covariance[0]`` to mark orientation absent.""" +UNKNOWN_COVARIANCE = [0.0] * 9 +"""All-zero 3x3 matrix: ROS ``sensor_msgs/Imu`` encoding for unknown covariance.""" + IDENTITY_QUATERNION: Quaternion = (0.0, 0.0, 0.0, 1.0) @@ -48,6 +57,18 @@ def diagonal_covariance(stddev: float) -> list: ] +def covariance_from_stddev(stddev: float) -> list: + """ + Return ROS covariance for a 3-vector. + + A non-positive ``stddev`` yields :data:`UNKNOWN_COVARIANCE` (all zeros). A + positive value yields a diagonal matrix from :func:`diagonal_covariance`. + """ + if float(stddev) <= 0.0: + return list(UNKNOWN_COVARIANCE) + return diagonal_covariance(stddev) + + def build_raw_imu_message( reading: ImuReading, frame_id: str, @@ -60,6 +81,8 @@ def build_raw_imu_message( Orientation is always marked unavailable here, even if the reading happens to carry one, because this topic is defined as un-fused sensor output. + Measurement covariances stay unknown unless a positive standard deviation + has been supplied from a datasheet or calibration. """ message = Imu() message.header.stamp = to_time_msg(reading.stamp_ns) @@ -75,12 +98,14 @@ def build_raw_imu_message( message.angular_velocity.x = float(reading.angular_velocity[0]) message.angular_velocity.y = float(reading.angular_velocity[1]) message.angular_velocity.z = float(reading.angular_velocity[2]) - message.angular_velocity_covariance = diagonal_covariance(angular_velocity_stddev) + message.angular_velocity_covariance = covariance_from_stddev(angular_velocity_stddev) message.linear_acceleration.x = float(reading.linear_acceleration[0]) message.linear_acceleration.y = float(reading.linear_acceleration[1]) message.linear_acceleration.z = float(reading.linear_acceleration[2]) - message.linear_acceleration_covariance = diagonal_covariance(linear_acceleration_stddev) + message.linear_acceleration_covariance = covariance_from_stddev( + linear_acceleration_stddev + ) return message @@ -99,7 +124,8 @@ def build_demo_orientation_message( The orientation here is generated for visualisation. It is published on its own topic so that nothing subscribing to the raw topic can mistake it for a - measured attitude. + measured attitude. Synthetic orientation covariance, when used, stays on + this topic only. """ message = build_raw_imu_message( reading, @@ -112,7 +138,7 @@ def build_demo_orientation_message( message.orientation.y = float(orientation[1]) message.orientation.z = float(orientation[2]) message.orientation.w = float(orientation[3]) - message.orientation_covariance = diagonal_covariance(orientation_stddev) + message.orientation_covariance = covariance_from_stddev(orientation_stddev) return message diff --git a/waybionic_sensors/waybionic_sensors/imu_publisher_node.py b/waybionic_sensors/waybionic_sensors/imu_publisher_node.py index a6c894a..bf85d4b 100644 --- a/waybionic_sensors/waybionic_sensors/imu_publisher_node.py +++ b/waybionic_sensors/waybionic_sensors/imu_publisher_node.py @@ -119,10 +119,11 @@ def _declare_parameters(self) -> None: # otherwise imply the raw sensor knows its own attitude. self.declare_parameter('publish_demo_orientation', False) self.declare_parameter('publish_demo_tf', False) - # Placeholder noise figures. Replace with datasheet or bench values once - # the sensor is chosen; see docs/HARDWARE_INTERFACE.md. - self.declare_parameter('angular_velocity_stddev', 0.01) - self.declare_parameter('linear_acceleration_stddev', 0.05) + # 0.0 means unknown covariance (ROS all-zero matrix). Do not invent + # datasheet values; electrical must supply them. See HARDWARE_INTERFACE.md. + self.declare_parameter('angular_velocity_stddev', 0.0) + self.declare_parameter('linear_acceleration_stddev', 0.0) + # Demo topic only: synthetic placeholder, never copied onto data_raw. self.declare_parameter('orientation_stddev', 0.05) self.declare_parameter('mock_stall_after_sec', 0.0) self.declare_parameter('serial_port', '') From 6eda4c8871bef1b07086d6abdb174e196cfd419e Mon Sep 17 00:00:00 2001 From: Khuzaymah Bin Haris Date: Fri, 11 Sep 2026 04:38:48 -0600 Subject: [PATCH 4/7] Finish remaining PR #11 IMU review items Explain raw vs demo in beginner terms, latch mock stall so stale telemetry cannot recover, and measure publish rate from timestamps so the test does not flake on startup delay. Refs #11 --- waybionic_sensors/README.md | 30 +++++++-- waybionic_sensors/docs/HARDWARE_INTERFACE.md | 4 +- waybionic_sensors/docs/IMU_CONTRACT.md | 17 ++++- waybionic_sensors/docs/PR_NOTES.md | 63 +++++++++++-------- .../test/test_imu_publisher_node.py | 12 +++- waybionic_sensors/test/test_mock_source.py | 7 +++ .../test/test_package_metadata.py | 19 ++++++ .../waybionic_sensors/imu_diagnostics.py | 9 ++- .../waybionic_sensors/mock_source.py | 4 ++ 9 files changed, 123 insertions(+), 42 deletions(-) diff --git a/waybionic_sensors/README.md b/waybionic_sensors/README.md index ab328b2..8e6987d 100644 --- a/waybionic_sensors/README.md +++ b/waybionic_sensors/README.md @@ -63,13 +63,31 @@ The mock stops after five seconds. Once the sample age passes `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. -## What is measured and what is generated +## Raw vs demo data, in plain English -The raw topic carries gyroscope and accelerometer data only. It marks -orientation unavailable with `orientation_covariance[0] = -1`, because an -accelerometer and a gyroscope cannot observe absolute heading. The synthetic -orientation lives on its own `data_demo` topic and is off by default, so nothing -can mistake it for a measurement. +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`. diff --git a/waybionic_sensors/docs/HARDWARE_INTERFACE.md b/waybionic_sensors/docs/HARDWARE_INTERFACE.md index 904dbbc..dc4d6fb 100644 --- a/waybionic_sensors/docs/HARDWARE_INTERFACE.md +++ b/waybionic_sensors/docs/HARDWARE_INTERFACE.md @@ -72,9 +72,9 @@ implementing that interface in a new module and constructing it instead of ```python class MyImuReader(ImuHardwareReader): - def open(self): ... + def start(self): ... def read(self, stamp_ns) -> Optional[ImuReading]: ... - def close(self): ... + def stop(self): ... def describe(self) -> str: ... ``` diff --git a/waybionic_sensors/docs/IMU_CONTRACT.md b/waybionic_sensors/docs/IMU_CONTRACT.md index f5bf5d3..57adfe2 100644 --- a/waybionic_sensors/docs/IMU_CONTRACT.md +++ b/waybionic_sensors/docs/IMU_CONTRACT.md @@ -3,6 +3,21 @@ What `waybionic_sensors` publishes, in what units, and which parts are measured versus generated for display. +## Plain English + +The IMU tells us how fast it is spinning and how it is accelerating. It does +not, by itself, know which way the robot is facing. + +- **Raw** (`/waybionic/imu/data_raw`) is the honest sensor stream: gyro and + accelerometer only. Use this for anything that must not consume invented + attitude. +- **Demo** (`/waybionic/imu/data_demo`) adds a made-up facing direction so RViz + has something to draw. It is off unless you launch `imu_demo.launch.py`. Do + not feed it to control or localisation. + +RViz watches the demo topic. A later fusion node, when one exists, should +publish its own fused topic rather than overwrite `data_raw`. + ## Topics | Topic | Type | Default | Meaning | @@ -35,7 +50,7 @@ localisation node, treat invented data as a measurement. The raw topic therefore always sets: -``` +```text orientation = (0, 0, 0, 1) # placeholder, not a measurement orientation_covariance[0] = -1.0 # "orientation not available" ``` diff --git a/waybionic_sensors/docs/PR_NOTES.md b/waybionic_sensors/docs/PR_NOTES.md index 6f402c6..e34fbe0 100644 --- a/waybionic_sensors/docs/PR_NOTES.md +++ b/waybionic_sensors/docs/PR_NOTES.md @@ -22,8 +22,9 @@ Requested changes on the open IMU PR, without expanding scope: | Jazzy has no `rviz_default_plugins/Imu` | `imu_demo.rviz` uses `rviz_imu_plugin/Imu`; `package.xml` and `robostack.yaml` declare the dependency so `rosdep install` / macOS setup pull it | | Display subscribed to `data_raw` | Orientation display now subscribes to `/waybionic/imu/data_demo` | | Stale gyro/accel left OK | When samples age out, `imu.angular_velocity` and `imu.linear_acceleration` report STALE (last values still shown). Rate also goes STALE, not just WARN | -| No regression for the stall path | `test_stale_stall_marks_heartbeat_rate_and_telemetry` plus unit tests on the diagnostics builder | +| No regression for the stall path | `test_stale_stall_marks_heartbeat_rate_and_telemetry` plus unit tests on the diagnostics builder. Mock stall latches so a later earlier-timestamp read cannot unstall | | Placeholder stddev implied confidence | Raw/live gyro and accel covariances default to all-zero (ROS unknown). Positive `*_stddev` is opt-in for datasheet/calibration. Synthetic orientation covariance stays on the demo topic only | +| Docs should be beginner-readable | README and `IMU_CONTRACT.md` explain raw vs demo in plain English before the ROS field names | ## What changed relative to the old IMU branch @@ -103,56 +104,54 @@ absent sensor. `ros2 topic hz /waybionic/imu/data_raw`: -``` -average rate: 49.988 - min: 0.019s max: 0.021s std dev: 0.00030s window: 51 +```text +average rate: 50.005 + min: 0.019s max: 0.021s std dev: 0.00026s window: 52 ``` `ros2 topic echo /waybionic/imu/data_raw --once`: ```yaml header: - stamp: {sec: 1785808547, nanosec: 917613821} + stamp: {sec: 1789120170, nanosec: 981566836} frame_id: imu_link orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -angular_velocity: {x: -0.0736..., y: -0.0383..., z: -0.1473...} +angular_velocity: {x: -0.0568..., y: 0.0479..., z: -0.1136...} angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -linear_acceleration: {x: -0.0368..., y: -0.0153..., z: 9.80665} +linear_acceleration: {x: -0.0284..., y: 0.0192..., z: 9.80665} linear_acceleration_covariance: [0.0, ...] ``` -`ros2 topic hz /diagnostics` gives `average rate: 2.000`, comfortably above the -1 Hz requirement. +`ros2 topic hz /diagnostics` is 2 Hz. While streaming, heartbeat/rate/gyro/accel are OK. -Heartbeat while streaming: +Default launch topic list has `/waybionic/imu/data_raw` and `/diagnostics` only. +`imu_demo.launch.py launch_rviz:=false` adds `/waybionic/imu/data_demo` and `/tf`. +Demo orientation covariance is the placeholder `0.05^2 = 0.0025`; gyro/accel stay unknown. -``` -name: imu.heartbeat -message: IMU streaming from mock generator -values: [{key: value, value: '0.00'}, {key: unit, value: s}] -``` +Heartbeat after `mock_stall_after_sec:=2.0` (timeout 1.00 s): -Heartbeat after `mock_stall_after_sec:=3.0`: - -``` +```text name: imu.heartbeat -message: No IMU sample for 6.64 s (timeout 1.00 s) -values: [{key: value, value: '6.64'}, {key: unit, value: s}] +message: No IMU sample for 4.14 s (timeout 1.00 s) +values: [{key: value, value: '4.14'}, {key: unit, value: s}] +name: imu.rate +message: Publishing at 0.0 Hz; IMU samples are stale +name: imu.angular_velocity +message: Gyroscope magnitude (stale) +name: imu.linear_acceleration +message: Accelerometer magnitude, including gravity (stale) ``` Heartbeat with `use_mock:=false`: -``` +```text name: imu.heartbeat message: No IMU samples received from unconfigured IMU driver; awaiting sensor model, transport and packet format from electrical values: [{key: value, value: never}, {key: unit, value: s}] ``` -`/waybionic/imu/data_demo` is absent from `ros2 topic list` on a default launch, -confirming the demo output is off unless asked for. - ## Tests ```bash @@ -160,16 +159,26 @@ colcon test --packages-select waybionic_sensors colcon test-result --all --verbose ``` -92 tests, 0 failures. +95 tests, 0 failures. + +Full workspace on Ubuntu 24.04 / ROS 2 Jazzy / WSL2: + +```bash +colcon build --symlink-install +colcon test +colcon test-result --all --verbose +``` + +4 packages finished. **136 tests, 0 errors, 0 failures, 0 skipped.** | Suite | Count | Covers | |-------|-------|--------| | `test_imu_messages.py` | 17 | Frame, timestamp, orientation-unavailable marker, unknown vs datasheet covariance, demo message, demo TF | | `test_imu_diagnostics.py` | 19 | Heartbeat OK/STALE, custom timeout, never-received, age units, rate WARN/STALE, telemetry OK then STALE, absence of roll/pitch/yaw | | `test_imu_publisher_node.py` | 16 | Runtime: rate, timestamps, frame IDs, demo defaults, unknown covariance, heartbeat OK then STALE, stall marks all four signals, live mode without hardware | -| `test_mock_source.py` | 13 | Determinism, gravity, amplitude bounds, stall behaviour, quaternion normalisation | +| `test_mock_source.py` | 14 | Determinism, gravity, amplitude bounds, stall latch, quaternion normalisation | | `test_hardware_reader.py` | 9 | Interface surface, stub behaviour, a custom reader satisfying the boundary | -| `test_package_metadata.py` | 16 | Module separation, node delegation, launch defaults, `rviz_imu_plugin` on `data_demo`, docs, entry point, no invented protocol | +| `test_package_metadata.py` | 18 | Module separation, node delegation, launch defaults, `rviz_imu_plugin` on `data_demo`, raw vs demo docs, hardware lifecycle, entry point, no invented protocol | | `test_flake8.py`, `test_pep257.py` | 2 | Style and docstrings | ## Known limitations diff --git a/waybionic_sensors/test/test_imu_publisher_node.py b/waybionic_sensors/test/test_imu_publisher_node.py index b088c79..d7e4ba2 100644 --- a/waybionic_sensors/test/test_imu_publisher_node.py +++ b/waybionic_sensors/test/test_imu_publisher_node.py @@ -151,9 +151,15 @@ def test_raw_messages_use_stddev_when_explicitly_configured(): def test_publish_rate_follows_the_parameter(): - duration = 1.5 - with Harness(duration_sec=duration, publish_rate_hz=20.0) as collector: - measured = len(collector.raw) / duration + with Harness(duration_sec=2.0, publish_rate_hz=20.0) as collector: + assert len(collector.raw) >= 15 + stamps = [ + message.header.stamp.sec + message.header.stamp.nanosec * 1e-9 + for message in collector.raw + ] + elapsed = stamps[-1] - stamps[0] + assert elapsed > 0.5 + measured = (len(stamps) - 1) / elapsed assert 12.0 < measured < 28.0 diff --git a/waybionic_sensors/test/test_mock_source.py b/waybionic_sensors/test/test_mock_source.py index a0dc057..75855be 100644 --- a/waybionic_sensors/test/test_mock_source.py +++ b/waybionic_sensors/test/test_mock_source.py @@ -68,6 +68,13 @@ def test_stall_stops_samples_after_the_configured_delay(): assert source.read(START_NS + 3 * SECOND_NS) is None +def test_stall_stays_latched_if_a_later_read_uses_an_earlier_timestamp(): + source = MockImuSource(stall_after_sec=2.0) + assert source.read(START_NS) is not None + assert source.read(START_NS + 3 * SECOND_NS) is None + assert source.read(START_NS + 1 * SECOND_NS) is None + + def test_elapsed_is_measured_from_the_first_sample(): source = MockImuSource() source.read(START_NS) diff --git a/waybionic_sensors/test/test_package_metadata.py b/waybionic_sensors/test/test_package_metadata.py index 3b19e2b..7e78434 100644 --- a/waybionic_sensors/test/test_package_metadata.py +++ b/waybionic_sensors/test/test_package_metadata.py @@ -108,3 +108,22 @@ def test_no_invented_serial_protocol_is_implemented(): reader_source = read('waybionic_sensors/hardware_reader.py') for token in ('import serial', 'baudrate', 'struct.unpack'): assert token not in reader_source + + +def test_readme_explains_raw_versus_demo_topics(): + readme = read('README.md') + assert '/waybionic/imu/data_raw' in readme + assert '/waybionic/imu/data_demo' in readme + assert 'made-up facing direction' in readme + contract = read('docs/IMU_CONTRACT.md') + assert 'Plain English' in contract + assert '/waybionic/imu/data_raw' in contract + assert '/waybionic/imu/data_demo' in contract + + +def test_hardware_docs_match_the_reader_lifecycle(): + docs = read('docs/HARDWARE_INTERFACE.md') + assert 'def start(self)' in docs + assert 'def stop(self)' in docs + assert 'def open(self)' not in docs + assert 'def close(self)' not in docs diff --git a/waybionic_sensors/waybionic_sensors/imu_diagnostics.py b/waybionic_sensors/waybionic_sensors/imu_diagnostics.py index 256eacd..6cf394e 100644 --- a/waybionic_sensors/waybionic_sensors/imu_diagnostics.py +++ b/waybionic_sensors/waybionic_sensors/imu_diagnostics.py @@ -106,12 +106,15 @@ def build( return array + def _age_sec(self, now_ns: int, last_reading: ImuReading) -> float: + """Return the age of ``last_reading`` in seconds, never negative.""" + return max(0.0, (now_ns - last_reading.stamp_ns) / 1e9) + def _is_stale(self, now_ns: int, last_reading: Optional[ImuReading]) -> bool: """Return True when no sample exists or the newest sample is too old.""" if last_reading is None: return True - age_sec = max(0.0, (now_ns - last_reading.stamp_ns) / 1e9) - return age_sec > self._stale_timeout_sec + return self._age_sec(now_ns, last_reading) > self._stale_timeout_sec def _heartbeat_status( self, @@ -130,7 +133,7 @@ def _heartbeat_status( status.values = _key_values('never', 's') return status - age_sec = max(0.0, (now_ns - last_reading.stamp_ns) / 1e9) + age_sec = self._age_sec(now_ns, last_reading) status.values = _key_values(f'{age_sec:.2f}', 's') if age_sec > self._stale_timeout_sec: diff --git a/waybionic_sensors/waybionic_sensors/mock_source.py b/waybionic_sensors/waybionic_sensors/mock_source.py index 049aca7..da69ed1 100644 --- a/waybionic_sensors/waybionic_sensors/mock_source.py +++ b/waybionic_sensors/waybionic_sensors/mock_source.py @@ -59,6 +59,7 @@ def __init__( self._linear_amplitude = linear_amplitude self._stall_after_sec = stall_after_sec self._start_ns: Optional[int] = None + self._stalled = False @property def stalled_deliberately(self) -> bool: @@ -73,11 +74,14 @@ def elapsed_sec(self, stamp_ns: int) -> float: def read(self, stamp_ns: int) -> Optional[ImuReading]: """Produce the sample for ``stamp_ns``, or ``None`` once stalled.""" + if self._stalled: + return None if self._start_ns is None: self._start_ns = stamp_ns elapsed = self.elapsed_sec(stamp_ns) if self._stall_after_sec > 0.0 and elapsed > self._stall_after_sec: + self._stalled = True return None slow = math.sin(elapsed * 0.5) From 0641a39ac5fabc8b1c9537771aad203b19a13144 Mon Sep 17 00:00:00 2001 From: Khuzaymah Bin Haris Date: Sat, 12 Sep 2026 13:38:35 -0600 Subject: [PATCH 5/7] Remove ament_python rosdep declaration so Jazzy rosdep install succeeds without -r. --- waybionic_sensors/README.md | 2 +- waybionic_sensors/docs/PR_NOTES.md | 7 ++++--- waybionic_sensors/package.xml | 2 -- waybionic_sensors/test/test_package_metadata.py | 8 ++++++++ 4 files changed, 13 insertions(+), 6 deletions(-) diff --git a/waybionic_sensors/README.md b/waybionic_sensors/README.md index 8e6987d..177f0c6 100644 --- a/waybionic_sensors/README.md +++ b/waybionic_sensors/README.md @@ -12,7 +12,7 @@ boundary a real driver will plug into once electrical confirms the sensor. ```bash source /opt/ros/jazzy/setup.bash cd -rosdep install --from-paths src --ignore-src -r -y +rosdep install --from-paths src --ignore-src -y colcon build --packages-select waybionic_sensors --symlink-install source install/setup.bash diff --git a/waybionic_sensors/docs/PR_NOTES.md b/waybionic_sensors/docs/PR_NOTES.md index e34fbe0..d649430 100644 --- a/waybionic_sensors/docs/PR_NOTES.md +++ b/waybionic_sensors/docs/PR_NOTES.md @@ -25,6 +25,7 @@ Requested changes on the open IMU PR, without expanding scope: | No regression for the stall path | `test_stale_stall_marks_heartbeat_rate_and_telemetry` plus unit tests on the diagnostics builder. Mock stall latches so a later earlier-timestamp read cannot unstall | | Placeholder stddev implied confidence | Raw/live gyro and accel covariances default to all-zero (ROS unknown). Positive `*_stddev` is opt-in for datasheet/calibration. Synthetic orientation covariance stays on the demo topic only | | Docs should be beginner-readable | README and `IMU_CONTRACT.md` explain raw vs demo in plain English before the ROS field names | +| `ament_python` has no Jazzy rosdep key | Removed `ament_python`; retained `ament_python`. Strict `rosdep install` no longer needs `-r` or `--skip-keys ament_python` for this package | ## What changed relative to the old IMU branch @@ -35,7 +36,7 @@ Requested changes on the open IMU PR, without expanding scope: | Covariances all zero | Zero was misread as "perfectly certain" | Raw gyro/accel now stay unknown (all-zero) until a datasheet stddev is supplied; demo orientation covariance is synthetic and demo-only | | No `/diagnostics` output | Panel could not show IMU health | `imu.heartbeat` plus rate and telemetry rows at 2 Hz | | One 120-line node doing everything | Serial work would have to be bolted into the publisher | Six modules: reading type, mock source, hardware boundary, message builder, diagnostics builder, node | -| Three metadata tests | No behavioural coverage | 92 tests including a runtime suite that spins the node | +| Three metadata tests | No behavioural coverage | 96 tests including a runtime suite that spins the node | | `serial_port` parameter with no reader | Suggested a driver existed | Documented boundary plus a stub that makes the missing driver visible in diagnostics | ## Raw versus fused orientation @@ -159,7 +160,7 @@ colcon test --packages-select waybionic_sensors colcon test-result --all --verbose ``` -95 tests, 0 failures. +96 tests, 0 failures. Full workspace on Ubuntu 24.04 / ROS 2 Jazzy / WSL2: @@ -178,7 +179,7 @@ colcon test-result --all --verbose | `test_imu_publisher_node.py` | 16 | Runtime: rate, timestamps, frame IDs, demo defaults, unknown covariance, heartbeat OK then STALE, stall marks all four signals, live mode without hardware | | `test_mock_source.py` | 14 | Determinism, gravity, amplitude bounds, stall latch, quaternion normalisation | | `test_hardware_reader.py` | 9 | Interface surface, stub behaviour, a custom reader satisfying the boundary | -| `test_package_metadata.py` | 18 | Module separation, node delegation, launch defaults, `rviz_imu_plugin` on `data_demo`, raw vs demo docs, hardware lifecycle, entry point, no invented protocol | +| `test_package_metadata.py` | 19 | Module separation, node delegation, launch defaults, `rviz_imu_plugin` on `data_demo`, no ament_python rosdep, raw vs demo docs, hardware lifecycle, entry point, no invented protocol | | `test_flake8.py`, `test_pep257.py` | 2 | Style and docstrings | ## Known limitations diff --git a/waybionic_sensors/package.xml b/waybionic_sensors/package.xml index 47a69a0..d17bf2b 100644 --- a/waybionic_sensors/package.xml +++ b/waybionic_sensors/package.xml @@ -7,8 +7,6 @@ Khuzaymah Bin Haris Apache-2.0 - ament_python - builtin_interfaces diagnostic_msgs geometry_msgs diff --git a/waybionic_sensors/test/test_package_metadata.py b/waybionic_sensors/test/test_package_metadata.py index 7e78434..828078b 100644 --- a/waybionic_sensors/test/test_package_metadata.py +++ b/waybionic_sensors/test/test_package_metadata.py @@ -68,6 +68,14 @@ def test_package_declares_diagnostics_dependency(): assert 'diagnostic_msgs' in read('package.xml') +def test_package_does_not_declare_ament_python_as_a_rosdep(): + # ament_python has no Jazzy rosdep key. Declaring it as a buildtool_depend + # breaks a strict rosdep install; keep only the ament_python build type. + manifest = read('package.xml') + assert 'ament_python' not in manifest + assert 'ament_python' in manifest + + def test_package_declares_rviz_imu_plugin_dependency(): assert 'rviz_imu_plugin' in read('package.xml') From e317df4cd15b501e08facbe924dc8d3858fd7152 Mon Sep 17 00:00:00 2001 From: Khuzaymah Bin Haris Date: Sat, 12 Sep 2026 14:20:24 -0600 Subject: [PATCH 6/7] Record Jazzy rosdep, workspace-test, and launch evidence for PR #11. --- waybionic_sensors/README.md | 15 +++++----- waybionic_sensors/docs/PR_NOTES.md | 44 ++++++++++++++++++++++++++++-- 2 files changed, 50 insertions(+), 9 deletions(-) diff --git a/waybionic_sensors/README.md b/waybionic_sensors/README.md index 177f0c6..00775d8 100644 --- a/waybionic_sensors/README.md +++ b/waybionic_sensors/README.md @@ -121,9 +121,9 @@ 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, because the sensor model, -transport, and packet format are unconfirmed. The open questions for electrical -are tracked in `docs/HARDWARE_INTERFACE.md`. +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. @@ -135,10 +135,11 @@ colcon test --packages-select waybionic_sensors colcon test-result --all --verbose ``` -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. +96 tests, 0 failures on Ubuntu 24.04 / ROS 2 Jazzy. 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 diff --git a/waybionic_sensors/docs/PR_NOTES.md b/waybionic_sensors/docs/PR_NOTES.md index d649430..9eb2b70 100644 --- a/waybionic_sensors/docs/PR_NOTES.md +++ b/waybionic_sensors/docs/PR_NOTES.md @@ -170,7 +170,7 @@ colcon test colcon test-result --all --verbose ``` -4 packages finished. **136 tests, 0 errors, 0 failures, 0 skipped.** +4 packages finished. **137 tests, 0 errors, 0 failures, 0 skipped.** | Suite | Count | Covers | |-------|-------|--------| @@ -184,9 +184,49 @@ colcon test-result --all --verbose ## Known limitations -- No physical IMU driver. Blocked on `docs/HARDWARE_INTERFACE.md`. +- No physical IMU driver. Sensor model, transport, mounting, calibration, and + noise values stay pending until Electrical answers + `docs/HARDWARE_INTERFACE.md`. - Covariance values on the raw topic stay unknown until electrical answers question 14. `orientation_stddev` is a demo-topic-only placeholder. - The demo orientation and demo TF are visualisation aids, not estimates. - The `base_link` to `imu_link` offset in the demo TF is a placeholder 0.1 m, not a mounting claim. + +## Verification (Ubuntu 24.04 / ROS 2 Jazzy / WSL2) + +Standard setup, no `-r` and no `--skip-keys`: + +```bash +source /opt/ros/jazzy/setup.bash +rosdep update +rosdep install --from-paths . --ignore-src -y +#All required rosdeps installed successfully +dpkg -s ros-jazzy-rviz-imu-plugin # install ok; class rviz_imu_plugin/Imu +colcon build --symlink-install # 4 packages finished +colcon test && colcon test-result --all +# Summary: 137 tests, 0 errors, 0 failures, 0 skipped +# waybionic_sensors: 96 passed +``` + +Launch checks from that overlay: + +| Command | Result | +|---------|--------| +| `ros2 launch waybionic_sensors imu_publisher.launch.py` | 151 raw msgs; `data_demo` absent; `orientation_covariance[0] = -1`; gyro/accel covariance all-zero; heartbeat/rate/gyro/accel OK | +| `ros2 launch waybionic_sensors imu_demo.launch.py launch_rviz:=false` | raw + 150 demo msgs; demo orientation covariance usable | +| `ros2 launch waybionic_sensors imu_demo.launch.py` | RViz started (`OpenGl version: 4.5`); config class `rviz_imu_plugin/Imu` on `/waybionic/imu/data_demo`; no plugin load error | +| `mock_stall_after_sec:=1.0 stale_timeout_sec:=0.5` | heartbeat, rate, angular_velocity, linear_acceleration all STALE (3); last magnitudes still shown | +| restart default publisher | all four rows recovered to OK | +| `use_mock:=false` | 0 sensor samples; heartbeat STALE | + +### Environment notes (not part of the standard setup) + +- A non-interactive WSL user session cannot type a sudo password. The first + `rosdep install --from-paths . --ignore-src -y` therefore stopped on + `sudo: a password is required`. Installing `ros-jazzy-rviz-imu-plugin` as + root after `apt-get update` (the previous apt candidate 404'd on a stale + index) made the same rosdep command exit 0 with no `-r` or skip key. +- Building the checkout under a Windows path that contains a space (`Uni Work`) + makes `xacro` split the URDF argument in `waybionic_bringup`'s launch test. + Full-workspace evidence above used a copy at `/home/khuzaymah/pr11_ws`. From 4022337540209b8f2c4f1ce988f31537b8bd9a41 Mon Sep 17 00:00:00 2001 From: Khuzaymah Bin Haris Date: Sat, 19 Sep 2026 12:08:34 -0600 Subject: [PATCH 7/7] Add Malik runtime handoff and mark Electrical IMU facts as still open. --- waybionic_sensors/README.md | 78 ++++++++++++++++++- waybionic_sensors/docs/HARDWARE_INTERFACE.md | 23 ++++++ waybionic_sensors/docs/IMU_CONTRACT.md | 8 ++ waybionic_sensors/docs/PR_NOTES.md | 23 +++++- .../test/test_package_metadata.py | 26 +++++++ 5 files changed, 154 insertions(+), 4 deletions(-) diff --git a/waybionic_sensors/README.md b/waybionic_sensors/README.md index 00775d8..3a0cd01 100644 --- a/waybionic_sensors/README.md +++ b/waybionic_sensors/README.md @@ -128,6 +128,76 @@ 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 @@ -135,7 +205,8 @@ colcon test --packages-select waybionic_sensors colcon test-result --all --verbose ``` -96 tests, 0 failures on Ubuntu 24.04 / ROS 2 Jazzy. Coverage spans message +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 @@ -143,7 +214,8 @@ transitioning from OK to STALE. ## Related docs -- `docs/IMU_CONTRACT.md` — topics, units, covariance, and parameters -- `docs/HARDWARE_INTERFACE.md` — questions for electrical and how to add a driver +- `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 diff --git a/waybionic_sensors/docs/HARDWARE_INTERFACE.md b/waybionic_sensors/docs/HARDWARE_INTERFACE.md index dc4d6fb..6b18458 100644 --- a/waybionic_sensors/docs/HARDWARE_INTERFACE.md +++ b/waybionic_sensors/docs/HARDWARE_INTERFACE.md @@ -8,6 +8,29 @@ arrive. package ships a mock source and an unimplemented driver stub rather than a guessed serial protocol. +Khuzaymah owns IMU-specific ROS semantics for this package (raw vs demo topics, +orientation-unavailable marker, unknown covariance until a verified stddev +exists). Electrical owns every physical-sensor fact below. Assumptions are not +production defaults. + +## Open items (owner / status) + +| Item | Owner | Status | +|------|-------|--------| +| Physical sensor model | Electrical | OPEN / NEEDS ELECTRICAL CONFIRMATION | +| Transport / interface | Electrical | OPEN / NEEDS ELECTRICAL CONFIRMATION | +| Mounting (`base_link` → `imu_link`) | Electrical | OPEN / NEEDS ELECTRICAL CONFIRMATION | +| Axis convention / orientation | Electrical | OPEN / NEEDS ELECTRICAL CONFIRMATION | +| Timestamp source / semantics | Electrical | OPEN / NEEDS ELECTRICAL CONFIRMATION | +| Calibration process / values | Electrical | OPEN / NEEDS ELECTRICAL CONFIRMATION | +| Sensor noise / standard deviation | Electrical | OPEN / NEEDS ELECTRICAL CONFIRMATION | +| IMU ROS message semantics (raw vs demo, unavailable orientation, unknown covariance) | Khuzaymah | Implemented in this package; not a hardware claim | + +Until Electrical confirms timestamp source, mock and unconfigured live mode use +the node clock as `ImuReading.stamp_ns`. Diagnostic heartbeat age is +`now - that stamp` (sample freshness). Do not rewrite stamps to make replayed +or delayed data look fresh. + ## Questions for electrical ### Sensor diff --git a/waybionic_sensors/docs/IMU_CONTRACT.md b/waybionic_sensors/docs/IMU_CONTRACT.md index 57adfe2..cc4924f 100644 --- a/waybionic_sensors/docs/IMU_CONTRACT.md +++ b/waybionic_sensors/docs/IMU_CONTRACT.md @@ -87,6 +87,14 @@ onto `/waybionic/imu/data_raw`. See `HARDWARE_INTERFACE.md` question 14. +## Timestamps + +`header.stamp` is the source sample time from `ImuReading.stamp_ns`. The mock +uses the node clock at acquisition. Whether a physical device stamps packets +itself is OPEN / NEEDS ELECTRICAL CONFIRMATION (`HARDWARE_INTERFACE.md` +question 10). `imu.heartbeat` reports age as now minus that source stamp. That +is freshness of the sample, not a replay or observation clock. + ## Diagnostics Names, `value`/`unit` keys, and level mapping follow diff --git a/waybionic_sensors/docs/PR_NOTES.md b/waybionic_sensors/docs/PR_NOTES.md index 9eb2b70..4532ca1 100644 --- a/waybionic_sensors/docs/PR_NOTES.md +++ b/waybionic_sensors/docs/PR_NOTES.md @@ -179,7 +179,7 @@ colcon test-result --all --verbose | `test_imu_publisher_node.py` | 16 | Runtime: rate, timestamps, frame IDs, demo defaults, unknown covariance, heartbeat OK then STALE, stall marks all four signals, live mode without hardware | | `test_mock_source.py` | 14 | Determinism, gravity, amplitude bounds, stall latch, quaternion normalisation | | `test_hardware_reader.py` | 9 | Interface surface, stub behaviour, a custom reader satisfying the boundary | -| `test_package_metadata.py` | 19 | Module separation, node delegation, launch defaults, `rviz_imu_plugin` on `data_demo`, no ament_python rosdep, raw vs demo docs, hardware lifecycle, entry point, no invented protocol | +| `test_package_metadata.py` | 21 | Module separation, node delegation, launch defaults, `rviz_imu_plugin` on `data_demo`, no ament_python rosdep, raw vs demo docs, hardware lifecycle, electrical OPEN items, runtime handoff commands, entry point, no invented protocol | | `test_flake8.py`, `test_pep257.py` | 2 | Style and docstrings | ## Known limitations @@ -230,3 +230,24 @@ Launch checks from that overlay: - Building the checkout under a Windows path that contains a space (`Uni Work`) makes `xacro` split the URDF argument in `waybionic_bringup`'s launch test. Full-workspace evidence above used a copy at `/home/khuzaymah/pr11_ws`. + +Closeout re-run against `e317df4` (Ubuntu 24.04.4 / Jazzy / Python 3.12.3 / WSL2): +strict `rosdep install --from-paths . --ignore-src -y` succeeded; IMU suite +**96 passed**; raw/demo/stall/recovery/live-unconfigured/RViz checks succeeded. +See README **Runtime handoff**. + +## FOLLOW-UP AFTER PR #11 MERGES + +Do not implement these on this branch. They belong on a later reader-validation +PR once a real `ImuHardwareReader` exists. Attach tests next to +`test_hardware_reader.py` / `test_imu_publisher_node.py`, around +`ImuHardwareReader.read()` and the publisher sample loop: + +- no sample +- delayed sample +- malformed / non-finite sample +- reader exception +- recovery after those failures + +`UnconfiguredImuReader.read()` already returns `None` (no fake live samples). +Do not invent protocol, calibration, noise, axes, or device timestamps there. diff --git a/waybionic_sensors/test/test_package_metadata.py b/waybionic_sensors/test/test_package_metadata.py index 828078b..07b2168 100644 --- a/waybionic_sensors/test/test_package_metadata.py +++ b/waybionic_sensors/test/test_package_metadata.py @@ -135,3 +135,29 @@ def test_hardware_docs_match_the_reader_lifecycle(): assert 'def stop(self)' in docs assert 'def open(self)' not in docs assert 'def close(self)' not in docs + + +def test_hardware_docs_mark_electrical_items_open(): + docs = read('docs/HARDWARE_INTERFACE.md') + assert 'OPEN / NEEDS ELECTRICAL CONFIRMATION' in docs + for item in ( + 'Physical sensor model', + 'Transport / interface', + 'Mounting', + 'Axis convention', + 'Timestamp source', + 'Calibration', + 'Sensor noise', + ): + assert item in docs, item + assert 'Khuzaymah' in docs + + +def test_readme_has_runtime_handoff_commands(): + readme = read('README.md') + assert 'Runtime handoff' in readme + assert 'ros2 launch waybionic_sensors imu_publisher.launch.py' in readme + assert 'ros2 launch waybionic_sensors imu_demo.launch.py' in readme + assert 'mock_stall_after_sec' in readme + assert 'use_mock:=false' in readme + assert 'no physical imu driver' in readme.lower()