Skip to content

fix(ros2): pluggable state extractors (joint_state / imu / odom)#133

Merged
rylinjames merged 1 commit into
mainfrom
fix/ros2-bridge-state-extractor
May 15, 2026
Merged

fix(ros2): pluggable state extractors (joint_state / imu / odom)#133
rylinjames merged 1 commit into
mainfrom
fix/ros2-bridge-state-extractor

Conversation

@rylinjames
Copy link
Copy Markdown
Collaborator

Summary

Replaces #121's fragile hasattr duck-typing with an explicit --state-msg-type CLI flag that dispatches to one of three pure-Python state extractors. The 10-DOF odom path lines up end-to-end with the quadcopter preset's state_dim so silent shape mismatches surface loudly at the right layer.

state_msg_type ROS2 message What's extracted DOF Used by
joint_state (default) sensor_msgs/JointState .position 7 (franka) Arms — unchanged
imu sensor_msgs/Imu .orientation quaternion xyzw 4 Drones (partial state)
odom nav_msgs/Odometry pose + linear twist 10 Drones (full state)

The 10-DOF odom state matches exactly what EmbodimentConfig.load_preset("quadcopter").state_dim returns after #132 — pinned by a cross-layer test so future drift in either side breaks at CI time.

Adapted from #121

@DsThakurRawat opened #121 trying to fix the same bug (the bridge couldn't read drone state via ROS2). The original PR had two real problems I've fixed here:

  1. Duck-typing was fragile. hasattr(msg, "position") is True for JointState (correct) but False for nav_msgs/Odometry (which nests its position under msg.pose.pose.position). The PR's logic would silently fall through with _last_state = None for the most important drone topic. Replaced with an explicit msg-type registry.
  2. The IMU-only state was 4-DOF — incompatible with the quadcopter preset's 10-DOF state. Without velocity in the state, the policy can't damp or correct drift; the bridge was wired up to silently produce useless inputs. Added the odom path which gives full pos+quat+linear-vel and matches the preset by design.

Credit for the conceptual direction (drones need a different state extractor than arms) preserved via Co-Authored-By on the commit.

