Add fuse odometry localization for the hangar_sim mobile base

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

@@ -160,6 +160,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 +212,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 +298,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

src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro

@@ -31,11 +31,11 @@
           <param name="mujoco_model">${mujoco_model}</param>
           <param name="mujoco_model_package">hangar_sim</param>
           <param name="render_publish_rate">20</param>
-          <param name="tf_publish_rate">60</param>
+          <param name="tf_publish_rate">30</param>
           <param name="lidar_publish_rate">10</param>
           <param name="publish_odom">${publish_odom}</param>
           <param name="odom_child_frame">ridgeback_base_link</param>
-          <param name="odom_rate">150</param>
+          <param name="odom_rate">50</param>
           <!-- Zero z/roll/pitch in /odom for this planar base. The previous tag here,
                odom_zero_z, is not a recognized odom publisher parameter and was silently
                ignored; odom_planar is the real one. -->

src/hangar_sim/description/ur5e_ridgeback.xml

@@ -313,6 +313,10 @@
             conaffinity="0"
             group="2"
           />
+          <!-- Ground-truth base pose for localization testing and QA. MujocoSystem auto-publishes
+               every site as TF mj_world -> <name> at its true world pose, independent of
+               fuse/AMCL, so it can be compared against the estimate. Sites add no DOF. -->
+          <site name="base_gt" pos="0 0 0" size="0.01" rgba="1 0 0 1" />
           <!-- Front SICK TIM571: mounted at x=+0.45m (front bumper), z=0.15m.
                91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at -135 deg in robot frame.
                Quat rule: site Z axis = beam-0 firing direction. Site X axis must point in world -Z

src/hangar_sim/include/hangar_sim/amcl_odom_gate_logic.hpp

@@ -0,0 +1,186 @@
+// Copyright 2026 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the PickNik Inc. nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+// Pure decision logic for the degeneracy-aware map->odom gate. No ROS, no TF, so it
+// is unit-testable in isolation (see test/test_amcl_odom_gate.cpp). The node
+// (src/amcl_odom_gate.cpp) does the ROS I/O and calls detail::updateGate.
+//
+// A large innovation in AMCL's implied map->odom is neither hard-accepted nor
+// hard-rejected: it goes PROVISIONAL and is accepted only if it PERSISTS at the same
+// place for persist_time (a valid correction persists; an ambiguity teleport
+// thrashes). Small innovations track AMCL transparently. Particle spread is a
+// second, independent hold trigger (hysteresis) for the self-reported-lost case.
+
+#include <algorithm>
+#include <cmath>
+#include <numbers>
+#include <optional>
+#include <vector>
+
+#include "hangar_sim/se2.hpp"
+
+namespace amcl_odom_gate
+{
+// SE(2) primitives are shared with slip_aware_odom (se2.hpp); alias them in so the logic below and
+// the tests keep referring to Pose2/compose/invert unqualified.
+using hangar_sim::se2::compose;
+using hangar_sim::se2::invert;
+using hangar_sim::se2::Pose2;
+// Fraction alpha of the way from a to b, wrapping the yaw through the short arc.
+[[nodiscard]] inline Pose2 blend(const Pose2& a, const Pose2& b, double alpha)
+{
+  const double dyaw = std::remainder(b.yaw - a.yaw, 2.0 * std::numbers::pi);
+  return { a.x + alpha * (b.x - a.x), a.y + alpha * (b.y - a.y), a.yaw + alpha * dyaw };
+}
+[[nodiscard]] inline double planarDist(const Pose2& a, const Pose2& b)
+{
+  return std::hypot(a.x - b.x, a.y - b.y);
+}
+// Absolute yaw difference through the short arc [0, pi].
+[[nodiscard]] inline double yawDist(const Pose2& a, const Pose2& b)
+{
+  return std::abs(std::remainder(a.yaw - b.yaw, 2.0 * std::numbers::pi));
+}
+
+// A timestamped odom->base sample, used to reconstruct where odom->base was at a past
+// instant so a latency-lagged AMCL correction can be composed at the CONTENT time, not now.
+struct OdomSample
+{
+  double t;    // seconds
+  Pose2 pose;  // odom->base at t
+};
+
+// Append `s` to a time-ascending odom buffer, then drop samples older than keep_window_sec behind
+// the newest. Ignores a sample that is not strictly newer than the current back, so the buffer
+// stays strictly ascending (which keeps interpolateOdom's span positive). If a sample arrives more
+// than one window OLDER than the back, the clock jumped backward (sim reset / bag loop) -- the
+// strictly-newer rule would otherwise freeze the buffer stale forever, so flush and re-seed from it.
+// Pure -- unit-tested.
+inline void appendOdomSample(std::vector<OdomSample>& buf, const OdomSample& s, double keep_window_sec)
+{
+  if (!buf.empty())
+  {
+    if (s.t < buf.back().t - keep_window_sec)
+    {
+      buf.clear();  // backward time jump: discard the now-unreachable pre-jump samples and re-seed.
+    }
+    else if (s.t <= buf.back().t)
+    {
+      return;  // not strictly newer (duplicate / near-stale replay): keep the buffer ascending.
+    }
+  }
+  buf.push_back(s);
+  const double cutoff = s.t - keep_window_sec;
+  while (buf.size() > 1 && buf.front().t < cutoff)
+  {
+    buf.erase(buf.begin());
+  }
+}
+
+// Linear interpolation of odom->base at `target` [s] from a time-ordered (ascending t) buffer.
+// Clamps to the nearest end if `target` is outside [front, back] (e.g. before the buffer fills).
+// nullopt only if the buffer is empty. Pure -- unit-tested in isolation.
+[[nodiscard]] inline std::optional<Pose2> interpolateOdom(const std::vector<OdomSample>& buf, double target)
+{
+  if (buf.empty())
+  {
+    return std::nullopt;
+  }
+  if (target <= buf.front().t)
+  {
+    return buf.front().pose;
+  }
+  if (target >= buf.back().t)
+  {
+    return buf.back().pose;
+  }
+  for (size_t i = 1; i < buf.size(); ++i)
+  {
+    if (buf[i].t >= target)
+    {
+      const double span = buf[i].t - buf[i - 1].t;
+      // span is always positive here (the front/back clamps above mean buf[i-1].t < target <=
+      // buf[i].t at this point), so the guard is defensive against a hand-built zero-span buffer,
+      // not something reachable through appendOdomSample (which keeps stamps strictly ascending).
+      const double alpha = (span > 1e-9) ? (target - buf[i - 1].t) / span : 0.0;
+      return blend(buf[i - 1].pose, buf[i].pose, alpha);
+    }
+  }
+  return buf.back().pose;  // unreachable given the guards above, but keeps the compiler happy.
+}
+
+// Tunables for the gate decision.
+struct GateParams
+{
+  double spread_hold = 1.5;               // spread [m] above which AMCL is held as untrustworthy
+  double spread_resume = 0.6;             // resume below this (< spread_hold): hysteresis, no flicker
+  double jump_hold = 0.5;                 // position innovation [m] above which a candidate goes provisional
+  double jump_hold_yaw = 0.35;            // yaw innovation [rad, ~20 deg] above which a candidate goes provisional
+  double provisional_tol = 0.3;           // position drift [m] within which successive candidates are the same target
+  double provisional_tol_yaw = 0.2;       // yaw drift [rad] within which successive candidates are the same target
+  double persist_time = 1.0;              // a provisional target must persist this long [s] to be accepted
+  double spread_accept_max = 3.0;         // spread [m] above which a persisted correction is NEVER accepted
+                                          // (keep above spread_hold; default is 2x it). Persistence distinguishes
+                                          // a real correction from a thrashing ambiguity, but a confident-WRONG
+                                          // lock (e.g. plane scan ambiguity) also persists. A legitimate
+                                          // correction converges so its spread drops below this; a divergence
+                                          // stays wide, so the gate keeps coasting on odom instead of adopting it.
+                                          // Guards POSITIONAL trust only -- heading multimodality is caught by
+                                          // jump_hold_yaw + persistence, not by (positional) spread.
+  double latency_compensation_sec = 0.0;  // AMCL estimate lag [s]. beluga's cloud content trails the
+                                          // true pose by ~this long (its stamp under-reports the lag), so
+                                          // during motion the correction is stale by speed*lag. Compose the
+                                          // mean with odom->base from (cloud stamp - this), not the latest, so
+                                          // map->odom is consistent at the CONTENT time and the live odom
+                                          // carries it to now. 0 disables (compose at latest); tune to the
+                                          // measured lag (relative to the cloud stamp).
+  double alpha_slew = 0.05;               // max change in follow fraction per update (smooth ramp)
+};
+
+namespace detail
+{
+// Mutable state carried between updates.
+struct GateState
+{
+  Pose2 held{ 0.0, 0.0, 0.0 };  // current broadcast map->odom
+  double alpha = 1.0;           // follow fraction: 1 transparent, 0 held
+  bool have_held = false;
+  bool spread_holding = false;         // spread hysteresis latch
+  Pose2 provisional{ 0.0, 0.0, 0.0 };  // candidate being watched for persistence
+  double provisional_since = 0.0;      // time the current provisional target began persisting [s]
+  bool have_provisional = false;
+};
+
+// Pure decision. Given AMCL's implied candidate map->odom, the particle spread, the
+// current time, params, and mutable state, update and return the map->odom to
+// broadcast. No ROS, no TF -- unit-tested in test/test_amcl_odom_gate.cpp.
+[[nodiscard]] Pose2 updateGate(const Pose2& candidate, double spread, double now, const GateParams& p, GateState& s);
+}  // namespace detail
+}  // namespace amcl_odom_gate