OptiTrack (1/3): autonomy fixes — natnet_ros2 client + PX4 external-vision fusion - #374
Draft
JohnYanxinLiu wants to merge 6 commits into
Draft
OptiTrack (1/3): autonomy fixes — natnet_ros2 client + PX4 external-vision fusion#374JohnYanxinLiu wants to merge 6 commits into
JohnYanxinLiu wants to merge 6 commits into
Conversation
…ion baseline Take the natnet_ros2 package from #367 onto the reworked base: the C++ NatNet client (natnet_ros2_node + client adapter + natnet_logic seam), the base mavros_gp_origin and vision_pose_converter nodes, per-robot natnet_config profiles, launch files, and the co-located C++/Python unit tests. natnet_ros2 is already listed in tests/colcon_unit_test_packages.yaml, so the base's YAML-driven collection picks up the updated unit tests directly — no proxy files. Real-robot PX4 external-vision fusion (px4_param_setter, geoid-corrected origin, EV-pose bounds) is layered on next. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Layer the Hummingbird real-robot fusion pipeline onto natnet_ros2 so an OptiTrack-only drone (no GNSS/mag/baro) fuses mocap pose into PX4 EKF2: - mavros_gp_origin_node: publishes a guarded synthetic GPS origin. On real HW, use_geoid_altitude feeds the egm96-5 geoid undulation (N ≈ 54 m at Lisbon) so mavros's ellipsoidal→AMSL conversion cancels and local z == OptiTrack z (fixes the ~36 m = 90 − 54 boot offset; see docs). Auto-skipped in sim. - vision_pose_converter_node: rate-limited mocap → MAVROS vision_pose bridge. - px4_params.yaml: the external-vision EKF2 param set. - natnet_ros2.launch.py wires the bridges when a robot's vision_pose block is on. px4_param_setter reworked into a **checker** (R3): auto_set=false by default — it reads and *flags* FCU params that differ from the desired set instead of writing them; on_mismatch=warn|halt (default warn). Set the params in QGroundControl; the node is the pre-flight safety net. auto_set=true restores the legacy enforce path. Excludes the duplicate vendored NatNet SDK (sensors/natnet_ros2) and deployment override .envs. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Move the PX4 external-vision setup guide into docs/ (was a repo-root markdown) and wire it into the mkdocs nav under Perception. Adapt it to the reworked param checker (auto_set default off; check-and-flag, not enforce), and add a "height datum" section explaining the ~36 m local_z offset: AirStack's 90.0 ellipsoidal world datum minus the egm96-5 geoid undulation (N ≈ 54 m at Lisbon) = 36 m; fixed by publishing the geoid-corrected origin altitude so mavros's conversion cancels. Documents why it's invisible in sim and why the shared 90.0 datum must not be changed globally. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
…hema Refine the perception bringup comment on the LAUNCH_NATNET include so it points at the per-robot natnet_config.yaml schema parsed by natnet_ros2.launch.py. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Version-increment gate for the OptiTrack autonomy PR (base test-infra alpha.8). Records the real-robot PX4 external-vision fusion in the changelog. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
…uning Three defects that together meant the OptiTrack client could never connect to anything, in sim or on a real robot. 1. NATNET_SERVER_IP was unreachable config. natnet_config.yaml resolves it via $(env ...), but docker compose only injects variables named in a service's `environment:` block and no service declared it — not the compose files, not .env, not tests/system/test_optitrack_e2e.py. The client therefore always fell back to its hardcoded default (192.168.123.199), which is neither the in-sim emulator (172.31.0.200) nor any Motive host. Forwarded in robot-base-docker-compose.yaml, defaulting to the emulator so the sim path works unconfigured. 2. The tracked rigid body could never match. robot_1 pinned "Hummingbird" id 1146 while the emulator streams "Drone" id 1, and the NatNet client filters incoming frames by NUMERIC id — a mismatch yields a connected client that silently never publishes. Body name/id now accept $(env ...) (expanded in _build_node_params, with the id still coerced to int) and default to the emulator's body; sites override via NATNET_BODY_NAME / NATNET_BODY_ID. 3. EV tuning was not the deployment-validated set. EKF2_EV_DELAY 8.0 -> 7.0 and EKF2_EVP_NOISE 0.01 -> 0.05. EKF2_EVP_NOISE is not marker precision: it also sets the innovation gate at EKF2_EVP_GATE (default 5) sigma, so 0.01 gave a 5 cm gate that rejected legitimate mocap updates and refused to arm. 0.05 is a 25 cm gate, still far tighter than PX4's 0.1 default. px4_params.yaml keeps the evidence inline, including two results that are expensive to rediscover: raising EKF2_EV_DELAY to 50.0 measurably degrades tracking (the negative best-fit time shift shows the estimate running ahead of truth), and the drift-and-snap excursions were a 90 deg body-yaw offset in the Motive rigid-body definition, not a gate problem — so the fix belongs in Motive, never as yaw compensation in code. Adds two unit tests covering body-field env expansion and the emulator-matching defaults (natnet_ros2: 14 -> 16 passing). Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
1 of 3 in the OptiTrack split (supersedes #367; stacked on #372). Robot-side only, unit-tested, non-breaking — no sim/GPU needed.
natnet_ros2client baseline (from Johnliu/optitrack emulation #367) + real-robot PX4 external-vision fusion:mavros_gp_origin(geoid-corrected synthetic GPS origin →local_position.z== OptiTrack z, fixing the ~36 m boot offset = 90 m ellipsoidal datum − ~54 m egm96-5 geoid),vision_pose_converter, and a PX4 param checker (px4_param_setter:auto_setoff by default — reads + flags, never writes;on_mismatchwarn/halt).docs/robot/px4_external_vision.md(setup + height-datum explainer) + mkdocs nav.Tests:
airstack test -m unit -v— natnet_ros2 co-located unit tests pass (21). No sim validation here; the emulator that exercises this end-to-end lands in 2/3 and 3/3.Version:
0.19.0-alpha.9. Base:johnliu/test-infra-rework(#372); retarget todevelopafter #371+#372 land.🤖 Generated with Claude Code