What I built

  • _STATE_EXTRACTORS registry in ros2_bridge.py — three pure-Python functions (no rclpy needed), unit-testable with SimpleNamespace mocks on any machine.
  • _resolve_state_msg_class() — lazy-imports the right ROS2 message class (so arm deployments that don't have nav_msgs installed still work fine; the import only happens when you actually use --state-msg-type=odom).
  • --state-msg-type CLI flag on reflex ros2-serve with validation + helpful error on unknown types.
  • One-time state-dim mismatch warning — when the loaded embodiment's state_dim doesn't match what the extractor produces (e.g. you point a quadcopter preset at the IMU extractor by mistake), the bridge logs once instead of silently feeding garbage to the policy.
  • Graceful malformed-message handling — catches AttributeError/TypeError from the extractor (e.g. user pointed --state-msg-type=imu at a topic publishing JointState) and logs instead of crashing the node.
  • Updated --state-topic help text to point drone deployments at /mavros/local_position/odom (correct full-state topic) or /mavros/imu/data (orientation-only fallback).

Closes / supersedes

Test plan

  • pytest tests/test_ros2_bridge.py — 24 new + existing tests pass (1 pre-existing test TestServeRos2Flag fails on main already due to CliRunner subprocess sys.path issue — unrelated to this PR)
  • pytest -k "embodiment or safety or guard or preset or ros2" — 225 passed, 4 unrelated skips
  • Quadcopter preset's state_dim (10) verified to match the odom extractor's output length (10) by a dedicated cross-layer test
  • Live smoke against an actual ROS2 install (deferred — no MAVROS testbed available locally; the unit tests cover the duck-typing failure mode fix(ros2): support IMU state vector for drone embodiments #121 had, and the structure follows the existing bridge patterns)

The ROS2 bridge previously hard-coded `msg.position` extraction, which
works for arms (JointState) but breaks for drones (Imu has .orientation;
Odometry has nested .pose.pose.position + .twist.twist.linear). The
#121 attempt to fix this with `hasattr` duck-typing was fragile:
nav_msgs/Odometry has neither attribute at the top level, so it would
silently fall through with `_last_state = None`.

This PR replaces duck-typing with an explicit `--state-msg-type` CLI
flag that selects one of three extractors:

  joint_state  sensor_msgs/JointState  .position           7-DOF (arms)
  imu          sensor_msgs/Imu         .orientation quat   4-DOF (drone partial)
  odom         nav_msgs/Odometry       pose + linear twist 10-DOF (drone full)

The 10-DOF odom path matches the quadcopter preset's `state_dim`
end-to-end — caught at CI time by `test_matches_quadcopter_preset_state_dim`
which cross-pins the extractor output length against the preset.

### Runtime

- `_STATE_EXTRACTORS` registry of three pure-Python functions, decoupled
  from rclpy imports so they can be unit-tested without a ROS2 install.
- `_resolve_state_msg_class()` lazy-imports the right ROS2 message type
  (nav_msgs is only loaded when --state-msg-type=odom is used, so arm
  deployments that don't have nav_msgs installed still work.
-  parameter; the bridge
  subscribes to the right msg class and dispatches to the matching
  extractor.
-  catches / from the extractor
  (e.g. user pointed --state-msg-type at a topic publishing a different
  message type) and logs instead of crashing the node.
- One-time warning when the extractor's output length doesn't match the
  loaded embodiment's . Silent shape mismatches are the most
  common drone deployment failure mode (silently garbage actions instead
  of a loud error); fires once per node to avoid log spam.

### CLI

-  (default
   — arm path unchanged).
- Updated  help text to point drone deployments at
   (full state) or
  (orientation-only fallback).

### Tests

- 16 new tests covering: each extractor (with SimpleNamespace mocks, no
  rclpy needed), registry consistency, unknown-type validation,
  per-msg-type dispatch through the bridge, malformed-message handling,
  one-time mismatch warning behaviour, and the quadcopter cross-pin.
- 225 passing across embodiments + safety + guard + ros2 (1 pre-existing
  unrelated test deselected: TestServeRos2Flag fails on main due to a
  CliRunner subprocess sys.path issue unrelated to this PR).

Supersedes #121.

Co-Authored-By: Divyansh Rawat <186957976+DsThakurRawat@users.noreply.github.com>
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
EOF
)
@rylinjames rylinjames merged commit e1bec22 into main May 15, 2026
6 checks passed
@rylinjames rylinjames deleted the fix/ros2-bridge-state-extractor branch May 15, 2026 20:43
rylinjames added a commit that referenced this pull request May 15, 2026
Adds docs/cli_reference.md — a single user-facing reference for every
visible reflex command. Audited against src/reflex/cli.py so every verb,
flag, and example matches the live source. Examples for each top-level
verb are real (no fabricated flag combinations).

Drops fabricated content from the original attempt (#127) — the
deprecated 'reflex turbo' verb, the internal-only 'reflex bench',
the never-existed 'AGV / GPS-fused state' guidance — and replaces it
with content grounded in the FastCrest customer research vault: the
Vertical Quick-Start Matrix now mirrors the P0 customer subcategories
(warehouse AMR, autonomous tractors, mining, drone surveillance,
last-mile drone delivery, traffic management, smart-camera retail/
warehouse, ADAS/autonomous trucking, port inspection, baggage tugs).

Documents the new --state-msg-type flag from #133 alongside the
recommended drone state topics (/mavros/local_position/odom for full
state, /mavros/imu/data for orientation-only fallback).

Supersedes #127.

Co-authored-by: Divyansh Rawat <186957976+DsThakurRawat@users.noreply.github.com>
Co-authored-by: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
rylinjames added a commit that referenced this pull request May 15, 2026
Adds docs/adding_a_robot.md walking new contributors through adding
a fifth embodiment to Reflex. Two worked examples — MyArm-6 (6-DOF
arm + gripper) and SkyScout (quadcopter delivery drone) — both
validated against the live src/reflex/embodiments/schema.json before
commit. The example JSONs use only real schema fields (mean_action /
std_action / mean_state / std_state, resolution + color_space, etc.)
so anyone copy-pasting can run validate_embodiment_config on them
and get a clean pass after Step 3.

Drops fabrications from the original attempt (#128):
- Field names like 'labels', 'mean'/'std' (instead of mean_action),
  camera 'width'/'height'/'encoding' (instead of resolution +
  color_space), 'workspace_bbox' — none of which are in schema.json
- The 'configs/embodiments/' duplicate-file step (that location was
  a dev fallback; #132 made src/reflex/embodiments/presets/ canonical)

Reflects what's actually shipped after recent foundation merges:
- Optional gripper + payload_release (from #132)
- New quadcopter preset with 10-DOF state matching MAVROS Odometry
- ROS2 bridge --state-msg-type flag (from #133) for drone deployments
- 'Common patterns by vertical' section reflects the FastCrest
  customer research vault's P0 verticals (warehouse AMR, farm, drone,
  smart-camera)

Supersedes #128.

Co-authored-by: Divyansh Rawat <186957976+DsThakurRawat@users.noreply.github.com>
Co-authored-by: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
rylinjames added a commit that referenced this pull request May 16, 2026
 #63) (#137)

Adds docs/troubleshooting.md with the most common error signatures and
fixes Reflex users hit on edge devices, cloud GPUs, ROS2 robots, and
drones. Structure: CUDA/GPU → Jetson → ROS2 bridge → Drone/MAVROS →
Export/validation → Registry → Quick diagnostics.

Adapted from #129 — kept all the original error signatures and fix
patterns (they're real and useful). Only refreshed:
- onnxruntime-gpu pin: 1.18.0 → >=1.25.1 (matches v0.9.2 floor)
- cuDNN floor: 9.0+ → 9.5+ (matches v0.9.2 floor)
- Driver floor: R525+ → R555+ for cuDNN 9.5+ (per v0.9.4 doctor guard)
- Added Blackwell sm_120 section (RTX 5090 / B200) per v0.9.3 guard
- Added the four v0.9.4 reflex doctor guards as the prescribed first
  step on any failure (multi-GPU arch, Jetson R35, cuDNN/driver skew,
  TRT EP empirical session test)
- Replaced #129's reference to PR #121 with the actual shipped
  --state-msg-type flag from #133 (joint_state/imu/odom dispatch)
- Updated cross-ref from understanding_verification.md → verification.md
  (renamed in #136)
- Updated cross-ref to adding_a_robot.md (shipped in #135)
- File renamed TROUBLESHOOTING.md → troubleshooting.md for consistency
  with sibling docs (lowercase, no ALL_CAPS)

Supersedes #129.

Co-authored-by: Divyansh Rawat <186957976+DsThakurRawat@users.noreply.github.com>
Co-authored-by: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant