From e559cdc2108f1cc3ed0f20a6d1a51a4c2ee4bbfc Mon Sep 17 00:00:00 2001 From: Khuzaymah Bin Haris Date: Tue, 4 Aug 2026 04:59:11 +0300 Subject: [PATCH] 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)