Add fuse odometry localization for the hangar_sim mobile base

.github/workflows/batch-merge-release-branch.yaml

@@ -67,4 +67,4 @@ jobs:
             PR_BODY="⚠️Merge failed with conflicts! Please pull this branch and manually resolve the conflict by resetting this branch and re-merging."
           fi
           git push origin "$TEMP_BRANCH"
-          gh pr create --title "Merge $RELEASE_BRANCH into main" --body "$PR_BODY" --base main --head "$TEMP_BRANCH" --draft --reviewer infrastructure-devs
+          gh pr create --title "Merge $RELEASE_BRANCH into main" --body "$PR_BODY" --base main --head "$TEMP_BRANCH" --draft --reviewer JWhitleyWork

.gitmodules

@@ -17,9 +17,6 @@
 	path = src/external_dependencies/franka_config/franka_description
 	url = https://github.com/frankarobotics/franka_description.git
 	branch = main
-[submodule "src/moveit_pro_clipseg"]
-	path = src/moveit_pro_clipseg
-	url = https://github.com/PickNikRobotics/moveit_pro_clipseg.git
 [submodule "src/external_dependencies/phoebe_ws"]
 	path = src/external_dependencies/phoebe_ws
 	url = https://github.com/PickNikRobotics/phoebe_ws.git

CLAUDE.md

