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 }} diff --git a/Dockerfile b/Dockerfile index 5355ec61a..0e07ca6dc 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 || 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 ENV USER_WS=${USER_WS} 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..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 @@ -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 Publisher Allocation"). + ### 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 @@ -804,11 +810,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 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 +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"], ) ``` @@ -821,15 +835,19 @@ mj_world (MuJoCo simulation root) │ ├─ [static] → map │ │ - │ └─ [static] → odom + │ └─ [static identity | slam_toolbox when slam:=True] → 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. +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 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. --- @@ -864,14 +882,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` | `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 | + +`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 +907,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** @@ -903,13 +929,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 @@ -927,7 +952,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 --- @@ -1027,9 +1052,9 @@ 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 | 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..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 @@ -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,22 @@ 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: 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 + # 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 +364,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 +376,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 +386,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),