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)