@@ -27,6 +27,28 @@ The two coupled numbers live in different files: the actuator `kv` is in the `<v
 
 Rule of thumb when changing a sim `timestep`: for every velocity actuator, check `armature/kv < timestep`. The symptom of violation is a joint that ignores commands (pinned), not one that oscillates.
 
+### Mecanum wheel radius: rolling radius != outer radius
+
+`hangar_sim`'s mecanum wheels have no cylindrical tread — each is a ring of spheres
+(`description/{front,rear}_{left,right}_wheel_link.xml`). Two different radii come out of
+that geometry and they are NOT interchangeable:
+
+- **Outer radius** `a + r` — ring radius plus sphere radius. This is the static ride height,
+  and is what the base's z-offset is derived from.
+- **Rolling radius** — perimeter actually covered per revolution over 2*pi. The contact point
+  rides the scalloped convex hull of the spheres, not the enclosing circle, so this is
+  strictly smaller: `(n * 2*a*sin(pi/n) + 2*pi*r) / (2*pi)` for n spheres.
+
+`kinematics.wheels_radius` in `config/control/picknik_ur.ros2_control.yaml` needs the
+**rolling** radius. Getting it wrong is doubly bad and easy to miss: the controller divides
+commanded body velocity by it (base drives too fast) while wheel odometry multiplies by it
+(distance under-reported), so the base overshoots *and* believes it undershot. A value ~12%
+low showed up as localization drift, which invites the wrong fix — inflating the AMCL
+motion-model alphas in `params/nav2_params.yaml` to spread the particle cloud over it.
+
+The value is geometry-dependent, so re-derive it whenever the roller count or ring radius
+changes; the derivation is recorded in a comment at the constant itself.
+
 ### MuJoCo documentation
 
 Refer to [docs.picknik.ai](https://docs.picknik.ai) for MuJoCo configuration guides:

src/dual_arm_sim/objectives/sort_blocks.xml

@@ -2,7 +2,7 @@
 <root BTCPP_format="4" main_tree_to_execute="Sort Blocks">
   <BehaviorTree
     ID="Sort Blocks"
-    _description="Clears colored blocks off the table by color, right arm for red and left for green. A CLIPSeg no-masks error is expected when a color runs out of blocks."
+    _description="Clears colored blocks off the table by color, right arm for red and left for green. An error is expected when a color runs out of blocks: segmentation returns no masks, and the downstream point cloud lookup then fails on the empty result."
     _favorite="true"
   >
     <Control ID="Sequence">

src/hangar_sim/CMakeLists.txt

@@ -3,6 +3,31 @@ project(hangar_sim)
 
 find_package(ament_cmake REQUIRED)
 find_package(picknik_accessories REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+add_executable(slip_aware_odom src/slip_aware_odom.cpp)
+target_include_directories(slip_aware_odom PRIVATE include)
+target_compile_features(slip_aware_odom PRIVATE cxx_std_20)
+ament_target_dependencies(slip_aware_odom rclcpp nav_msgs tf2 tf2_geometry_msgs)
+
+add_executable(amcl_odom_gate src/amcl_odom_gate.cpp src/amcl_odom_gate_logic.cpp)
+target_include_directories(amcl_odom_gate PRIVATE include)
+target_compile_features(amcl_odom_gate PRIVATE cxx_std_20)
+ament_target_dependencies(amcl_odom_gate rclcpp geometry_msgs tf2 tf2_geometry_msgs tf2_ros)
+
+add_executable(odom_world_drift src/odom_world_drift.cpp)
+target_include_directories(odom_world_drift PRIVATE include)
+target_compile_features(odom_world_drift PRIVATE cxx_std_20)
+ament_target_dependencies(odom_world_drift rclcpp nav_msgs sensor_msgs geometry_msgs tf2 tf2_geometry_msgs
+                          tf2_ros)
+
+install(TARGETS slip_aware_odom amcl_odom_gate odom_world_drift DESTINATION lib/${PROJECT_NAME})
 
 install(
   DIRECTORY
@@ -47,6 +72,22 @@ if(BUILD_TESTING)
           ENV MOVEIT_CONFIG_PACKAGE=hangar_sim
               MOVEIT_HOST_USER_WORKSPACE=${CMAKE_SOURCE_DIR}
               ROS_LOG_DIR=${CMAKE_CURRENT_BINARY_DIR}/test_results/${PROJECT_NAME}/ros_logs)
+
+  # Unit tests for the pure amcl_odom_gate decision logic (no ROS/TF).
+  find_package(ament_cmake_gmock REQUIRED)
+  ament_add_gmock(test_amcl_odom_gate test/test_amcl_odom_gate.cpp src/amcl_odom_gate_logic.cpp)
+  target_include_directories(test_amcl_odom_gate PRIVATE include)
+  target_compile_features(test_amcl_odom_gate PRIVATE cxx_std_20)
+
+  # Unit tests for the pure odom_world_drift rail-joint index resolver (no ROS).
+  ament_add_gmock(test_odom_world_drift test/test_odom_world_drift.cpp)
+  target_include_directories(test_odom_world_drift PRIVATE include)
+  target_compile_features(test_odom_world_drift PRIVATE cxx_std_20)
+
+  # Unit tests for the pure slip_aware_odom re-anchor/stitch and covariance-ramp logic (no ROS).
+  ament_add_gmock(test_slip_aware_odom test/test_slip_aware_odom.cpp)
+  target_include_directories(test_slip_aware_odom PRIVATE include)
+  target_compile_features(test_slip_aware_odom PRIVATE cxx_std_20)
 endif()
 
 ament_package()

src/hangar_sim/config/control/picknik_ur.ros2_control.yaml

@@ -46,7 +46,17 @@ platform_velocity_controller:
     rear_right_wheel_command_joint_name: "rear_right_wheel"
     rear_left_wheel_command_joint_name: "rear_left_wheel"
 
-    kinematics.wheels_radius: 0.0666
+    # Effective ROLLING radius of the mecanum wheels: the perimeter actually covered
+    # per revolution, divided by 2*pi. The tread is a ring of 20 spheres of r=0.010 whose
+    # centres sit at a=0.0659002081 (see description/*_wheel_link.xml). Rolling on that
+    # ring traces the convex hull of the spheres: 20 tangent segments of 2*a*sin(pi/20)
+    # plus arcs summing to one full circle of radius r, so
+    #   (20 * 2*a*sin(pi/20) + 2*pi*r) / (2*pi) = 0.07563.
+    # NOT the 0.0759 outer radius (a + r) — that is the static ride height and is what
+    # PR #882 derives the base height from; a wheel does not cover its outer circumference
+    # per turn because the contact point rides the scalloped hull, not the enclosing circle.
+    # Do not "correct" this back to 0.0759 or to the old 0.0666.
+    kinematics.wheels_radius: 0.0756
     kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.59
     wheel_separation_multiplier: 1.0
     wheel_radius_multiplier: 1.0
@@ -107,7 +117,17 @@ platform_velocity_controller_nav2:
     rear_right_wheel_command_joint_name: "rear_right_wheel"
     rear_left_wheel_command_joint_name: "rear_left_wheel"
 
-    kinematics.wheels_radius: 0.0666
+    # Effective ROLLING radius of the mecanum wheels: the perimeter actually covered
+    # per revolution, divided by 2*pi. The tread is a ring of 20 spheres of r=0.010 whose
+    # centres sit at a=0.0659002081 (see description/*_wheel_link.xml). Rolling on that
+    # ring traces the convex hull of the spheres: 20 tangent segments of 2*a*sin(pi/20)
+    # plus arcs summing to one full circle of radius r, so
+    #   (20 * 2*a*sin(pi/20) + 2*pi*r) / (2*pi) = 0.07563.
+    # NOT the 0.0759 outer radius (a + r) — that is the static ride height and is what
+    # PR #882 derives the base height from; a wheel does not cover its outer circumference
+    # per turn because the contact point rides the scalloped hull, not the enclosing circle.
+    # Do not "correct" this back to 0.0759 or to the old 0.0666.
+    kinematics.wheels_radius: 0.0756
     kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.59
     wheel_separation_multiplier: 1.0
     wheel_radius_multiplier: 1.0
@@ -160,6 +180,7 @@ platform_velocity_controller_nav2:
 joint_state_broadcaster:
   ros__parameters:
     use_local_topics: false
+    update_rate: 50  # publish /joint_states at 50 Hz, not the 600 Hz control loop
     joints:
       - shoulder_pan_joint
       - shoulder_lift_joint
@@ -211,19 +232,19 @@ joint_trajectory_controller:
     action_monitor_rate: 20.0
     allow_partial_joints_goal: true
     open_loop_control: false # this is closed loop
-    # Workaround for PickNikRobotics/moveit_pro#20766: trajectory generation
-    # leaves sub-epsilon terminal-velocity residuals that Jazzy's JTC rejects
-    # by default — measured ~1e-4 on `linear_x_joint` from the MTC Cartesian
-    # approach in "ML Move Boxes to Loading Zone", and ~4e-7 from joint
-    # interpolation in "Point-to-Point Trajectory" (previously skipped in
-    # test/objectives_integration_test.py for exactly this rejection).
+    # Workaround: trajectory generation leaves sub-epsilon terminal-velocity
+    # residuals that Jazzy's JTC rejects by default — measured ~1e-4 on
+    # `linear_x_joint` from the MTC Cartesian approach in "ML Move Boxes to
+    # Loading Zone", and ~4e-7 from joint interpolation in "Point-to-Point
+    # Trajectory" (previously skipped in test/objectives_integration_test.py
+    # for exactly this rejection).
     # CAUTION: this flag is binary and controller-wide — it disables the
     # endpoint-still-moving check for every magnitude on every joint,
     # including the mecanum base, where a genuinely nonzero terminal velocity
     # means the base is still translating at trajectory end and then snaps to
     # a position hold. Acceptable for this sim config; re-evaluate before
-    # copying to real hardware. Remove when moveit_pro#20766 clamps the
-    # residuals at trajectory generation.
+    # copying to real hardware. Remove once trajectory generation clamps the
+    # residuals upstream.
     allow_nonzero_velocity_at_trajectory_end: true
     gains:
       shoulder_pan_joint:
@@ -297,6 +318,7 @@ force_torque_sensor_broadcaster:
 
 imu_sensor_broadcaster:
   ros__parameters:
+    update_rate: 100  # 100 Hz IMU is plenty for fuse (optimizes at 10 Hz); was inheriting the 600 Hz control loop
     sensor_name: imu_site
     frame_id: ridgeback_base_link
     # Static covariance values (row-major 3x3 matrices)

src/hangar_sim/config/fuse/fuse.yaml

@@ -3,9 +3,9 @@
 state_estimator:
   ros__parameters:
     # Fixed-lag smoother configuration
-    optimization_frequency: 20.0
+    optimization_frequency: 10.0  # match publish_frequency (10 Hz); optimizing 2x faster than we publish was wasted work
     transaction_timeout: 0.01
-    lag_duration: 0.5
+    lag_duration: 0.3  # chosen: median 0.465deg (<0.5 goal), clean transient tail vs 0.25
 
     # Motion model for mobile base (3D omnidirectional)
     motion_models:
@@ -51,7 +51,7 @@ state_estimator:
     # only the relative change between consecutive messages, preventing
     # accumulated drift from corrupting the estimate.
     wheel_odom_sensor:
-      topic: /platform_velocity_controller_nav2/odom
+      topic: /odom_slip_aware  # slip_aware_odom republishes with spin-aware yaw covariance
       queue_size: 10
       pose_loss:
         type: fuse_loss::HuberLoss