From 56b03497d58c8feb1277198877afbe6af39c2aca Mon Sep 17 00:00:00 2001 From: "marty-mcfly-bot[bot]" <297226227+marty-mcfly-bot[bot]@users.noreply.github.com> Date: Fri, 10 Jul 2026 13:38:49 -0600 Subject: [PATCH 1/8] Fix Ubuntu 24.04 user ID collision (#56) Co-authored-by: marty-mcfly-bot[bot] --- Dockerfile | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Dockerfile b/Dockerfile index 5355ec61a..214c1de2d 100644 --- a/Dockerfile +++ b/Dockerfile @@ -23,6 +23,10 @@ ARG USERNAME ARG USER_UID ARG USER_GID +# Ubuntu 24.04 images include user `ubuntu` with UID/GID 1000. +# Remove it before creating the workspace user with the host UID/GID. +RUN if id -u ubuntu > /dev/null 2>&1; then userdel -r ubuntu; fi + # Copy source code from the workspace's ROS 2 packages to a workspace inside the container ARG USER_WS=/home/${USERNAME}/user_ws ENV USER_WS=${USER_WS} From c333a1a61a7dafff66542d2bb30e0fe4702aefb0 Mon Sep 17 00:00:00 2001 From: Josh Whitley Date: Mon, 17 Aug 2026 16:54:43 -0600 Subject: [PATCH 2/8] ci: bump moveit_pro_ci to v0.9.1 for the license hardware identity The workspace integration test runs MoveIt Pro inside a bridge-networked container on an ephemeral runner -- no TPM, and no interface the licensing fingerprint will accept, since a container veth is rejected by design. Through 9.4.1 that was fine: the fingerprint hashed eth0's MAC and took the veth's. From 9.4.2 (moveit_pro#21596) it resolves a TPM endorsement key, else a permanent hardware NIC, and fails closed with neither -- so this job would stop activating a license the moment it pulls a 9.4.2 image. v0.9.1 is one commit on top of the pinned v0.9.0, adding only the step that builds a synthetic sysfs tree with a fixed MAC and points MOVEIT_HOST_SYSFS at it. No input signature change. The MAC is fixed so the runner fleet presents one identity rather than each ephemeral runner consuming its own activation. v10.0 already pins v0.9.1; this brings the 9.4 line in line ahead of 9.4.2. --- .github/workflows/ci.yaml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c89f0888f..0f3a40726 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -243,7 +243,10 @@ jobs: # can assume the LFS S3 cache IAM role via OIDC (moveit_pro_ci v0.5.x). contents: read id-token: write - uses: PickNikRobotics/moveit_pro_ci/.github/workflows/workspace_integration_test.yaml@a0e3b30c9aa8f8f82f95c91e5a34989d01caa046 # v0.9.0 + # v0.9.1 adds the synthetic host NIC the integration container licenses against. + # From MoveIt Pro 9.4.2 the fingerprint needs a TPM or a permanent NIC and fails + # closed with neither, which is every runner this job lands on. + uses: PickNikRobotics/moveit_pro_ci/.github/workflows/workspace_integration_test.yaml@2eff8c8aa644ed07acaaebc9eb15af4af7169e79 # v0.9.1 with: image_tag: ${{ needs.resolve.outputs.image_tag }} image_ref: ${{ needs.resolve.outputs.image_ref }} From eb7eb2cf606c5b188911c9714a7a06081c6d47c7 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Fri, 10 Jul 2026 11:33:47 -0400 Subject: [PATCH 3/8] Merge pull request #756 from PickNikRobotics/fix/18174-tf-single-owner-base-link Fix: Make robot_state_publisher the sole owner of the ridgeback_base_link transform in hangar_sim --- src/hangar_sim/config/config.yaml | 5 +- src/hangar_sim/config/fuse/fuse.yaml | 6 +- .../picknik_ur_mujoco_ros2_control.xacro | 15 +++- .../description/ur5e_ridgeback.xacro | 10 ++- ...V2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md | 47 ++++++---- .../robot_drivers_to_persist_sim.launch.py | 73 +++------------- .../test/objectives_integration_test.py | 86 +++++++++++++++++++ 7 files changed, 163 insertions(+), 79 deletions(-) diff --git a/src/hangar_sim/config/config.yaml b/src/hangar_sim/config/config.yaml index 6162586fb..11b75f8bf 100644 --- a/src/hangar_sim/config/config.yaml +++ b/src/hangar_sim/config/config.yaml @@ -21,7 +21,10 @@ hardware: path: "config/moveit/joint_limits.yaml" - mujoco_model: "description/hangar_scene.xml" - mujoco_viewer: false # set to true locally to open the MuJoCo viewer window - # Set to false when use_fuse:=true so fuse is the sole odom -> ridgeback_base_link publisher. + # Publishes /odom messages for Nav2. The odom -> ridgeback_base_link TF edge is NOT + # published by MuJoCo (odom_publish_tf is false in the ros2_control xacro); + # robot_state_publisher owns the live transform to ridgeback_base_link via + # the virtual-rail joint chain, so this can stay true with fuse enabled. # Rebuild hangar_sim after changing this value. - publish_odom: true diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml index 2903b306d..4ad1039f5 100644 --- a/src/hangar_sim/config/fuse/fuse.yaml +++ b/src/hangar_sim/config/fuse/fuse.yaml @@ -87,7 +87,11 @@ state_estimator: base_link_output_frame_id: 'ridgeback_base_link' odom_frame_id: 'odom' map_frame_id: 'map' + # robot_state_publisher owns the live TF to ridgeback_base_link via the + # virtual-rail joint chain; fuse must not publish a competing parent. + # Its estimate stays available on odom_filtered. On real hardware, feed + # the estimate into the virtual-rail joint states instead of TF. world_frame_id: 'odom' - publish_tf: true + publish_tf: false publish_frequency: 10.0 predict_to_current_time: true diff --git a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro index 56dca9249..7b9372a27 100644 --- a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro +++ b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro @@ -23,9 +23,22 @@ ${publish_odom} ridgeback_base_link 150 - true + + true + + false odom false + + ridgeback_base_link ${mujoco_viewer} default diff --git a/src/hangar_sim/description/ur5e_ridgeback.xacro b/src/hangar_sim/description/ur5e_ridgeback.xacro index 4863292d8..bb6e49c56 100644 --- a/src/hangar_sim/description/ur5e_ridgeback.xacro +++ b/src/hangar_sim/description/ur5e_ridgeback.xacro @@ -19,7 +19,9 @@ - + @@ -76,7 +78,11 @@ - + + diff --git a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md index 2b3253674..63aab85f2 100644 --- a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md +++ b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md @@ -804,12 +804,19 @@ static_tf_world_to_map = Node( arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "mj_world", "map"], ) -# Static transform: map to odometry frame +# Static map->odom TF fallback: only used when neither SLAM nor AMCL is publishing it static_tf_map_to_odom = Node( package="tf2_ros", executable="static_transform_publisher", arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "map", "odom"], ) + +# Static transform anchoring MoveIt's planning root under the odometry frame +static_tf_odom_to_world = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "odom", "world"], +) ``` ### Frame Hierarchy @@ -821,15 +828,17 @@ mj_world (MuJoCo simulation root) │ ├─ [static] → map │ │ - │ └─ [static] → odom + │ └─ [beluga_amcl | static fallback] → odom │ │ - │ └─ [robot_state_publisher] → ridgeback_base_link - │ (via virtual joint chain) + │ └─ [static] → world + │ │ + │ └─ [robot_state_publisher] → ridgeback_base_link + │ (via world → virtual_rail_link_1 → virtual_rail_link_2 chain) │ └─ [Other scene elements] ``` -In production deployments with active localization, the static `map` to `odom` transform would be replaced with a dynamic transform published by the localization system (e.g., AMCL), representing the estimated pose correction between odometry and the global map frame. +With `localization:=True` (the default), beluga_amcl publishes the dynamic `map` → `odom` correction and the static fallback is suppressed; its correction shifts the entire robot subtree (everything under `odom`), which is exactly the REP-105 localization semantics. --- @@ -864,14 +873,22 @@ The `enable_odom_tf: false` parameter prevents the mecanum drive controllers fro | Transform | Publishing Node | Mechanism | |-----------|----------------|-----------| | `mj_world` → `map` | `static_transform_publisher` | Launch file configuration | -| `map` → `odom` | `static_transform_publisher` | Launch file configuration (simulation only) | -| `odom` → `ridgeback_base_link` | `robot_state_publisher` | URDF virtual joint chain with joint state feedback | +| `map` → `odom` | `beluga_amcl` (or `static_transform_publisher` fallback) | Localization when `localization:=True`; static identity otherwise | +| `odom` → `world` | `static_transform_publisher` | Launch file configuration (anchors the URDF root under the odometry frame) | +| `world` → … → `ridgeback_base_link` | `robot_state_publisher` | URDF virtual joint chain with joint state feedback | +| `ridgeback_base_link` → lidar mounts | MuJoCo hardware plugin | Lidar fill-in chain, stopped at the base by the `base_link_name` hardware parameter | + +`robot_state_publisher` is the **sole owner** of the live transform into `ridgeback_base_link`. Every other subsystem that knows the base pose expresses it as data, not as a competing TF parent: + +- The MuJoCo odom publisher emits `/odom` messages for Nav2 but not TF (`odom_publish_tf: false`). +- The MuJoCo lidar fill-in chain stops at `ridgeback_base_link` (`base_link_name` hardware parameter) instead of broadcasting the ground-truth body chain (`base_platform` → `ridgeback_base_link`) up to the MJCF worldbody. +- fuse keeps `publish_tf: false`; its estimate stays on `odom_filtered`. On real hardware the estimate feeds the virtual-rail joint states (odometry-to-joint-state bridge) rather than TF. ### Transform Source Analysis -The `robot_state_publisher` node computes and publishes the `odom` to `ridgeback_base_link` transform based on: +The `robot_state_publisher` node computes and publishes the transform into `ridgeback_base_link` based on: 1. The URDF virtual joint chain definition (`world` → `virtual_rail_link_1` → `virtual_rail_link_2` → `ridgeback_base_link`) -2. Current joint state values received on `/joint_states` (populated by the odometry bridge) +2. Current joint state values received on `/joint_states` — in simulation these come straight from the MuJoCo virtual-rail joints via `joint_state_broadcaster` (ground truth); on real hardware they come from an odometry-to-joint-state bridge fed by the state estimator If the mecanum drive controllers were configured with `enable_odom_tf: true`, they would publish the same transform based on wheel odometry integration. This would result in multiple publishers for the same transform, violating the ROS 2 transform system's requirement for unique transform publishers. @@ -881,10 +898,10 @@ Enabling odometry transform publication on the controllers would produce the fol **Multiple Transform Publishers** ``` -Transform odom → ridgeback_base_link published by: -1. robot_state_publisher (from virtual joint states) -2. platform_velocity_controller (from wheel odometry) -3. platform_velocity_controller_nav2 (from wheel odometry) +Transforms into ridgeback_base_link published by: +1. robot_state_publisher (world → … → ridgeback_base_link, from virtual joint states) +2. platform_velocity_controller (odom → ridgeback_base_link, from wheel odometry) +3. platform_velocity_controller_nav2 (odom → ridgeback_base_link, from wheel odometry) ``` **System-Level Effects** @@ -927,7 +944,7 @@ In real-world systems with active localization: - Remove the static `map` → `odom` transform publisher - Configure the localization system (AMCL, Cartographer, etc.) to publish the dynamic `map` → `odom` transform - Maintain `enable_odom_tf: false` on platform controllers -- The `odom` → `base_link` transform continues to be published by `robot_state_publisher` via virtual joints +- The `world` → … → `ridgeback_base_link` chain continues to be published by `robot_state_publisher` via virtual joints; `odom` → `world` stays a static link. On hardware, the state estimate feeds the virtual-rail joint states (odometry-to-joint-state bridge) rather than TF - Localization system consumes odometry messages to estimate drift and publish correction transforms --- @@ -1029,7 +1046,7 @@ For implementing whole body planning on mobile manipulator platforms: | Mobile base unresponsive during whole body planning | Trajectory controller not activated | Query controller manager state: `ros2 control list_controllers` | Verify controller activates when executing trajectories; check action server connection | | Base motion in incorrect direction | Frame transformation error | Verify yaw joint in `/joint_states`; check `body_frame_yaw_joint` parameter | Confirm `odometry_joint_state_publisher.py` is running; verify parameter configuration | | Nav2 commands not executed | Incorrect controller active state | Check controller manager state; verify command topic remapping | Activate `platform_velocity_controller_nav2`; verify `/cmd_vel` remapping | -| Virtual joints absent from state | Odometry bridge failure | Monitor `/joint_states` for virtual joint messages; check node status | Restart `odometry_joint_state_publisher.py`; verify `/odom` topic publication | +| Virtual joints absent from state | Joint state source failure | Monitor `/joint_states` for virtual joint messages; check node status | In simulation the virtual-rail joints come from MuJoCo via `joint_state_broadcaster` — check `ros2 control list_controllers`. On hardware, restart the odometry-to-joint-state bridge and verify odometry publication | | Planning failures for whole body group | Incorrect planning group configuration | Examine SRDF planning group definition; verify joint names | Use `manipulator` group for whole body; confirm SRDF includes virtual joints | | TF_REPEATED_DATA warnings | Multiple transform publishers | Execute `ros2 run tf2_tools view_frames`; identify duplicate publishers | Set `enable_odom_tf: false` in both platform controllers; verify single source per transform | | Discontinuous position in visualization | Transform tree inconsistency | Monitor `/tf` for conflicting transforms; check timing | Verify transform publication sources; ensure consistent timestamp sources | diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py index cb8725353..1a0e14a48 100644 --- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -28,7 +28,6 @@ import os -import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -36,10 +35,7 @@ DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, - LogInfo, - OpaqueFunction, SetEnvironmentVariable, - Shutdown, ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -55,52 +51,6 @@ from nav2_common.launch import RewrittenYaml, ReplaceString -def _check_fuse_publish_odom(context, *args, **kwargs): - """Fail fast if use_fuse:=true but MuJoCo is still configured to publish odom TF. - - When fuse is enabled it is the sole authority on odom → ridgeback_base_link. - MuJoCo must not also publish that edge or consumers will see jittery, interleaved updates. - Set 'publish_odom: "false"' in config.yaml under hardware.robot_description.urdf_params. - """ - if LaunchConfiguration("use_fuse").perform(context).lower() != "true": - return [] - - config_path = os.path.join( - get_package_share_directory("hangar_sim"), "config", "config.yaml" - ) - with open(config_path) as f: - robot_config = yaml.safe_load(f) - - urdf_params = ( - robot_config.get("hardware", {}) - .get("robot_description", {}) - .get("urdf_params", []) - ) - publish_odom = True # MuJoCo default - for param in urdf_params: - if isinstance(param, dict) and "publish_odom" in param: - publish_odom = str(param["publish_odom"]).lower() not in ( - "false", - "0", - "no", - ) - break - - if publish_odom: - return [ - LogInfo( - msg=( - "use_fuse:=true but MuJoCo is still configured to publish" - " odom -> ridgeback_base_link. When fuse is enabled, fuse must be the sole" - " publisher of that TF edge. Set 'publish_odom: false' in config.yaml under" - " hardware.robot_description.urdf_params, then rebuild hangar_sim." - ) - ), - Shutdown(reason="publish_odom must be false when use_fuse:=true"), - ] - return [] - - def generate_launch_description(): # Get the relevant directories # The config directory (Contains the map and the parameters) @@ -328,15 +278,21 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(["not ", slam])), ) - # Static TF connecting MoveIt's planning root ('world') to the simulation root ('mj_world'). - # The UI (pose-utils.ts) hardcodes 'world' as the reference frame for all user-clicked poses, - # so this link is required for nav2 goals to be transformable to 'map'. - static_tf_mj_world_to_world = Node( + # Static TF anchoring MoveIt's planning root ('world') under the odometry frame. + # robot_state_publisher owns the only live chain into ridgeback_base_link + # (world -> virtual_rail_... -> ridgeback_base_link), so 'odom' must sit above + # 'world' for REP-105 semantics: AMCL's live map->odom correction then shifts the + # whole robot subtree, and its own odom->base lookups resolve through this link. + # (Previously this was mj_world->world, and the 'odom' frame only existed because + # MuJoCo broadcast a competing odom->ridgeback_base_link TF — removed in this + # change.) The UI (pose-utils.ts) hardcodes 'world' for user-clicked poses, so + # this link also keeps nav2 goals transformable to 'map'. + static_tf_odom_to_world = Node( package="tf2_ros", executable="static_transform_publisher", - name="static_tf_mj_world_to_world", + name="static_tf_odom_to_world", output="log", - arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "mj_world", "world"], + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "odom", "world"], ) # QoS relay to bridge BEST_EFFORT odom and IMU to RELIABLE for fuse @@ -407,7 +363,7 @@ def generate_launch_description(): # Set environment variables ld.add_action(stdout_linebuf_envvar) - # Declare the launch options — all args must be declared before the OpaqueFunction runs. + # Declare the launch options. ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_slam_cmd) @@ -419,7 +375,6 @@ def generate_launch_description(): ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) ld.add_action(declare_use_fuse_cmd) - ld.add_action(OpaqueFunction(function=_check_fuse_publish_odom)) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) @@ -430,7 +385,7 @@ def generate_launch_description(): # ld.add_action(rviz_cmd) ld.add_action(static_tf_world_to_map) - ld.add_action(static_tf_mj_world_to_world) + ld.add_action(static_tf_odom_to_world) ld.add_action(static_tf_map_to_odom) ld.add_action(sensor_qos_relay) ld.add_action(laser_filter_front_node) diff --git a/src/hangar_sim/test/objectives_integration_test.py b/src/hangar_sim/test/objectives_integration_test.py index 38d69cdac..195afe2ad 100644 --- a/src/hangar_sim/test/objectives_integration_test.py +++ b/src/hangar_sim/test/objectives_integration_test.py @@ -34,8 +34,11 @@ import pytest import rclpy import tf2_ros +from tf2_msgs.msg import TFMessage +from nav_msgs.msg import Odometry import yaml from rclpy.time import Time +from rclpy.qos import qos_profile_sensor_data from control_msgs.action import GripperCommand from controller_manager_msgs.srv import ListControllers from moveit_pro_test_utils.objective_test_fixture import ( @@ -318,6 +321,89 @@ def _expected_end_state_by_id( return {objective_id: EndStateSpec(joints=JointTarget(positions=arm_target))} +BASE_LINK_FRAME = "ridgeback_base_link" +# Phase 1: how long to wait for the FIRST transform into the base link (and the +# first /odom message) before declaring the stack broken. Generous on purpose: +# it only bounds startup latency on a loaded CI runner, not the assertion. +TF_FIRST_SIGHTING_TIMEOUT_S = 60.0 +# Phase 2: once the frame has been seen, how long to keep sampling to catch +# additional (conflicting) parents. Competing publishers broadcast at 10-120 Hz, +# so a second parent would appear hundreds of times within this window. +TF_PARENT_SAMPLE_DURATION_S = 4.0 + + +def test_base_link_has_single_tf_parent( + execute_objective_resource: ExecuteObjectiveResource, +) -> None: + """The live transform into ridgeback_base_link must have exactly one publisher. + + Regression guard for the three-parent TF conflict (MuJoCo lidar fill-in + chain, MuJoCo odom TF, and robot_state_publisher all claiming the frame), + which made the robot oscillate below base_link in the web UI. + robot_state_publisher owns the edge via the virtual-rail chain; a second + parent appearing here means a MuJoCo TF publisher was re-enabled (check + base_link_name / odom_publish_tf in the ros2_control xacro) or fuse's + publish_tf was turned back on. + """ + node = execute_objective_resource.node + parents: set[str] = set() + odom_z_samples: list[float] = [] + + # Watching /tf only is correct while rotational_yaw_joint (the edge into + # ridgeback_base_link) is a moving joint; if it ever becomes fixed the edge + # moves to /tf_static and this test must follow it there. + def collect(msg: TFMessage) -> None: + for transform in msg.transforms: + if transform.child_frame_id == BASE_LINK_FRAME: + parents.add(transform.header.frame_id) + + def collect_odom(msg: Odometry) -> None: + odom_z_samples.append(msg.pose.pose.position.z) + + subscription = node.create_subscription(TFMessage, "/tf", collect, 100) + # Piggyback an odom_planar check: odom_zero_z was a silently-ignored + # parameter for months, so pin the real one's effect (z zeroed in /odom) + # rather than trusting the spelling. + odom_subscription = node.create_subscription( + Odometry, "/odom", collect_odom, qos_profile_sensor_data + ) + # Phase 1: wait for the first sighting so slow stack bring-up on a loaded + # CI runner cannot masquerade as "wrong publishers" (or eat into the + # sampling window below). + first_sighting_deadline = time.monotonic() + TF_FIRST_SIGHTING_TIMEOUT_S + while time.monotonic() < first_sighting_deadline and not ( + parents and odom_z_samples + ): + rclpy.spin_once(node, timeout_sec=0.1) + + # Distinguish "frame never seen" (a startup failure or renamed frame) from + # "wrong publishers" so failures read correctly. + assert parents, ( + f"No transform into {BASE_LINK_FRAME} observed on /tf within " + f"{TF_FIRST_SIGHTING_TIMEOUT_S:.1f}s — robot_state_publisher may not " + f"be up, or the frame was renamed." + ) + + # Phase 2: the frame is live; sample long enough that any competing + # publisher (10-120 Hz) would land in `parents`. + sample_deadline = time.monotonic() + TF_PARENT_SAMPLE_DURATION_S + while time.monotonic() < sample_deadline: + rclpy.spin_once(node, timeout_sec=0.1) + node.destroy_subscription(subscription) + node.destroy_subscription(odom_subscription) + assert parents == {"virtual_rail_link_2"}, ( + f"Expected robot_state_publisher's virtual_rail_link_2 as the sole TF " + f"parent of {BASE_LINK_FRAME}, saw parents: {sorted(parents)}" + ) + assert ( + odom_z_samples + ), "No /odom messages observed; MuJoCo odom publisher may be off." + assert all(abs(z) < 1e-6 for z in odom_z_samples), ( + f"odom_planar should zero /odom pose z for this planar base; " + f"saw max |z| = {max(abs(z) for z in odom_z_samples):.4f}" + ) + + @pytest.mark.parametrize( "objective_id, should_cancel", get_objective_pytest_params("hangar_sim", cancel_objectives, skip_objectives), From 860f4754f4ad3152b176227960a8feab5325f046 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 19 Aug 2026 16:50:00 -0400 Subject: [PATCH 4/8] no-mistakes(review): Correct fabricated AMCL doc claims; guard userdel exit code --- Dockerfile | 2 +- .../docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md | 8 +++++--- .../launch/sim/robot_drivers_to_persist_sim.launch.py | 5 +++-- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/Dockerfile b/Dockerfile index 214c1de2d..a38141306 100644 --- a/Dockerfile +++ b/Dockerfile @@ -25,7 +25,7 @@ ARG USER_GID # Ubuntu 24.04 images include user `ubuntu` with UID/GID 1000. # Remove it before creating the workspace user with the host UID/GID. -RUN if id -u ubuntu > /dev/null 2>&1; then userdel -r ubuntu; fi +RUN if id -u ubuntu > /dev/null 2>&1; then userdel -r ubuntu || userdel ubuntu; fi # Copy source code from the workspace's ROS 2 packages to a workspace inside the container ARG USER_WS=/home/${USERNAME}/user_ws diff --git a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md index 63aab85f2..0e55fafcd 100644 --- a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md +++ b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md @@ -828,7 +828,7 @@ mj_world (MuJoCo simulation root) │ ├─ [static] → map │ │ - │ └─ [beluga_amcl | static fallback] → odom + │ └─ [static identity | slam_toolbox when slam:=True] → odom │ │ │ └─ [static] → world │ │ @@ -838,7 +838,9 @@ mj_world (MuJoCo simulation root) └─ [Other scene elements] ``` -With `localization:=True` (the default), beluga_amcl publishes the dynamic `map` → `odom` correction and the static fallback is suppressed; its correction shifts the entire robot subtree (everything under `odom`), which is exactly the REP-105 localization semantics. +In this configuration `map` → `odom` is a static identity: AMCL is commented out in `localization_launch.py` (`nav2_params.yaml` states "amcl is not used because odom is received directly from MuJoCo"), and `static_tf_map_to_odom` is published whenever `slam:=False` (the default). Only `slam:=True` replaces it, with `slam_toolbox` owning the correction. Whichever node owns it, the correction applies above `odom` and therefore shifts the entire robot subtree, which is the REP-105 localization semantics. + +> Note: this section diverges from the same doc on `main` (PR #756), which describes a beluga_amcl-driven `map` → `odom`. The v9.4 branch has no AMCL/localization wiring and no `localization` launch argument, so the static identity described above is what actually runs here. --- @@ -873,7 +875,7 @@ The `enable_odom_tf: false` parameter prevents the mecanum drive controllers fro | Transform | Publishing Node | Mechanism | |-----------|----------------|-----------| | `mj_world` → `map` | `static_transform_publisher` | Launch file configuration | -| `map` → `odom` | `beluga_amcl` (or `static_transform_publisher` fallback) | Localization when `localization:=True`; static identity otherwise | +| `map` → `odom` | `static_transform_publisher` (or `slam_toolbox` when `slam:=True`) | Static identity by default; AMCL is not launched on v9.4 | | `odom` → `world` | `static_transform_publisher` | Launch file configuration (anchors the URDF root under the odometry frame) | | `world` → … → `ridgeback_base_link` | `robot_state_publisher` | URDF virtual joint chain with joint state feedback | | `ridgeback_base_link` → lidar mounts | MuJoCo hardware plugin | Lidar fill-in chain, stopped at the base by the `base_link_name` hardware parameter | diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py index 1a0e14a48..5073d6fe8 100644 --- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -281,8 +281,9 @@ def generate_launch_description(): # Static TF anchoring MoveIt's planning root ('world') under the odometry frame. # robot_state_publisher owns the only live chain into ridgeback_base_link # (world -> virtual_rail_... -> ridgeback_base_link), so 'odom' must sit above - # 'world' for REP-105 semantics: AMCL's live map->odom correction then shifts the - # whole robot subtree, and its own odom->base lookups resolve through this link. + # 'world' for REP-105 semantics: a live map->odom correction (slam_toolbox today, + # AMCL if it is ever enabled here) then shifts the whole robot subtree, and its + # own odom->base lookups resolve through this link. # (Previously this was mj_world->world, and the 'odom' frame only existed because # MuJoCo broadcast a competing odom->ridgeback_base_link TF — removed in this # change.) The UI (pose-utils.ts) hardcodes 'world' for user-clicked poses, so From a60dbbc5ce660d9e3ed0dc19f782e274b7f9ac54 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 19 Aug 2026 17:41:20 -0400 Subject: [PATCH 5/8] no-mistakes(document): Sync hangar_sim architecture doc with TF ownership change --- ...V2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md | 77 ++++++++++--------- 1 file changed, 41 insertions(+), 36 deletions(-) diff --git a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md index 0e55fafcd..7712eea78 100644 --- a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md +++ b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md @@ -69,17 +69,20 @@ The whole body planning capability requires **custom modifications** to the `cle │ MuJoCo Sim │ └───────────────┘ │ - ▼ - ┌───────────────┐ - │ Odometry │ - │ Publisher │ - └───────────────┘ - │ - ▼ + ┌───────────┴───────────┐ + │ │ + ▼ ▼ + ┌───────────────────────┐ │ + │ Odometry Publisher │ │ + │ → /odom (Nav2 only, │ │ + │ no TF broadcast) │ │ + └───────────────────────┘ │ + ▼ ┌───────────────────────────────────┐ - │ odometry_joint_state_publisher.py│ + │ joint_state_broadcaster │ │ │ - │ Converts /odom → /joint_states │ + │ Publishes /joint_states for the │ + │ MuJoCo virtual-rail joints: │ │ - linear_x_joint │ │ - linear_y_joint │ │ - rotational_yaw_joint │ @@ -230,7 +233,7 @@ These modifications follow the same architectural pattern regardless of drive co The mobile base's three planar degrees of freedom (translation in X and Y, rotation about Z) are represented as joints within the URDF kinematic tree. This abstraction allows MoveIt to treat the mobile base as additional articulated joints, enabling unified trajectory planning across the combined arm-base system. ### Implementation Details -**Location**: `src/hangar_sim/description/ur5e_ridgeback.xacro:73-100` +**Location**: `src/hangar_sim/description/ur5e_ridgeback.xacro:75-108` ```xml @@ -239,8 +242,10 @@ The mobile base's three planar degrees of freedom (translation in X and Y, rotat - - + + @@ -271,7 +276,7 @@ The mobile base's three planar degrees of freedom (translation in X and Y, rotat The virtual joints establish the following kinematic chain: `world` → `virtual_rail_link_1` → `virtual_rail_link_2` → `ridgeback_base_link`. This chain represents the mobile base's pose in the global reference frame through three sequential transformations corresponding to planar motion. ### Control Characteristics -These joints lack direct physical actuators. Instead, their commanded velocities are transformed into mecanum wheel commands through inverse kinematics performed by the platform velocity controller. Joint state feedback is derived from odometry integration via the odometry-to-joint-state bridge. +These joints lack direct physical actuators. Instead, their commanded velocities are transformed into mecanum wheel commands through inverse kinematics performed by the platform velocity controller. In simulation, joint state feedback comes straight from the MuJoCo virtual-rail joints via `joint_state_broadcaster`; on hardware it is derived from odometry integration via the odometry-to-joint-state bridge (Component 3). --- @@ -323,6 +328,8 @@ The `manipulator` group defines a kinematic chain originating from `ridgeback_ba ### System Requirements MoveIt requires joint state information for all joints in the planning group to maintain an accurate robot state representation. For virtual joints representing the mobile base, these states must be derived from the platform's odometry. +**In this simulation the bridge below is not launched.** The virtual-rail joints are real MuJoCo joints, so `joint_state_broadcaster` publishes their ground-truth positions on `/joint_states` directly. `odometry_joint_state_publisher.py` is installed by `CMakeLists.txt` and documented here as the hardware pattern: on a real base the state estimate is converted into virtual-rail joint states this way rather than broadcast as TF (see "Transform Publishing Ownership"). + ### Implementation **Location**: `src/hangar_sim/script/odometry_joint_state_publisher.py` @@ -353,21 +360,19 @@ class OdometryJointStateRepublisher(Node): ### Data Flow Architecture ``` -MuJoCo Simulation → /odom (nav_msgs/Odometry) → odometry_joint_state_publisher.py +# Hardware (bridge active): +State estimate → /odom (nav_msgs/Odometry) → odometry_joint_state_publisher.py + → /joint_states (sensor_msgs/JointState) → MoveIt Robot State + +# Simulation (what runs here): +MuJoCo virtual-rail joints → joint_state_broadcaster → /joint_states (sensor_msgs/JointState) → MoveIt Robot State ``` ### Node Configuration -**Location**: `src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py:274-280` - -```python -odom_to_joint_state_repub = Node( - package="hangar_sim", - executable="odometry_joint_state_publisher.py", - name="odometry_joint_state_publisher", - output="log", -) -``` +No launch file instantiates this node; it would be added to the driver launch on a real +base. Running it in simulation would put a second publisher of the virtual-rail joint +positions on `/joint_states`, competing with `joint_state_broadcaster`. ### Functional Description This node subscribes to odometry messages published by the mecanum drive controller and converts the pose information into joint states for the three virtual joints. The conversion extracts the X and Y positions directly and computes the yaw angle from the quaternion orientation. These joint states are then published on the `/joint_states` topic, where they are consumed by `robot_state_publisher` and MoveIt's planning scene monitor. @@ -705,8 +710,8 @@ Wheel velocity commands are written to the MuJoCo simulation interfaces, causing **Step 10: Odometry Integration** The simulation integrates wheel velocities to compute platform motion and publishes odometry messages on `/odom`. -**Step 11: State Conversion** -The `odometry_joint_state_publisher.py` node converts odometry messages to joint state messages and publishes them on `/joint_states`. +**Step 11: State Feedback** +`joint_state_broadcaster` publishes the MuJoCo virtual-rail joint positions on `/joint_states`. (On hardware this step is the odometry-to-joint-state bridge instead — see Component 3.) **Step 12: State Update** MoveIt's planning scene monitor receives the joint state updates, updating the robot state representation. The `joint_trajectory_controller` uses this feedback for closed-loop trajectory tracking and error correction. @@ -717,22 +722,23 @@ MoveIt's planning scene monitor receives the joint state updates, updating the r ### Initial Configuration -**Location**: `src/hangar_sim/config/config.yaml:94-105` +**Location**: `src/hangar_sim/config/config.yaml:98-111` ```yaml -ros2_control: +ros2_control: # config.yaml:89 controllers_active_at_startup: - "force_torque_sensor_broadcaster" + - "imu_sensor_broadcaster" - "joint_state_broadcaster" - "platform_velocity_controller" - "vacuum_gripper" - controllers_inactive_at_startup: - - "joint_trajectory_controller" - - "servo_controller" - "platform_velocity_controller_nav2" + - "joint_trajectory_controller" - "velocity_force_controller" - "arm_only_velocity_force_controller" + - "joint_velocity_controller" + - "arm_only_joint_velocity_controller" ``` ### State Transition Model @@ -780,7 +786,7 @@ The trajectory and navigation controllers are activated on-demand when their res ### Command Routing Configuration -**Location**: `src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py:81-85` +**Location**: `src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py:86-90` ```python remappings = [ @@ -794,7 +800,7 @@ Nav2's velocity commands are remapped to the `platform_velocity_controller_nav2` ### Transform Tree Configuration -**Location**: `src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py:256-272` +**Location**: `src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py:260-297` ```python # Static transform: MuJoCo world to map frame @@ -922,13 +928,12 @@ The controllers continue to publish odometry data as messages despite disabled t | `platform_velocity_controller` | No (`enable_odom_tf: false`) | Yes (to `/platform_velocity_controller/odom`) | | `platform_velocity_controller_nav2` | No (`enable_odom_tf: false`) | Yes (to `/platform_velocity_controller_nav2/odom`) | | `robot_state_publisher` | Yes (from URDF with joint states) | No | -| `odometry_joint_state_publisher.py` | No | No (converts messages to joint states) | +| `joint_state_broadcaster` | No | No (publishes the virtual-rail joint states) | ### Message vs Transform Distinction **Odometry Messages**: Data structures of type `nav_msgs/Odometry` published on topics, containing pose and twist estimates with covariance information. These messages are consumed by: - Nav2 for localization and path tracking -- The odometry bridge for joint state conversion - Monitoring and diagnostic tools - Data logging systems @@ -1046,7 +1051,7 @@ For implementing whole body planning on mobile manipulator platforms: | Symptom | Probable Cause | Diagnostic Steps | Resolution | |---------|---------------|------------------|------------| | Mobile base unresponsive during whole body planning | Trajectory controller not activated | Query controller manager state: `ros2 control list_controllers` | Verify controller activates when executing trajectories; check action server connection | -| Base motion in incorrect direction | Frame transformation error | Verify yaw joint in `/joint_states`; check `body_frame_yaw_joint` parameter | Confirm `odometry_joint_state_publisher.py` is running; verify parameter configuration | +| Base motion in incorrect direction | Frame transformation error | Verify yaw joint in `/joint_states`; check `body_frame_yaw_joint` parameter | Confirm `joint_state_broadcaster` is active (`ros2 control list_controllers`); verify parameter configuration | | Nav2 commands not executed | Incorrect controller active state | Check controller manager state; verify command topic remapping | Activate `platform_velocity_controller_nav2`; verify `/cmd_vel` remapping | | Virtual joints absent from state | Joint state source failure | Monitor `/joint_states` for virtual joint messages; check node status | In simulation the virtual-rail joints come from MuJoCo via `joint_state_broadcaster` — check `ros2 control list_controllers`. On hardware, restart the odometry-to-joint-state bridge and verify odometry publication | | Planning failures for whole body group | Incorrect planning group configuration | Examine SRDF planning group definition; verify joint names | Use `manipulator` group for whole body; confirm SRDF includes virtual joints | From bb8a034efcc70b9d0d042ad9108cbcfb7ef01f7e Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 19 Aug 2026 17:43:32 -0400 Subject: [PATCH 6/8] no-mistakes(document): Reword AMCL divergence note as self-contained behavior --- .../docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md index 7712eea78..a353d31ca 100644 --- a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md +++ b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md @@ -846,7 +846,7 @@ mj_world (MuJoCo simulation root) In this configuration `map` → `odom` is a static identity: AMCL is commented out in `localization_launch.py` (`nav2_params.yaml` states "amcl is not used because odom is received directly from MuJoCo"), and `static_tf_map_to_odom` is published whenever `slam:=False` (the default). Only `slam:=True` replaces it, with `slam_toolbox` owning the correction. Whichever node owns it, the correction applies above `odom` and therefore shifts the entire robot subtree, which is the REP-105 localization semantics. -> Note: this section diverges from the same doc on `main` (PR #756), which describes a beluga_amcl-driven `map` → `odom`. The v9.4 branch has no AMCL/localization wiring and no `localization` launch argument, so the static identity described above is what actually runs here. +> Note: this configuration has no active AMCL-based localization — `localization_launch.py` includes no live `amcl`/`beluga_amcl` node, so nothing estimates a `map` → `odom` correction unless `slam:=True` starts `slam_toolbox`. Newer releases do run beluga_amcl for that correction; here the transform is the static identity described above. --- From 1469de9074c9a45b597830d31017a5f3e0f1317e Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 19 Aug 2026 18:02:17 -0400 Subject: [PATCH 7/8] no-mistakes(review): Fix userdel fallback; add slam condition to TF doc snippet --- Dockerfile | 2 +- .../docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Dockerfile b/Dockerfile index a38141306..0e07ca6dc 100644 --- a/Dockerfile +++ b/Dockerfile @@ -25,7 +25,7 @@ ARG USER_GID # Ubuntu 24.04 images include user `ubuntu` with UID/GID 1000. # Remove it before creating the workspace user with the host UID/GID. -RUN if id -u ubuntu > /dev/null 2>&1; then userdel -r ubuntu || userdel ubuntu; fi +RUN if id -u ubuntu > /dev/null 2>&1; then userdel -r ubuntu || true; fi # Copy source code from the workspace's ROS 2 packages to a workspace inside the container ARG USER_WS=/home/${USERNAME}/user_ws diff --git a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md index a353d31ca..592e37152 100644 --- a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md +++ b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md @@ -810,11 +810,12 @@ static_tf_world_to_map = Node( arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "mj_world", "map"], ) -# Static map->odom TF fallback: only used when neither SLAM nor AMCL is publishing it +# Static map->odom bootstrap: skipped under slam:=True, where slam_toolbox owns map->odom static_tf_map_to_odom = Node( package="tf2_ros", executable="static_transform_publisher", arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "map", "odom"], + condition=IfCondition(PythonExpression(["not ", slam])), ) # Static transform anchoring MoveIt's planning root under the odometry frame From 40e81ae48c7784e27df854fb9436691e155b1b52 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 19 Aug 2026 18:58:32 -0400 Subject: [PATCH 8/8] no-mistakes(document): Fix dangling section cross-reference in hangar_sim architecture doc --- .../docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md index 592e37152..96fc922f0 100644 --- a/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md +++ b/src/hangar_sim/docs/NAV2_AND_WHOLE_BODY_PLANNING_ARCHITECTURE.md @@ -328,7 +328,7 @@ The `manipulator` group defines a kinematic chain originating from `ridgeback_ba ### System Requirements MoveIt requires joint state information for all joints in the planning group to maintain an accurate robot state representation. For virtual joints representing the mobile base, these states must be derived from the platform's odometry. -**In this simulation the bridge below is not launched.** The virtual-rail joints are real MuJoCo joints, so `joint_state_broadcaster` publishes their ground-truth positions on `/joint_states` directly. `odometry_joint_state_publisher.py` is installed by `CMakeLists.txt` and documented here as the hardware pattern: on a real base the state estimate is converted into virtual-rail joint states this way rather than broadcast as TF (see "Transform Publishing Ownership"). +**In this simulation the bridge below is not launched.** The virtual-rail joints are real MuJoCo joints, so `joint_state_broadcaster` publishes their ground-truth positions on `/joint_states` directly. `odometry_joint_state_publisher.py` is installed by `CMakeLists.txt` and documented here as the hardware pattern: on a real base the state estimate is converted into virtual-rail joint states this way rather than broadcast as TF (see "Transform Publisher Allocation"). ### Implementation **Location**: `src/hangar_sim/script/odometry_joint_state_publisher.py`