diff --git a/.github/workflows/batch-merge-release-branch.yaml b/.github/workflows/batch-merge-release-branch.yaml index 8854536c2..4882746c9 100644 --- a/.github/workflows/batch-merge-release-branch.yaml +++ b/.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 diff --git a/.gitmodules b/.gitmodules index 6c03417d0..308646393 100644 --- a/.gitmodules +++ b/.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 diff --git a/CLAUDE.md b/CLAUDE.md index dc3de155c..86075d7c8 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -27,6 +27,28 @@ The two coupled numbers live in different files: the actuator `kv` is in the ` diff --git a/src/external_dependencies/phoebe_ws b/src/external_dependencies/phoebe_ws index f5014310f..22a1c0048 160000 --- a/src/external_dependencies/phoebe_ws +++ b/src/external_dependencies/phoebe_ws @@ -1 +1 @@ -Subproject commit f5014310f7f7975b5f1949a95d18e4be836f9c03 +Subproject commit 22a1c004895e1ca35bbcc287ddeccdc41d486056 diff --git a/src/hangar_sim/CMakeLists.txt b/src/hangar_sim/CMakeLists.txt index 0d49770e5..61482d938 100644 --- a/src/hangar_sim/CMakeLists.txt +++ b/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() diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml index ab6e0afb3..d88e3ea56 100644 --- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml +++ b/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) diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml index 4ad1039f5..22f5135b6 100644 --- a/src/hangar_sim/config/fuse/fuse.yaml +++ b/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 diff --git a/src/hangar_sim/description/front_left_wheel_link.xml b/src/hangar_sim/description/front_left_wheel_link.xml index 9d16b089f..e4ecf5389 100644 --- a/src/hangar_sim/description/front_left_wheel_link.xml +++ b/src/hangar_sim/description/front_left_wheel_link.xml @@ -13,7 +13,7 @@ armature="1.0" /> - + - + - + - + - + - + - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/hangar_sim/description/front_right_wheel_link.xml b/src/hangar_sim/description/front_right_wheel_link.xml index a99d647c2..159da066a 100644 --- a/src/hangar_sim/description/front_right_wheel_link.xml +++ b/src/hangar_sim/description/front_right_wheel_link.xml @@ -13,7 +13,7 @@ armature="1.0" /> - + - + - + - + - + - + - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml index 405962ca5..86ab2289c 100644 --- a/src/hangar_sim/description/hangar_scene.xml +++ b/src/hangar_sim/description/hangar_scene.xml @@ -12,6 +12,11 @@ objectives. 0.008 keeps the runner at-or-under realtime while staying finer than the original 0.025. Wheel velocity actuators still hold: armature/kv = 1.0/500 = 0.002 s < 0.008 s (see CLAUDE.md). --> + @@ -428,7 +428,6 @@ /> @@ -44,13 +46,15 @@ ID="Interpolate to Joint State" _collapsed="false" target_joint_state="{target_joint_state}" - controller_names="joint_trajectory_controller" - controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" - execution_pipeline="jtc" + controller_names="{joint_trajectory_controller_name}" + controller_action_server="{controller_action_server}" + planning_group_name="{default_planning_group}" + velocity_scale_factor="{velocity_scale_factor}" + execution_pipeline="{execution_pipeline}" /> @@ -58,7 +62,7 @@ @@ -94,17 +98,23 @@ @@ -112,7 +122,7 @@ @@ -136,17 +146,16 @@ _collapsed="false" target_joint_state="{target_joint_state}" acceleration_scale_factor="1.0" - controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" + velocity_scale_factor="{velocity_scale_factor}" + controller_action_server="{controller_action_server}" controller_names="joint_trajectory_controller;platform_velocity_controller" - joint_group_name="manipulator" - link_padding="0.0" - velocity_scale_factor="1.0" - seed="0" - execution_pipeline="jtc" + joint_group_name="{default_planning_group}" + link_padding="{link_padding}" + execution_pipeline="{execution_pipeline}" /> @@ -154,7 +163,7 @@ @@ -163,45 +172,55 @@ - + - + + - + - - + - + - @@ -230,13 +248,59 @@ - - - + + + Enable interactive user prompts during teleoperation mode selection + + + Maximum number of IK solutions PlanToPose will try to produce for a + given pose + + + Maximum number of MTC solutions to find before returning early (0 to + find all solutions) + + + + + + + + Fraction of each joint's maximum velocity that the motion branches plan + with. DoTeleoperateAction overwrites it with the speed the operator + selects; this default only covers the window before its first feedback, + and matches the speed the UI itself starts at. + + + + + Whether trajectory previews from Interactive Marker teleop must be + approved before execution. Set to false to skip the approval prompt. + diff --git a/src/hangar_sim/package.xml b/src/hangar_sim/package.xml index e701dee7c..a1e0411e5 100644 --- a/src/hangar_sim/package.xml +++ b/src/hangar_sim/package.xml @@ -14,6 +14,13 @@ ament_cmake pluginlib + rclcpp + nav_msgs + geometry_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros admittance_controller clearpath_mecanum_drive_controller @@ -34,7 +41,6 @@ realsense2_description ridgeback_description robotiq_description - sensor_msgs slam_toolbox ur_description velocity_force_controller @@ -47,6 +53,7 @@ ament_clang_format ament_clang_tidy ament_cmake_copyright + ament_cmake_gmock ament_cmake_lint_cmake picknik_ament_copyright ament_flake8 diff --git a/src/hangar_sim/params/nav2_params.yaml b/src/hangar_sim/params/nav2_params.yaml index 2ccfc4ee2..2dd40ae47 100644 --- a/src/hangar_sim/params/nav2_params.yaml +++ b/src/hangar_sim/params/nav2_params.yaml @@ -5,8 +5,9 @@ amcl: # activation, so localization starts without a manual 2D pose estimate. # beluga_amcl is 2D: only x, y, yaw are declared (no z). The covariances set # the initial spread (~0.5 m position, ~15 deg yaw) so the seed is not - # overconfident; beluga's default covariance is near-zero. The SetInitialPose - # Behavior re-seeds at the robot's current pose at each navigation Objective. + # overconfident; beluga's default covariance is near-zero. The navigation + # Objectives no longer re-seed per run: slip_aware_odom keeps odom continuous + # across controller switches and amcl_odom_gate rides odom through divergences. set_initial_pose: true initial_pose: x: 0.0 @@ -15,6 +16,10 @@ amcl: covariance_x: 0.25 covariance_y: 0.25 covariance_yaw: 0.0685 + # Back at their pre-drift values, matching main. These had been raised ~2x to cover + # odom that under-reported distance by ~11.9%; that came from the wheel rolling radius + # (kinematics.wheels_radius), now corrected, so the compensation comes back off. + # Re-check these once the base has been driven at its real, corrected speed. alpha1: 0.05 alpha2: 0.05 alpha3: 0.1 @@ -31,25 +36,33 @@ amcl: laser_max_range: 25.0 laser_min_range: 0.0 laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 5000 + max_beams: 720 # full merged scan (0.5deg) -- max hits + max_particles: 2000 # E1: needed for box-cluster convergence min_particles: 1000 odom_frame_id: "odom" pf_err: 0.05 pf_z: 0.99 recovery_alpha_fast: 0.1 recovery_alpha_slow: 0.001 - resample_interval: 1 + resample_interval: 3 # resample less often than we correct: avoids particle depletion that diverged at update_min_a=0.05/0.1 robot_model_type: "nav2_amcl::OmniMotionModel" save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true + sigma_hit: 0.25 # E1 keeper: relaxed peak, boxes as outliers + tf_broadcast: false # AMCL never broadcasts map->odom: the amcl_odom_gate publishes it when + # use_fuse+localization are on, else a static map->odom fallback does (see the launch file). + # Coupled to use_fuse: the gate only launches with use_fuse:=true, so use_fuse:=false + + # localization:=true is unsupported (AMCL's correction is discarded) -- the launch file logs a + # loud warning for that combination. transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 + update_min_a: 0.1 # correct often for box robustness + update_min_d: 0.1 + # likelihood_field uses only z_hit + z_rand. Their RATIO (0.65:0.30) is the tuned quantity; + # the 0.95 sum is deliberate -- a constant scale on every beam's likelihood that cancels in the + # particle-weight normalization, so summing to 1.0 would be cosmetic while re-weighting one term + # would change the tuned mix. z_short/z_max are ignored in this model. + z_hit: 0.65 z_max: 0.05 - z_rand: 0.5 + z_rand: 0.3 z_short: 0.05 scan_topic: /scan_merged @@ -120,7 +133,7 @@ controller_server: vx_max: 0.5 vx_min: -0.35 vy_max: 0.5 - wz_max: 1.9 + wz_max: 0.6 # must stay below velocity_smoother max_velocity[2] (1.0) to bind; slower turns keep the base within AMCL's correction bandwidth so map->odom tracks instead of lagging iteration_count: 1 prune_distance: 1.7 transform_tolerance: 0.1 @@ -404,7 +417,7 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [1.0, 1.0, 1.0] + max_velocity: [1.0, 1.0, 1.0] # [2] (yaw) caps controller wz_max above; keep >= wz_max (0.6) or the gate's turn-rate assumption drifts min_velocity: [-1.0, -1.0, -1.0] max_accel: [2.5, 2.5, 3.2] max_decel: [-2.5, -2.5, -3.2] diff --git a/src/hangar_sim/script/odom_qos_relay.py b/src/hangar_sim/script/odom_qos_relay.py index b82fa5704..3d2a867c0 100755 --- a/src/hangar_sim/script/odom_qos_relay.py +++ b/src/hangar_sim/script/odom_qos_relay.py @@ -34,7 +34,6 @@ from rclpy.node import Node from geometry_msgs.msg import Twist -from nav_msgs.msg import Odometry from sensor_msgs.msg import Imu from rclpy.qos import ( @@ -67,12 +66,6 @@ def __init__(self): depth=10, ) - # Odometry relay - self.odom_sub = self.create_subscription( - Odometry, "/odom", self.odom_callback, qos_sub - ) - self.odom_pub = self.create_publisher(Odometry, "/odom_reliable", qos_pub) - # IMU relay self.imu_sub = self.create_subscription( Imu, "/imu_sensor_broadcaster/imu", self.imu_callback, qos_sub @@ -89,9 +82,6 @@ def __init__(self): Twist, "/platform_velocity_controller_nav2/cmd_vel_unstamped", 10 ) - def odom_callback(self, msg): - self.odom_pub.publish(msg) - def imu_callback(self, msg): self.imu_pub.publish(msg) diff --git a/src/hangar_sim/src/amcl_odom_gate.cpp b/src/hangar_sim/src/amcl_odom_gate.cpp new file mode 100644 index 000000000..810a22041 --- /dev/null +++ b/src/hangar_sim/src/amcl_odom_gate.cpp @@ -0,0 +1,373 @@ +// 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. + +// Degeneracy-aware map->odom gate (ROS node). +// +// AMCL matches the lidar to a static map. Where the scene is degenerate for that +// match -- pressed against the large smooth fuselage (slide-along ambiguity), or +// among unmapped boxes -- the estimate lurches/teleports and the map flips. +// odom->base (fuse's wheel+IMU dead-reckoning) stays locally accurate through those +// zones, so this node holds the last good map->odom and coasts on odom until AMCL is +// trustworthy again, then blends smoothly back. It replaces AMCL's own broadcast: +// set AMCL's tf_broadcast:=false so this node is the sole map->odom publisher. +// +// This file is only the ROS I/O: read /particle_cloud, distil it to (mean, spread), +// look up odom->base, and hand the implied map->odom to the pure decision function +// detail::updateGate (amcl_odom_gate_logic.hpp), which is unit-tested in isolation. +// +// ASSUMPTION (documented, load-bearing): odom->base is trustworthy for the DURATION +// of a degenerate zone. This holds because the zones are transient (a few seconds +// passing the fuselage or boxes) and fuse's drift over that span is small. If the +// robot were held in a degenerate zone long enough for odom to drift materially, or +// odometry failed grossly (severe wheel slip) exactly there, the gate would coast on +// bad data and degrade to "as good as odometry" -- the design's outer limit. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hangar_sim/amcl_odom_gate_logic.hpp" + +namespace +{ +constexpr double kPubPeriod = 0.033; // 30 Hz map->odom broadcast +constexpr double kTransformTolerance = 0.1; // future-date the transform so TF can extrapolate [s] +constexpr double kCoastAlpha = 0.5; // follow-fraction below which the gate counts as coasting +constexpr double kCoastWarnSeconds = 5.0; // warn once a continuous coast exceeds this [s] +constexpr int64_t kCoastWarnThrottleMs = 2000; // min interval between coast warnings [ms] +constexpr double kStaleInputSeconds = 3.0; // publish() warns if onCloud has not updated in this long: + // beluga publishes /particle_cloud faster than this even at + // rest, so a longer gap means input starvation (beluga/odom + // TF gone) and the node is republishing a frozen map->odom. +constexpr int64_t kStaleWarnThrottleMs = 2000; // min interval between stale-input warnings [ms] +constexpr double kOdomBufferSec = 1.0; // odom->base history kept for latency compensation [s]. + // Usable compensation ceiling is this minus the newest + // sample's age (the odom TF lag), not the full window. +constexpr double kOdomBufferStaleSec = 0.5; // treat the odom buffer as unavailable once its newest + // sample is older than this: appendOdomSample never empties + // the buffer on a running TF failure (it only clears on a + // backward clock jump), so without this age check a + // persistently-failing odom->base lookup would silently + // clamp interpolateOdom to an ever-staler sample forever + // instead of falling through to the retry/skip path below. +} // namespace + +using amcl_odom_gate::Pose2; + +class AmclOdomGate : public rclcpp::Node +{ +public: + AmclOdomGate() : Node("amcl_odom_gate"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), tf_broadcaster_(*this) + { + params_.spread_hold = declare_parameter("spread_hold", params_.spread_hold); + params_.spread_resume = declare_parameter("spread_resume", params_.spread_resume); + params_.jump_hold = declare_parameter("jump_hold", params_.jump_hold); + params_.jump_hold_yaw = declare_parameter("jump_hold_yaw", params_.jump_hold_yaw); + params_.provisional_tol = declare_parameter("provisional_tol", params_.provisional_tol); + params_.provisional_tol_yaw = declare_parameter("provisional_tol_yaw", params_.provisional_tol_yaw); + params_.persist_time = declare_parameter("persist_time", params_.persist_time); + params_.spread_accept_max = declare_parameter("spread_accept_max", params_.spread_accept_max); + params_.latency_compensation_sec = declare_parameter("latency_compensation_sec", params_.latency_compensation_sec); + params_.alpha_slew = declare_parameter("alpha_slew", params_.alpha_slew); + if (params_.latency_compensation_sec >= kOdomBufferSec) + { + // interpolateOdom would clamp to the oldest sample, silently degrading compensation. The two + // constants live apart (a parameter vs. a hardcoded window), so warn rather than fail silently. + RCLCPP_WARN(get_logger(), + "latency_compensation_sec (%.2f s) >= odom buffer window (%.2f s); compensation will " + "clamp to the oldest sample. Reduce it or enlarge kOdomBufferSec.", + params_.latency_compensation_sec, kOdomBufferSec); + } + // alpha_slew <= 0 freezes the gate: once s.alpha snaps to 0 on a hold, min(s.alpha + alpha_slew, 1) + // can never climb back, so the node would broadcast a frozen map->odom for the rest of the run. + // Fall back to the default rather than run permanently stuck. + if (params_.alpha_slew <= 0.0) + { + const double default_alpha_slew = amcl_odom_gate::GateParams{}.alpha_slew; + RCLCPP_WARN(get_logger(), + "alpha_slew (%.3f) must be > 0; the gate could never resume after a hold. Using the default (%.3f).", + params_.alpha_slew, default_alpha_slew); + params_.alpha_slew = default_alpha_slew; + } + // spread_resume must sit below spread_hold or the hysteresis loses its dead band and the hold latch + // can flicker every update. + if (params_.spread_resume >= params_.spread_hold) + { + RCLCPP_WARN(get_logger(), + "spread_resume (%.2f) >= spread_hold (%.2f); the hold hysteresis has no dead band and may flicker.", + params_.spread_resume, params_.spread_hold); + } + // Negative persist_time makes the persistence check vacuous (now - provisional_since >= negative is + // always true), accepting every large innovation at once -- the teleport guard is effectively off. + if (params_.persist_time < 0.0) + { + RCLCPP_WARN(get_logger(), "persist_time (%.2f s) is negative; the teleport-persistence guard is disabled.", + params_.persist_time); + } + // beluga publishes /particle_cloud BEST_EFFORT; match it or we receive nothing. + cloud_sub_ = create_subscription( + "/particle_cloud", rclcpp::SensorDataQoS(), + [this](geometry_msgs::msg::PoseArray::ConstSharedPtr m) { onCloud(*m); }); + // Node-clock timer (not wall): the node runs use_sim_time, and publish()'s coast/stale windows + // are measured in now() (sim time), so the tick must advance on sim time too or the two drift + // apart when the sim is not real-time. + timer_ = + rclcpp::create_timer(this, get_clock(), rclcpp::Duration::from_seconds(kPubPeriod), [this]() { publish(); }); + } + +private: + void onCloud(const geometry_msgs::msg::PoseArray& cloud) + { + const size_t n = cloud.poses.size(); + if (n < 2) + { + return; + } + // Refresh the odom buffer here too (not only in the publish timer): onCloud fires asynchronously + // to that 30 Hz timer, so without this the newest buffered sample could be up to a publish period + // stale relative to now, and interpolateOdom at (now - 0) would clamp to a stale pose -- making + // even latency_compensation_sec = 0 differ from a fresh latest lookup. Sampling now keeps the + // buffer as fresh as this callback and preserves the "0 == old compose-at-latest" invariant. + sampleOdomForBuffer(); + // Circular mean (AMCL's estimate) and RMS spread (its confidence) over the cloud. + double sx = 0.0, sy = 0.0, ss = 0.0, sc = 0.0; + for (const auto& p : cloud.poses) + { + sx += p.position.x; + sy += p.position.y; + const double y = tf2::getYaw(p.orientation); + ss += std::sin(y); + sc += std::cos(y); + } + const Pose2 mean{ sx / n, sy / n, std::atan2(ss, sc) }; + double var = 0.0; + for (const auto& p : cloud.poses) + { + const double dx = p.position.x - mean.x, dy = p.position.y - mean.y; + var += dx * dx + dy * dy; + } + const double spread = std::sqrt(var / n); + + // odom->base is fuse's estimate. Compose the AMCL mean with odom->base from the cloud's own + // content time (cloud stamp - latency_compensation_sec), NOT the latest: beluga's cloud CONTENT + // trails the true pose by ~that lag during motion (its stamp under-reports it -- a lookup at the + // cloud stamp itself fails as future extrapolation), so composing with the latest odom leaves + // map->odom stale by speed*lag. Referencing the cloud's OWN header stamp (not now()) keeps the + // compensation immune to onCloud transport/scheduling jitter, so the parameter measures one + // physical thing. The live odom->base then carries the consistent map->odom forward to now. + // odom_buf_ is filled at the publish rate and at the top of this callback; interpolateOdom clamps + // if the target predates the buffer (startup) and returns nullopt only if it is empty, in which + // case fall back to the latest TF lookup. lag = 0 reproduces compose-at-latest. + // INVARIANT: mean (from beluga's /particle_cloud, in ITS base_frame) and this odom->base lookup + // must refer to the SAME base link (ridgeback_base_link); a frame mismatch would bias every + // candidate by a constant offset that persistence would then accept as a real correction. + const double now_sec = this->now().seconds(); + const double odom_target = rclcpp::Time(cloud.header.stamp).seconds() - params_.latency_compensation_sec; + // A non-empty buffer whose newest sample is stale means the odom->base TF lookup has been + // failing (see kOdomBufferStaleSec above), not that a fresh sample is simply pending -- treat + // it the same as an empty buffer and fall through to the retry/skip path instead of silently + // clamping to an arbitrarily old pose. + const bool odom_buf_fresh = !odom_buf_.empty() && (now_sec - odom_buf_.back().t) <= kOdomBufferStaleSec; + Pose2 odom_base{ 0.0, 0.0, 0.0 }; + if (const auto interp = odom_buf_fresh ? amcl_odom_gate::interpolateOdom(odom_buf_, odom_target) : std::nullopt; + interp.has_value()) + { + odom_base = interp.value(); + if (params_.latency_compensation_sec > 0.0 && !odom_buf_.empty() && odom_target > odom_buf_.back().t) + { + // Compensation was requested but the target landed AFTER the newest odom sample, so + // interpolateOdom clamped to it -- i.e. the odom->base TF latency exceeds + // latency_compensation_sec and the feature is silently under-compensating. Surface it. + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kStaleWarnThrottleMs, + "amcl_odom_gate: latency-compensation target is newer than the odom buffer " + "(odom TF lag exceeds latency_compensation_sec=%.2f s); clamped, under-compensating.", + params_.latency_compensation_sec); + } + } + else + { + // Reached when the buffer is empty (narrow startup window) or its newest sample is stale + // (odom->base TF lookup has been failing -- see kOdomBufferStaleSec above). Distinguish the + // two: an empty buffer at startup is expected and not worth warning about, but a non-empty, + // stale buffer means the TF lookup was working and then stopped -- surface that specifically + // rather than relying on the generic starvation watchdog in publish() to eventually notice. + if (!odom_buf_.empty()) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kStaleWarnThrottleMs, + "amcl_odom_gate: odom->base buffer is stale (newest sample is %.2fs old, > " + "kOdomBufferStaleSec=%.2fs) -- the odom->ridgeback_base_link TF lookup has " + "stopped succeeding.", + now_sec - odom_buf_.back().t, kOdomBufferStaleSec); + } + // Retry once here; if it still fails, skip this cloud. + try + { + const auto t = tf_buffer_.lookupTransform("odom", "ridgeback_base_link", tf2::TimePointZero); + odom_base = { t.transform.translation.x, t.transform.translation.y, tf2::getYaw(t.transform.rotation) }; + } + catch (const tf2::TransformException&) + { + return; // odom->base not available yet: skip this cloud (timer keeps rebroadcasting last good). + } + } + + // AMCL's implied map->odom = (map->base) o inv(odom->base); gate decides what to broadcast. + // The persist_time clock uses now() (wall/sim), not the cloud stamp; under steady AMCL + // latency the constant offset leaves elapsed-duration measurement correct. + const Pose2 candidate = amcl_odom_gate::compose(mean, amcl_odom_gate::invert(odom_base)); + map_odom_ = amcl_odom_gate::detail::updateGate(candidate, spread, now_sec, params_, state_); + have_map_odom_ = true; + + // Watchdog: the gate is the SOLE map->odom source, so a prolonged hold (coasting on odom + // through a severe-spread wrong lock) silently degrades localization to unbounded dead + // reckoning. Surface a long coast so it does not quietly move the base's map pose out from + // under the nav stack -- the state_.alpha follow-fraction is near 0 whenever the gate holds. + if (state_.alpha < kCoastAlpha) + { + const auto t_now = now(); // one read: keep the logged elapsed consistent with the check + if (!coasting_) + { + coasting_ = true; + coast_start_ = t_now; + } + else if (const double coast_s = (t_now - coast_start_).seconds(); coast_s > kCoastWarnSeconds) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kCoastWarnThrottleMs, + "amcl_odom_gate coasting on odom for %.1f s (AMCL untrusted, spread=%.2f); " + "map->odom is dead-reckoning and will drift until AMCL re-locks.", + coast_s, spread); + } + } + else + { + coasting_ = false; + } + last_update_ = now(); // liveness: publish() warns if onCloud stops updating (input starvation). + } + + // Sample odom->base at the latest available TF and append to the ring buffer keyed by the TF's + // OWN stamp (which trails now by the odom publish latency, so labelling by now would mis-time it), + // trimming samples older than kOdomBufferSec. Runs at the publish rate; feeds interpolateOdom() in + // onCloud so a latency-lagged AMCL correction can be composed at its content time (now - lag). + void sampleOdomForBuffer() + { + try + { + const auto tf = tf_buffer_.lookupTransform("odom", "ridgeback_base_link", tf2::TimePointZero); + amcl_odom_gate::appendOdomSample( + odom_buf_, + { rclcpp::Time(tf.header.stamp).seconds(), + { tf.transform.translation.x, tf.transform.translation.y, tf2::getYaw(tf.transform.rotation) } }, + kOdomBufferSec); + } + catch (const tf2::TransformException&) + { + // odom->base not ready yet; leave the buffer as-is. + } + } + + void publish() + { + sampleOdomForBuffer(); // keep the latency-compensation buffer filled at the publish rate + + if (!have_map_odom_) + { + // No seed yet: /particle_cloud hasn't arrived or the odom->base lookup keeps failing, so this + // sole map->odom publisher is emitting nothing. The starvation watchdog below sits after this + // return and can't see a never-started gate, so surface it here (throttled). + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kStaleWarnThrottleMs, + "amcl_odom_gate: no map->odom yet -- waiting for /particle_cloud and the " + "odom->ridgeback_base_link TF; nothing is being broadcast."); + return; + } + // Liveness watchdog: this timer keeps firing regardless of input, so it is the only place + // that can see onCloud STARVATION -- if beluga stops publishing /particle_cloud or odom->base + // disappears, updateGate never runs, state_.alpha freezes, and the coast watchdog in onCloud + // is never reached, yet we keep broadcasting a frozen (future-dated) map->odom. Surface that + // here so the worst silent failure for a sole broadcaster does not pass unnoticed. + if (const double stale_s = (now() - last_update_).seconds(); stale_s > kStaleInputSeconds) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kStaleWarnThrottleMs, + "amcl_odom_gate: no /particle_cloud update for %.1f s -- broadcasting a frozen " + "map->odom. Check that beluga_amcl and the odom->base TF are alive.", + stale_s); + } + geometry_msgs::msg::TransformStamped t; + t.header.stamp = now() + rclcpp::Duration::from_seconds(kTransformTolerance); + t.header.frame_id = "map"; + t.child_frame_id = "odom"; + t.transform.translation.x = map_odom_.x; + t.transform.translation.y = map_odom_.y; + t.transform.rotation.z = std::sin(map_odom_.yaw / 2.0); + t.transform.rotation.w = std::cos(map_odom_.yaw / 2.0); + tf_broadcaster_.sendTransform(t); + } + + // onCloud (subscription) writes state_/map_odom_/coasting_/coast_start_/last_update_ and reads + // odom_buf_; publish (timer) reads map_odom_/last_update_ and writes odom_buf_. main() spins on the + // default SingleThreadedExecutor, so this node's two callbacks never run concurrently and the + // access is safe without locking. (tf2_ros::TransformListener spins its own thread that writes + // tf_buffer_, but that is internally synchronized and touches none of these members.) If this node + // is ever moved to a MultiThreadedExecutor or separate callback groups, guard the shared state + // (state_/map_odom_/last_update_/odom_buf_). + amcl_odom_gate::GateParams params_; + amcl_odom_gate::detail::GateState state_; + Pose2 map_odom_{ 0.0, 0.0, 0.0 }; + bool have_map_odom_ = false; + bool coasting_ = false; // watchdog: is the gate currently holding (alpha < kCoastAlpha)? + // coast_start_/last_update_ are default-constructed RCL_SYSTEM_TIME; now() is RCL_ROS_TIME under + // use_sim_time and subtracting mismatched clock types throws. Both are only subtracted AFTER being + // assigned now() on a prior tick (coast_start_ behind coasting_, last_update_ behind have_map_odom_), + // so the clock types always match at the subtraction sites -- keep that ordering if refactoring. + rclcpp::Time coast_start_; // when the current continuous coast began (valid only while coasting_) + rclcpp::Time last_update_; // time of the last successful onCloud update (valid once have_map_odom_) + std::vector odom_buf_; // recent odom->base samples for latency compensation + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + tf2_ros::TransformBroadcaster tf_broadcaster_; + rclcpp::Subscription::SharedPtr cloud_sub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/hangar_sim/src/amcl_odom_gate_logic.cpp b/src/hangar_sim/src/amcl_odom_gate_logic.cpp new file mode 100644 index 000000000..8d584efab --- /dev/null +++ b/src/hangar_sim/src/amcl_odom_gate_logic.cpp @@ -0,0 +1,113 @@ +// 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. + +#include "hangar_sim/amcl_odom_gate_logic.hpp" + +#include + +namespace amcl_odom_gate::detail +{ +Pose2 updateGate(const Pose2& candidate, double spread, double now, const GateParams& p, GateState& s) +{ + if (!s.have_held) + { + s.held = candidate; + s.have_held = true; + s.alpha = 1.0; + return s.held; + } + + // Trigger 1: spread hysteresis (filter self-reported uncertainty). + if (spread > p.spread_hold) + { + s.spread_holding = true; + } + else if (spread < p.spread_resume) + { + s.spread_holding = false; + } + + // Innovation is gated on BOTH position and yaw: a heading teleport (e.g. a 180 deg + // flip in a symmetric corridor) can leave x/y nearly fixed, so position alone would + // miss it and follow the flip transparently. + const bool large_innov = planarDist(candidate, s.held) > p.jump_hold || yawDist(candidate, s.held) > p.jump_hold_yaw; + bool follow; // follow (accept) the candidate this update, vs hold and coast on odom + + if (!large_innov) + { + // Small innovation: normal tracking. Transparent unless spread says hold. + s.have_provisional = false; + follow = !s.spread_holding; + } + else + { + // Trigger 2: large innovation -> provisional persistence check (the way to tell a + // valid correction from a teleport: valid persists at one pose, ambiguity thrashes). + // "Same target" is measured against the ANCHOR captured when the clock started, not + // the latest candidate, so a target creeping just under the tolerance each step + // still eventually resets rather than being accepted after drifting far. + const bool same_target = s.have_provisional && planarDist(candidate, s.provisional) <= p.provisional_tol && + yawDist(candidate, s.provisional) <= p.provisional_tol_yaw; + // A confident-WRONG lock (e.g. scan ambiguity sliding along a smooth surface) persists + // at one pose while its cloud stays SEVERELY spread -- persistence alone cannot tell it + // from a real correction. Fold the spread check INTO the persistence requirement rather + // than testing it only at the acceptance instant: re-anchoring the clock whenever spread + // exceeds spread_accept_max means persist_time must elapse with the cloud continuously + // tight. A single-frame spread dip during a wrong lock then cannot ratchet the held pose + // toward it (the blend below is stateful and never reverts). Persistence still overrides + // MODERATE spread by design (a bad-seed recovery converges below the cap and is accepted). + const bool trustworthy = spread <= p.spread_accept_max; + // A backward clock jump (sim reset / bag loop) leaves provisional_since in the future, so + // now - provisional_since goes negative and acceptance stalls until the clock passes the old + // value. Treat it like a new target: re-anchor and restart the clock. (appendOdomSample flushes + // its buffer on the same condition, so the two stay consistent.) + const bool clock_went_backward = s.have_provisional && now < s.provisional_since; + if (!same_target || !trustworthy || clock_went_backward) + { + // New/moving target, or cloud too spread to trust: (re)anchor and restart the clock, hold. + s.provisional = candidate; + s.provisional_since = now; + s.have_provisional = true; + follow = false; + } + else + { + // Candidate has persisted near the anchor with a tight-enough cloud for persist_time. + follow = (now - s.provisional_since >= p.persist_time); + } + } + + // Asymmetric slew: clamp to a hold IMMEDIATELY (react fast the instant a jump or + // high spread is detected, so the held pose is not dragged toward a bad candidate), + // but ease back to transparent GRADUALLY on resume (so re-engaging never teleports). + const double target = follow ? 1.0 : 0.0; + s.alpha = (target < s.alpha) ? target : std::min(s.alpha + p.alpha_slew, target); + s.held = blend(s.held, candidate, s.alpha); + return s.held; +} +} // namespace amcl_odom_gate::detail diff --git a/src/hangar_sim/src/odom_world_drift.cpp b/src/hangar_sim/src/odom_world_drift.cpp new file mode 100644 index 000000000..f44ced44d --- /dev/null +++ b/src/hangar_sim/src/odom_world_drift.cpp @@ -0,0 +1,195 @@ +// 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. + +// Publish odom -> world so that odom -> base equals fuse's (drifty) estimate, +// while robot_state_publisher keeps world -> base as ground truth. +// +// odom -> world = fuse_estimate(odom->base) (+) inverse(world->base_true) +// makes the TF lookup odom -> base resolve to fuse's estimate; AMCL then has real +// drift to correct. Replaces the static odom -> world identity when use_fuse:=true. +// +// Sim-only. The drift lives in the ground plane, so everything here is 2D pose +// algebra (x, y, yaw), shared with the other hangar_sim nodes via se2.hpp. The +// ground-truth base pose is read straight from the virtual-rail joints on +// /joint_states rather than a tf2 listener: the sim floods /tf at ~700 Hz across +// ~140 frames, and buffering that just to read one pose is wasteful. (The Python +// predecessor also avoided numpy here to dodge OpenBLAS's spin-waiting worker +// threads; in C++ there is no such dependency to avoid.) + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "hangar_sim/odom_world_drift_logic.hpp" +#include "hangar_sim/se2.hpp" + +namespace +{ +using hangar_sim::se2::compose; +using hangar_sim::se2::invert; +using hangar_sim::se2::Pose2; +// Rail-joint list + index resolver live in an internal-namespace header so their branches are +// unit-tested (test_odom_world_drift); the frame/kinematics assumptions are documented there. +using odom_world_drift::internal::kRailJoints; +using odom_world_drift::internal::resolveRailIndices; + +constexpr double kPubPeriod = 0.02; // 50 Hz, keeps odom->world fresh for AMCL +constexpr double kEstStaleSec = 0.5; // ~5x fuse's ~10 Hz publish period: treat /odom_filtered as + // stale (fuse crashed/stalled) beyond this and stop + // broadcasting, rather than replaying a frozen estimate + // against a still-advancing ground-truth base_ stamp. +} // namespace + +class OdomWorldDrift : public rclcpp::Node +{ +public: + OdomWorldDrift() : Node("odom_world_drift"), tf_broadcaster_(*this) + { + // Depth-10 default-reliable on both: fuse publishes /odom_filtered reliable, and + // joint_state_broadcaster publishes /joint_states reliable. The node only ever uses the latest + // sample of each (no history), so the small queue is intentional -- it just tolerates a brief + // callback-scheduling burst. + est_sub_ = create_subscription( + "/odom_filtered", 10, [this](nav_msgs::msg::Odometry::ConstSharedPtr m) { onEst(*m); }); + joints_sub_ = create_subscription( + "/joint_states", 10, [this](sensor_msgs::msg::JointState::ConstSharedPtr m) { onJoints(*m); }); + // Node-clock timer (not wall): the node runs use_sim_time, so the 50 Hz tick must advance on + // sim time too, matching the stamp taken from the same clock in publish(). + timer_ = + rclcpp::create_timer(this, get_clock(), rclcpp::Duration::from_seconds(kPubPeriod), [this]() { publish(); }); + } + +private: + void onEst(const nav_msgs::msg::Odometry& m) + { + const auto& p = m.pose.pose; + est_ = Pose2{ p.position.x, p.position.y, tf2::getYaw(p.orientation) }; + est_stamp_ = get_clock()->now(); // arrival time, not the sender's stamp: staleness is judged + // against how long WE have gone without a fresh sample. + } + + void onJoints(const sensor_msgs::msg::JointState& msg) + { + // /joint_states is multi-publisher: messages can omit the rail joints or order them + // differently, so re-resolve the indices whenever the name list changes rather than trusting + // a stale cache. Cache the name list unconditionally -- including on a failed resolve -- so a + // repeated rail-less list is recognized and skipped without re-searching every message. + if (msg.name != cached_names_) + { + cached_names_ = msg.name; + idx_ = resolveRailIndices(msg.name); + } + if (!idx_.has_value()) + { + return; + } + // position and name normally align, but position may be shorter than a stale index implies; + // bounds-check before operator[] (unlike Python, an out-of-range read here is UB, not an + // exception that merely drops the callback). + const auto& idx = idx_.value(); + if (std::any_of(idx.begin(), idx.end(), [&](std::size_t i) { return i >= msg.position.size(); })) + { + return; + } + // Read the rail joints directly as world->base (x, y, yaw). Exact only under the sim's rail + // kinematics and zero base-link offset -- see kRailJoints in odom_world_drift_logic.hpp. + base_ = Pose2{ msg.position[idx[0]], msg.position[idx[1]], msg.position[idx[2]] }; + } + + void publish() + { + if (!est_.has_value() || !base_.has_value()) + { + return; + } + // base_ keeps updating from /joint_states at 50 Hz independent of fuse's health, so a stale + // est_ (fuse crashed or stalled -- now recoverable via respawn) would otherwise broadcast + // odom->world with an advancing stamp but frozen content: the TF lookup stays "available" and + // AMCL silently localizes against a base that appears not to be moving. Withhold instead. + const double est_age = (get_clock()->now() - est_stamp_).seconds(); + if (est_age > kEstStaleSec) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "odom_world_drift: /odom_filtered is %.2fs stale (fuse down?) -- " + "withholding odom->world so downstream TF lookups fail loudly instead " + "of localizing against a frozen estimate.", + est_age); + return; + } + // odom->world = odom->base(est) (+) inverse(world->base(true)) + // Note: est (fuse, ~10 Hz) and base (rail joints, ~50 Hz) are the latest of each, sampled at + // different instants, so during motion this carries up to ~speed*0.1s of timing-skew noise + // (~5 cm at 0.5 m/s). Second-order vs the 300 ms lag the gate's latency-comp corrects, and it + // just presents as extra apparent drift for AMCL to absorb -- left as-is intentionally. + const Pose2 t = compose(est_.value(), invert(base_.value())); + + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = get_clock()->now(); + tf.header.frame_id = "odom"; + tf.child_frame_id = "world"; + tf.transform.translation.x = t.x; + tf.transform.translation.y = t.y; + tf.transform.rotation.z = std::sin(t.yaw / 2.0); + tf.transform.rotation.w = std::cos(t.yaw / 2.0); + tf_broadcaster_.sendTransform(tf); + } + + // All state below is written and read only from the two subscription callbacks and the timer, + // which share the node's default mutually-exclusive callback group -- so they never run + // concurrently and the state needs no locking. + std::optional est_; // fuse estimate odom->base + rclcpp::Time est_stamp_; // arrival time of the last est_ + std::optional base_; // ground-truth world->base + std::vector cached_names_; // /joint_states name list idx_ resolves against + std::optional> idx_; // cached rail-joint indices into that list + // ROS entities declared last so they destruct (and stop firing callbacks) before the state above. + tf2_ros::TransformBroadcaster tf_broadcaster_; + rclcpp::Subscription::SharedPtr est_sub_; + rclcpp::Subscription::SharedPtr joints_sub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/hangar_sim/src/slip_aware_odom.cpp b/src/hangar_sim/src/slip_aware_odom.cpp new file mode 100644 index 000000000..d17af4974 --- /dev/null +++ b/src/hangar_sim/src/slip_aware_odom.cpp @@ -0,0 +1,196 @@ +// 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. + +// Forward the active platform controller's odometry as one continuous stream with +// a yaw covariance that grows during spin, for fuse's wheel_odom_sensor. +// +// Two mecanum controllers drive the base -- platform_velocity_controller_nav2 for +// Nav2, platform_velocity_controller for whole-body -- and only the active one +// publishes odom. Fuse needs a single unbroken input across every nav <-> whole- +// body switch, so this forwards whichever controller is publishing and stitches +// the handoff: each source's pose is rigidly offset to continue from the last +// output, so the stream never jumps (fuse consumes it differentially). Forwarding +// the controllers' own ~600 Hz odometry keeps their accuracy rather than +// re-deriving it from wheel states. The yaw covariance ramps from kBaseCov (trust +// wheel yaw, bounding the IMU gyro's drift) toward kSpinCov (defer to the IMU) as +// spin accumulates, because mecanum rollers slip in place. +// +// The deactivating controller stops publishing a beat before the activating one +// starts, so at each switch there is a brief window where neither source is live. +// A timer bridges it by holding the last pose at zero velocity (see hold()): fuse +// then never loses its wheel constraint and cannot coast, which otherwise jumped +// the estimate ~1 m at the switch and flipped the map during whole-body motion. + +#include + +#include +#include +#include +#include + +#include "hangar_sim/slip_aware_odom_logic.hpp" + +namespace +{ +constexpr int kYaw = 35; // (yaw, yaw) pose element and (vyaw, vyaw) twist element +constexpr double kHoldPeriod = 0.02; // 50 Hz: rate at which to check for a silent source +constexpr double kHoldGap = 0.015; // hold the last pose once the source is quiet this long [s] +constexpr double kHoldMaxSec = 0.5; // stop holding (and warn) once the source has been quiet this + // long: well beyond the ~15 ms handoff beat kHoldGap bridges, + // so a hold reaching this means the active controller died, + // not a handoff -- keep publishing a fabricated "parked" state + // forever would tell fuse the base is stationary indefinitely. +constexpr int64_t kHoldMaxWarnThrottleMs = 2000; // min interval between hold-timeout warnings [ms] + +using slip_aware_odom::Pose2; +using slip_aware_odom::RelayParams; +using slip_aware_odom::RelayState; +using slip_aware_odom::updateRelay; +} // namespace + +class SlipAwareOdom : public rclcpp::Node +{ +public: + SlipAwareOdom() : Node("slip_aware_odom") + { + odom_.header.frame_id = "odom"; + odom_.child_frame_id = "ridgeback_base_link"; + // The published pose is a re-anchored stream seeded at the origin, not a true + // absolute odom->base -- fuse consumes it differentially, so only its deltas are + // meaningful. No TF is published from it. An absolute consumer would be wrong. + pub_ = create_publisher("/odom_slip_aware", 10); + // source 0 = nav2 controller, source 1 = whole-body controller. + nav_sub_ = create_subscription("/platform_velocity_controller_nav2/odom", 10, + [this](nav_msgs::msg::Odometry::ConstSharedPtr m) { + relay(*m, 0); + }); + body_sub_ = create_subscription( + "/platform_velocity_controller/odom", 10, [this](nav_msgs::msg::Odometry::ConstSharedPtr m) { relay(*m, 1); }); + last_relay_ = now(); + // Node-clock timer (not wall): the hold-gap check in hold() compares against now() (sim time), + // so the tick must advance on sim time too, not wall time, under use_sim_time. + hold_timer_ = + rclcpp::create_timer(this, get_clock(), rclcpp::Duration::from_seconds(kHoldPeriod), [this]() { hold(); }); + } + +private: + void relay(const nav_msgs::msg::Odometry& m, int source) + { + // Forward every message: fuse consumes each one as a differential yaw constraint, + // and that full density is what holds AMCL locked through the fast base turns. + // Throttling (tested at 50/200 Hz) starves those constraints and AMCL diverges + // mid-turn, so the controller's native rate is the necessary rate here. That rate + // is ~500 Hz (measured): the forked clearpath_mecanum_drive_controller publishes + // odom every update() and ignores its `publish_rate: 50.0` param, so kHoldGap is + // sized for ~500 Hz, not the 50 Hz the config implies. + const double t = rclcpp::Time(m.header.stamp).seconds(); + const auto& q = m.pose.pose.orientation; + const Pose2 p{ m.pose.pose.position.x, m.pose.pose.position.y, tf2::getYaw(q) }; + + // Only the active controller publishes odom (the inactive one is silent), so the + // two sources never interleave and a source change always means a genuine handoff. + // updateRelay re-anchors on that switch (or a stale gap) so the output continues + // seamlessly, and ramps the yaw covariance toward spin_cov as the leaky-integrated + // |yaw rate| accumulates. Pure logic lives in slip_aware_odom_logic.hpp -- unit-tested + // in test/test_slip_aware_odom.cpp. + const auto result = updateRelay(p, t, m.twist.twist.angular.z, source, relay_params_, relay_state_); + + odom_.header.stamp = m.header.stamp; + odom_.pose.pose.position.x = result.pose.x; + odom_.pose.pose.position.y = result.pose.y; + odom_.pose.pose.orientation.z = std::sin(result.pose.yaw / 2.0); + odom_.pose.pose.orientation.w = std::cos(result.pose.yaw / 2.0); + odom_.pose.covariance = m.pose.covariance; // keep the controller's x/y covariance + odom_.twist = m.twist; // body-frame twist is source-independent + // Grow both the pose-yaw and twist-yaw-rate covariance so fuse defers to the IMU + // gyro -- for orientation and yaw rate alike -- while the mecanum wheels slip. + odom_.pose.covariance[kYaw] = result.yaw_cov; + odom_.twist.covariance[kYaw] = result.yaw_cov; + pub_->publish(odom_); + last_relay_ = now(); + } + + // Bridge the brief source silence at a controller handoff: once the active + // controller has been quiet for kHoldGap, republish the last pose at zero + // velocity so fuse keeps a live wheel constraint and cannot coast. During + // normal driving the ~500 Hz stream keeps last_relay_ fresh and this no-ops. + void hold() + { + const double gap = (now() - last_relay_).seconds(); + if (gap > kHoldMaxSec) + { + // The active source has been quiet far longer than a handoff beat -- it died (crashed + // controller, deactivated with nothing taking over) rather than switched. Stop fabricating + // zero-velocity odometry: let fuse's wheel constraint lapse loudly instead of asserting + // "parked" indefinitely. + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), kHoldMaxWarnThrottleMs, + "slip_aware_odom: active source has been silent for %.2fs (> %.2fs) -- " + "treating it as dead, not a handoff. Withholding held odometry.", + gap, kHoldMaxSec); + return; + } + // computeHoldStamp returns the stamp (last relayed message time, on the controllers' + // clock, advanced by the elapsed gap) to publish a held sample at, or nullopt if + // nothing has been relayed yet or the gap hasn't reached kHoldGap. Pure logic in + // slip_aware_odom_logic.hpp -- unit-tested in test/test_slip_aware_odom.cpp. + const auto held_opt = slip_aware_odom::computeHoldStamp(relay_state_.active, relay_state_.last_t, gap, kHoldGap); + if (!held_opt.has_value()) + { + return; + } + const double held = held_opt.value(); + odom_.header.stamp.sec = static_cast(held); + odom_.header.stamp.nanosec = static_cast((held - std::floor(held)) * 1e9); + odom_.twist.twist.linear.x = 0.0; + odom_.twist.twist.linear.y = 0.0; + odom_.twist.twist.angular.z = 0.0; + pub_->publish(odom_); + } + + // All shared state below is written and read only from relay() and hold(), which + // share the node's default mutually-exclusive callback group -- so they never run + // concurrently and the state needs no locking. Do NOT move either callback to a + // separate or reentrant callback group without adding synchronization. + nav_msgs::msg::Odometry odom_; + RelayParams relay_params_; // relay/covariance-ramp tunables (defaults match the prior constants) + RelayState relay_state_; // mutable re-anchor/slip state, updated in relay() + rclcpp::Time last_relay_; // node-clock time of the last forwarded message + // ROS entities declared last so they destruct (and stop firing callbacks) before + // the state above that those callbacks read. + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr nav_sub_, body_sub_; + rclcpp::TimerBase::SharedPtr hold_timer_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/hangar_sim/test/test_amcl_odom_gate.cpp b/src/hangar_sim/test/test_amcl_odom_gate.cpp new file mode 100644 index 000000000..7ca5f478e --- /dev/null +++ b/src/hangar_sim/test/test_amcl_odom_gate.cpp @@ -0,0 +1,583 @@ +// 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. + +// Unit tests for the pure gate-decision function detail::updateGate. Each test +// encodes a failure mode observed in sim as a regression: run a candidate stream +// through the gate and assert the broadcast map->odom behaves. No ROS, no TF. + +#include +#include + +#include "hangar_sim/amcl_odom_gate_logic.hpp" + +namespace +{ +using amcl_odom_gate::appendOdomSample; +using amcl_odom_gate::blend; +using amcl_odom_gate::compose; +using amcl_odom_gate::GateParams; +using amcl_odom_gate::interpolateOdom; +using amcl_odom_gate::invert; +using amcl_odom_gate::OdomSample; +using amcl_odom_gate::planarDist; +using amcl_odom_gate::Pose2; +using amcl_odom_gate::yawDist; +using amcl_odom_gate::detail::GateState; +using amcl_odom_gate::detail::updateGate; + +constexpr double kDt = 0.033; // 30 Hz, matching the node's broadcast period. + +// Drive `count` updates of the same candidate/spread through the gate and return the +// final broadcast pose. `t` advances by kDt each step so persistence timers elapse. +Pose2 run(const Pose2& candidate, double spread, int count, const GateParams& p, GateState& s, double& t) +{ + Pose2 out = s.held; + for (int i = 0; i < count; ++i) + { + out = updateGate(candidate, spread, t, p, s); + t += kDt; + } + return out; +} +} // namespace + +// SE(2) math: compose(a, invert(a)) is the identity, and compose/invert round-trip a +// pose. Guards the transform algebra the gate's candidate computation relies on. +TEST(AmclOdomGate, ComposeInvertRoundTrips) +{ + const Pose2 a{ 1.5, -2.0, 0.9 }; + const Pose2 b{ -0.3, 0.7, -1.2 }; + // GIVEN a pose a; WHEN composed with its inverse; THEN the result is the identity. + const Pose2 id = compose(a, invert(a)); + EXPECT_NEAR(id.x, 0.0, 1e-9); + EXPECT_NEAR(id.y, 0.0, 1e-9); + EXPECT_NEAR(std::remainder(id.yaw, 2.0 * M_PI), 0.0, 1e-9); + // AND applying b then removing it via inverse-compose recovers b. + const Pose2 rt = compose(invert(a), compose(a, b)); + EXPECT_NEAR(rt.x, b.x, 1e-9); + EXPECT_NEAR(rt.y, b.y, 1e-9); + EXPECT_NEAR(yawDist(rt, b), 0.0, 1e-9); +} + +// blend wraps yaw through the SHORT arc: blending 170 deg and -170 deg lands near +// 180 deg, not near 0. A long-way-around implementation would fail this. +TEST(AmclOdomGate, BlendYawTakesShortArc) +{ + const Pose2 a{ 0.0, 0.0, 170.0 * M_PI / 180.0 }; + const Pose2 b{ 0.0, 0.0, -170.0 * M_PI / 180.0 }; + const Pose2 mid = blend(a, b, 0.5); + EXPECT_NEAR(yawDist(mid, Pose2{ 0.0, 0.0, M_PI }), 0.0, 1e-6); +} + +// interpolateOdom: empty buffer yields nullopt (caller falls back to a live TF lookup). +TEST(AmclOdomGate, InterpolateOdomEmptyIsNullopt) +{ + // GIVEN an empty odom buffer; WHEN interpolating; THEN there is no value. + const std::vector buf; + EXPECT_FALSE(interpolateOdom(buf, 1.0).has_value()); +} + +// interpolateOdom: a target strictly between two samples is linearly interpolated in x/y/yaw. +TEST(AmclOdomGate, InterpolateOdomLinearBetweenSamples) +{ + // GIVEN two samples 1 s apart moving +2 m in x and +0.4 rad in yaw. + const std::vector buf{ { 10.0, { 0.0, 0.0, 0.0 } }, { 11.0, { 2.0, 0.0, 0.4 } } }; + + // WHEN interpolating at the midpoint (t = 10.5). + const auto out = interpolateOdom(buf, 10.5); + + // THEN it is exactly halfway. + ASSERT_TRUE(out.has_value()); + EXPECT_NEAR(out->x, 1.0, 1e-9); + EXPECT_NEAR(out->y, 0.0, 1e-9); + EXPECT_NEAR(out->yaw, 0.2, 1e-9); +} + +// interpolateOdom: a target at or past the ends clamps to the nearest sample (no extrapolation) -- +// this is what happens at startup before the buffer spans (now - lag). +TEST(AmclOdomGate, InterpolateOdomClampsOutOfRange) +{ + const std::vector buf{ { 10.0, { 1.0, 2.0, 0.1 } }, { 11.0, { 3.0, 4.0, 0.2 } } }; + + // GIVEN a target BEFORE the buffer; THEN it clamps to the front. + const auto before = interpolateOdom(buf, 9.0); + ASSERT_TRUE(before.has_value()); + EXPECT_NEAR(before->x, 1.0, 1e-9); + EXPECT_NEAR(before->y, 2.0, 1e-9); + + // AND a target AFTER the buffer clamps to the back (never extrapolates forward). + const auto after = interpolateOdom(buf, 99.0); + ASSERT_TRUE(after.has_value()); + EXPECT_NEAR(after->x, 3.0, 1e-9); + EXPECT_NEAR(after->y, 4.0, 1e-9); +} + +// interpolateOdom: yaw interpolates through the SHORT arc across the +/-pi wrap, not the long way. +TEST(AmclOdomGate, InterpolateOdomYawWrapsShortArc) +{ + const std::vector buf{ { 0.0, { 0.0, 0.0, 170.0 * M_PI / 180.0 } }, + { 1.0, { 0.0, 0.0, -170.0 * M_PI / 180.0 } } }; + const auto mid = interpolateOdom(buf, 0.5); + ASSERT_TRUE(mid.has_value()); + EXPECT_NEAR(yawDist(*mid, Pose2{ 0.0, 0.0, M_PI }), 0.0, 1e-6); +} + +// interpolateOdom: a single-element buffer returns that element for any target (front() == back(), +// exercising both clamp branches against the same sample). +TEST(AmclOdomGate, InterpolateOdomSingleElement) +{ + const std::vector buf{ { 5.0, { 1.0, 2.0, 0.3 } } }; + for (const double target : { 4.0, 5.0, 6.0 }) + { + const auto out = interpolateOdom(buf, target); + ASSERT_TRUE(out.has_value()); + EXPECT_NEAR(out->x, 1.0, 1e-9); + EXPECT_NEAR(out->y, 2.0, 1e-9); + EXPECT_NEAR(out->yaw, 0.3, 1e-9); + } +} + +// interpolateOdom with duplicate trailing timestamps returns cleanly (no NaN) via the +// `target >= back().t` clamp -- NOT via the loop's zero-span guard, which is unreachable for any +// input (the loop returns at the first buf[i].t >= target, so buf[i-1].t < target <= buf[i].t +// always implies a positive span; the guard is purely defensive). appendOdomSample forbids +// duplicate stamps in production anyway; this only pins the pure function's clamp behaviour. +TEST(AmclOdomGate, InterpolateOdomDuplicateTrailingStampsClamp) +{ + const std::vector buf{ { 0.0, { 0.0, 0.0, 0.0 } }, + { 1.0, { 1.0, 0.0, 0.0 } }, + { 1.0, { 9.0, 0.0, 0.0 } } }; + const auto out = interpolateOdom(buf, 1.0); // clamps to back() since target == back().t + ASSERT_TRUE(out.has_value()); + EXPECT_FALSE(std::isnan(out->x)); + EXPECT_NEAR(out->x, 9.0, 1e-9); // the back sample, via the clamp +} + +// appendOdomSample: newer samples append and the buffer stays trimmed to the keep window behind +// the newest, leaving the buffer time-ascending. +TEST(AmclOdomGate, AppendOdomSampleTrimsToWindow) +{ + std::vector buf; + // GIVEN samples 0.0..1.5 s appended with a 1.0 s keep window. + for (int i = 0; i <= 15; ++i) + { + appendOdomSample(buf, { 0.1 * i, { 0.1 * i, 0.0, 0.0 } }, 1.0); + } + // THEN nothing older than (1.5 - 1.0) survives, and the buffer is strictly ascending. + ASSERT_FALSE(buf.empty()); + EXPECT_GE(buf.front().t, 0.5 - 1e-9); + EXPECT_NEAR(buf.back().t, 1.5, 1e-9); + for (size_t i = 1; i < buf.size(); ++i) + { + EXPECT_GT(buf[i].t, buf[i - 1].t); + } +} + +// appendOdomSample: a stale/duplicate stamp (not strictly newer than the back) is ignored, so the +// buffer never goes out of order even if TF replays an old or repeated transform. +TEST(AmclOdomGate, AppendOdomSampleIgnoresNonNewer) +{ + std::vector buf; + appendOdomSample(buf, { 10.0, { 1.0, 0.0, 0.0 } }, 5.0); + appendOdomSample(buf, { 10.0, { 2.0, 0.0, 0.0 } }, 5.0); // equal stamp -> ignored + appendOdomSample(buf, { 9.5, { 3.0, 0.0, 0.0 } }, 5.0); // older stamp -> ignored + ASSERT_EQ(buf.size(), 1u); + EXPECT_NEAR(buf.front().pose.x, 1.0, 1e-9); // still the original sample +} + +// appendOdomSample: a sample far older than the newest (a backward clock jump: sim reset / bag loop) +// flushes the stale buffer and re-seeds, rather than freezing forever under the strictly-newer rule. +TEST(AmclOdomGate, AppendOdomSampleFlushesOnBackwardJump) +{ + std::vector buf; + // GIVEN a buffer around t=100 with a 1 s window. + for (int i = 0; i <= 10; ++i) + { + appendOdomSample(buf, { 100.0 + 0.1 * i, { 0.1 * i, 0.0, 0.0 } }, 1.0); + } + ASSERT_GT(buf.size(), 1u); + + // WHEN the clock jumps back to t=5 (far more than one window older than the back). + appendOdomSample(buf, { 5.0, { 42.0, 0.0, 0.0 } }, 1.0); + + // THEN the stale pre-jump samples are gone and the buffer re-seeds from the post-jump sample. + ASSERT_EQ(buf.size(), 1u); + EXPECT_NEAR(buf.front().t, 5.0, 1e-9); + EXPECT_NEAR(buf.front().pose.x, 42.0, 1e-9); +} + +// GIVEN a fresh gate; WHEN the first candidate arrives; THEN it is adopted as-is +// (nothing to hold onto yet). +TEST(AmclOdomGate, FirstCandidateAdoptedImmediately) +{ + GateParams p; + GateState s; + const Pose2 first{ 1.0, 2.0, 0.3 }; + const Pose2 out = updateGate(first, 0.2, 0.0, p, s); + EXPECT_NEAR(out.x, 1.0, 1e-9); + EXPECT_NEAR(out.y, 2.0, 1e-9); + EXPECT_NEAR(out.yaw, 0.3, 1e-9); +} + +// Case 3 (normal tracking, no jitter/accuracy loss): small confident innovations are +// followed transparently -- the gate output equals AMCL within a hair. +TEST(AmclOdomGate, SmallInnovationsTrackTransparently) +{ + GateParams p; + GateState s; + double t = 0.0; + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); + // WHEN AMCL drifts slowly (each step well under jump_hold) at low spread + Pose2 out{ 0.0, 0.0, 0.0 }; + for (int i = 1; i <= 30; ++i) + { + const Pose2 c{ 0.01 * i, 0.0, 0.0 }; // 1 cm/step, far below jump_hold + out = updateGate(c, 0.2, t, p, s); + t += kDt; + } + // THEN the gate has followed AMCL essentially exactly (no held lag, no jitter). + EXPECT_NEAR(out.x, 0.30, 0.02); +} + +// Case 2 (plane/box ambiguity): a candidate that THRASHES (jumps to a different far +// pose every update) never persists, so the gate holds -- the robot coasts on odom. +TEST(AmclOdomGate, ThrashingTeleportsAreHeld) +{ + GateParams p; + GateState s; + double t = 0.0; + const Pose2 good{ 0.0, 0.0, 0.0 }; + run(good, 0.2, 1, p, s, t); // establish a confident held pose + + // WHEN AMCL teleports to a DIFFERENT far pose each update (ambiguity artifact), + // with spread also elevated as it does near unmapped/degenerate geometry. + Pose2 out{ 0.0, 0.0, 0.0 }; + const double far = 5.0; + for (int i = 0; i < 60; ++i) + { + const double ang = 0.7 * i; // walk the teleport around so it never repeats a spot + const Pose2 c{ far * std::cos(ang), far * std::sin(ang), 0.0 }; + out = updateGate(c, 2.0, t, p, s); // spread 2.0 > spread_hold + t += kDt; + } + // THEN the broadcast pose stayed near the last good pose (held), NOT chasing the + // teleports -- otherwise the map would flip. + EXPECT_LT(planarDist(out, good), 0.5); +} + +// Case 2b (yaw teleport): a heading flip with x/y essentially unchanged must still be +// caught -- position innovation alone would miss it. The flip thrashes (never +// persists), so the gate holds the last good heading. +TEST(AmclOdomGate, YawTeleportWithStablePositionIsHeld) +{ + GateParams p; + GateState s; + double t = 0.0; + const Pose2 good{ 0.0, 0.0, 0.0 }; + run(good, 0.2, 1, p, s, t); + + // WHEN yaw flips by ~+/-160 deg each update while x/y barely move, at LOW spread + // (position variance stays small -- the spread trigger cannot see a yaw-only flip). + Pose2 out{ 0.0, 0.0, 0.0 }; + for (int i = 0; i < 60; ++i) + { + const double yaw = ((i % 2) == 0) ? 2.8 : -2.8; // ~160 deg, alternating + out = updateGate({ 0.02, 0.0, yaw }, 0.2, t, p, s); + t += kDt; + } + // THEN the broadcast heading stayed near the last good heading (0), not chasing flips. + EXPECT_LT(yawDist(out, good), 0.35); +} + +// Case 1 (bad seed / genuinely-lost recovery): a large correction that PERSISTS at +// the same place is accepted after persist_time -- the gate must NOT lock in the +// wrong pose forever (the limitation the first gate hit). +TEST(AmclOdomGate, PersistentCorrectionIsAccepted) +{ + GateParams p; + GateState s; + double t = 0.0; + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); // held starts at origin (a bad seed) + + // WHEN AMCL consistently reports the true pose 8 m away (a real, stable correction), + // with the cloud converged there (low spread). + const Pose2 truth{ 8.0, 0.0, 0.0 }; + Pose2 out{ 0.0, 0.0, 0.0 }; + // Enough steps for persist_time to elapse AND the alpha ramp to converge. + for (int i = 0; i < 120; ++i) + { + out = updateGate(truth, 0.2, t, p, s); + t += kDt; + } + // THEN the gate eventually accepts and converges to the true pose (recovers), + // rather than holding the wrong seed indefinitely. + EXPECT_LT(planarDist(out, truth), 0.3); +} + +// Boundary: a persistent correction is NOT accepted before persist_time has elapsed +// (pins the timer -- proves it waits, and that persist_time is load-bearing). +TEST(AmclOdomGate, PersistentCorrectionHeldUntilTimerElapses) +{ + GateParams p; + p.persist_time = 1.0; + GateState s; + double t = 0.0; + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); + + const Pose2 truth{ 8.0, 0.0, 0.0 }; + // Just UNDER persist_time worth of updates (0.9 s of 0.033 s steps ~= 27). + Pose2 out{ 0.0, 0.0, 0.0 }; + for (int i = 0; i < 27; ++i) + { + out = updateGate(truth, 0.2, t, p, s); + t += kDt; + } + // THEN it is still held near origin -- the correction has not yet proven persistent. + EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.5); +} + +// Regression: a backward clock jump (sim reset / bag loop) must re-anchor the persistence timer. +// Without re-anchoring, provisional_since stays ahead of `now`, so now - provisional_since is +// negative and a valid persistent correction can never be accepted until the clock climbs back +// past the pre-jump timestamp -- the gate would strand itself holding a stale pose. +TEST(AmclOdomGate, BackwardClockJumpReanchorsPersistence) +{ + GateParams p; + p.persist_time = 1.0; + GateState s; + double t = 100.0; // start with a large clock so the jump below is unambiguously backward + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); + + // GIVEN a true correction that has begun persisting (anchor captured near t = 100), not yet accepted + const Pose2 truth{ 8.0, 0.0, 0.0 }; + for (int i = 0; i < 10; ++i) + { + updateGate(truth, 0.2, t, p, s); + t += kDt; + } + + // WHEN the clock jumps backward and the same true pose keeps persisting from there for > persist_time + t = 0.0; + Pose2 out{ 0.0, 0.0, 0.0 }; + for (int i = 0; i < 120; ++i) + { + out = updateGate(truth, 0.2, t, p, s); + t += kDt; + } + + // THEN the gate re-anchored on the jump and accepts after persist_time from the NEW clock, rather + // than staying stranded waiting for the pre-jump timestamp (which would leave it near origin). + EXPECT_LT(planarDist(out, truth), 0.3); +} + +// Case 1b (confident-WRONG lock): a large candidate that PERSISTS at one pose but with +// a SEVERELY spread cloud is a divergence (e.g. scan ambiguity sliding along a smooth +// surface), not a real correction. Persistence alone would accept it; spread_accept_max +// must reject it so the gate keeps coasting on odom instead of adopting the wrong pose. +TEST(AmclOdomGate, SevereSpreadPersistentPoseNotAccepted) +{ + GateParams p; + GateState s; + double t = 0.0; + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); // established a confident held pose + + // WHEN AMCL sits at the same far pose well past persist_time, but the cloud stays + // severely spread (above spread_accept_max) the whole time -- the wrong-lock signature. + const Pose2 wrong{ 8.0, 0.0, 0.0 }; + const double severe_spread = 5.0; // > spread_accept_max (3.0) + Pose2 out{ 0.0, 0.0, 0.0 }; + for (int i = 0; i < 120; ++i) + { + out = updateGate(wrong, severe_spread, t, p, s); + t += kDt; + } + // THEN the gate never adopts it -- it stays held near the last good pose (coasting on + // odom), NOT snapping to the persistent-but-wrong pose. + EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.5); +} + +// Case 1c (right case preserved): a genuine recovery whose cloud starts severely spread +// then CONVERGES (spread drops below the cap) must still be accepted once it tightens -- +// the cap waits for spread to fall, it does not reject legitimate corrections. +TEST(AmclOdomGate, RecoveryAcceptedOnceSpreadDrops) +{ + GateParams p; + GateState s; + double t = 0.0; + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); + + const Pose2 truth{ 8.0, 0.0, 0.0 }; + Pose2 out{ 0.0, 0.0, 0.0 }; + // Phase 1: persistent at truth but spread still severe -> held (not yet accepted). + for (int i = 0; i < 60; ++i) + { + out = updateGate(truth, 5.0, t, p, s); // severe spread > spread_accept_max + t += kDt; + } + EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.5) << "must not accept while spread severe"; + + // Phase 2: cloud converges (spread drops below the cap) while the pose stays put. + for (int i = 0; i < 120; ++i) + { + out = updateGate(truth, 0.2, t, p, s); // spread now tight + t += kDt; + } + // THEN the now-converged correction is accepted and the gate recovers to truth. + EXPECT_LT(planarDist(out, truth), 0.3); +} + +// Boundary: pin spread_accept_max. A persisted correction just BELOW the cap is accepted; +// just ABOVE it is held. Probes threshold +/- a small epsilon so the constant cannot +// drift without this test failing (per test-design threshold+/-1 rule). +TEST(AmclOdomGate, SpreadAcceptMaxBoundary) +{ + constexpr double kEps = 0.1; + + // GIVEN spread just UNDER spread_accept_max: a persistent correction is accepted. + { + GateParams p; // spread_accept_max = 3.0 + // Push the spread-hysteresis band above the probed spread so this case isolates + // spread_accept_max alone -- otherwise the 2.9 probe also trips the default spread_hold (1.5), + // latching a hold that freezes the alpha ramp part-way and leaves only a ~0.04 m margin. + p.spread_hold = p.spread_accept_max + 1.0; + p.spread_resume = p.spread_accept_max; + GateState s; + double t = 0.0; + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); + const Pose2 truth{ 8.0, 0.0, 0.0 }; + Pose2 out{ 0.0, 0.0, 0.0 }; + for (int i = 0; i < 120; ++i) + { + out = updateGate(truth, p.spread_accept_max - kEps, t, p, s); + t += kDt; + } + EXPECT_LT(planarDist(out, truth), 0.3) << "just below the cap must be accepted"; + } + + // GIVEN spread just OVER spread_accept_max: the same persistent correction is held. + { + GateParams p; + GateState s; + double t = 0.0; + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); + const Pose2 truth{ 8.0, 0.0, 0.0 }; + Pose2 out{ 0.0, 0.0, 0.0 }; + for (int i = 0; i < 120; ++i) + { + out = updateGate(truth, p.spread_accept_max + kEps, t, p, s); + t += kDt; + } + EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.5) << "just above the cap must be held"; + } +} + +// A persistent-but-wrong pose at severe spread must stay bounded near the last good pose +// for the FULL divergence window -- the gate never drifts toward it (regression for the +// organic plane-nose cascade where the gate snapped to a 6.7 m wrong lock after ~20 s). +TEST(AmclOdomGate, SevereSpreadWrongLockStaysBoundedThroughout) +{ + GateParams p; + GateState s; + double t = 0.0; + const Pose2 good{ 0.0, 0.0, 0.0 }; + run(good, 0.2, 1, p, s, t); + + // WHEN AMCL sits at a far wrong pose with severe spread for a long window (~20 s), + // far longer than persist_time. + const Pose2 wrong{ 6.7, 0.0, 0.0 }; + double worst = 0.0; + for (int i = 0; i < 600; ++i) // 600 * 0.033 s ~= 20 s + { + const Pose2 out = updateGate(wrong, 5.0, t, p, s); + worst = std::max(worst, planarDist(out, good)); + t += kDt; + } + // THEN the broadcast pose never crept toward the wrong lock at any point in the window. + EXPECT_LT(worst, 0.5); +} + +// The spread check is part of the persistence requirement, not an instantaneous gate: a +// wrong pose that persists while spread NOISILY dips below the cap on alternating frames +// must never ratchet the held pose toward it (the blend is stateful and never reverts). +TEST(AmclOdomGate, OscillatingSpreadAcrossCapDoesNotRatchet) +{ + GateParams p; + GateState s; + double t = 0.0; + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); + + // WHEN AMCL sits at a far wrong pose whose cloud spread flickers ABOVE and BELOW the cap + // (severe on even frames, just under on odd) -- so the longest continuously-tight run is a + // single frame, never persist_time. + const Pose2 wrong{ 8.0, 0.0, 0.0 }; + double worst = 0.0; + for (int i = 0; i < 300; ++i) + { + const double spread = ((i % 2) == 0) ? 5.0 : 2.0; // 2.0 < spread_accept_max(3.0) < 5.0 + const Pose2 out = updateGate(wrong, spread, t, p, s); + worst = std::max(worst, planarDist(out, Pose2{ 0.0, 0.0, 0.0 })); + t += kDt; + } + // THEN intermittent sub-cap dips never accumulate persist_time, so the held pose stays put. + EXPECT_LT(worst, 0.5); +} + +// Case 4 (spread hysteresis): high spread alone holds even for a small innovation, +// and it resumes only after spread drops below the lower resume threshold. +TEST(AmclOdomGate, HighSpreadHoldsAndResumesWithHysteresis) +{ + GateParams p; + GateState s; + double t = 0.0; + run({ 0.0, 0.0, 0.0 }, 0.2, 1, p, s, t); + + // WHEN spread is high, a small nearby candidate is held (not followed). + Pose2 out{ 0.0, 0.0, 0.0 }; + for (int i = 0; i < 20; ++i) + { + out = updateGate({ 0.2, 0.0, 0.0 }, 2.0, t, p, s); // spread 2.0 > spread_hold + t += kDt; + } + EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.15); + + // AND between spread_resume and spread_hold it stays held (hysteresis: no flicker). + for (int i = 0; i < 20; ++i) + { + out = updateGate({ 0.2, 0.0, 0.0 }, 1.0, t, p, s); // resume(0.6) < 1.0 < hold(1.5) + t += kDt; + } + EXPECT_LT(planarDist(out, Pose2{ 0.0, 0.0, 0.0 }), 0.2); + + // THEN once spread drops below spread_resume it follows AMCL again. + for (int i = 0; i < 40; ++i) + { + out = updateGate({ 0.2, 0.0, 0.0 }, 0.2, t, p, s); + t += kDt; + } + EXPECT_NEAR(out.x, 0.2, 0.03); +} diff --git a/src/hangar_sim/test/test_odom_world_drift.cpp b/src/hangar_sim/test/test_odom_world_drift.cpp new file mode 100644 index 000000000..b1b602d38 --- /dev/null +++ b/src/hangar_sim/test/test_odom_world_drift.cpp @@ -0,0 +1,104 @@ +// 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. + +#include +#include + +#include +#include + +#include "hangar_sim/odom_world_drift_logic.hpp" + +using odom_world_drift::internal::resolveRailIndices; +using ::testing::ElementsAre; + +// The three rail joints in canonical (x, y, yaw) order, plus decoys the real /joint_states carries. +TEST(ResolveRailIndices, AllPresentInCanonicalOrder) +{ + // GIVEN a name list with the rail joints first, in kRailJoints order + const std::vector names = { "linear_x_joint", "linear_y_joint", "rotational_yaw_joint" }; + + // WHEN resolving + const auto idx = resolveRailIndices(names); + + // THEN each rail joint maps to its own position + ASSERT_TRUE(idx.has_value()); + EXPECT_THAT(idx.value(), ElementsAre(0u, 1u, 2u)); +} + +TEST(ResolveRailIndices, ResolvesByNameNotColumnOrder) +{ + // GIVEN the rail joints interleaved with arm joints and reordered vs kRailJoints + const std::vector names = { "shoulder_pan_joint", "rotational_yaw_joint", "elbow_joint", + "linear_x_joint", "wrist_1_joint", "linear_y_joint" }; + + // WHEN resolving + const auto idx = resolveRailIndices(names); + + // THEN indices follow kRailJoints order (x, y, yaw), pointing at the actual columns + ASSERT_TRUE(idx.has_value()); + EXPECT_THAT(idx.value(), ElementsAre(3u, 5u, 1u)); +} + +TEST(ResolveRailIndices, MissingAnyRailJointReturnsNullopt) +{ + // GIVEN a list missing rotational_yaw_joint (e.g. an arm-only /joint_states publisher) + const std::vector names = { "linear_x_joint", "linear_y_joint", "elbow_joint" }; + + // WHEN resolving + const auto idx = resolveRailIndices(names); + + // THEN the whole resolve fails rather than returning a partial/garbage index + EXPECT_FALSE(idx.has_value()); +} + +TEST(ResolveRailIndices, EmptyNameListReturnsNullopt) +{ + // GIVEN an empty name list + const std::vector names = {}; + + // WHEN resolving + const auto idx = resolveRailIndices(names); + + // THEN it fails cleanly + EXPECT_FALSE(idx.has_value()); +} + +TEST(ResolveRailIndices, DuplicateNameTakesFirstOccurrence) +{ + // GIVEN linear_x_joint appearing twice + const std::vector names = { "linear_x_joint", "linear_x_joint", "linear_y_joint", + "rotational_yaw_joint" }; + + // WHEN resolving + const auto idx = resolveRailIndices(names); + + // THEN the first occurrence wins (index 0, not 1) + ASSERT_TRUE(idx.has_value()); + EXPECT_THAT(idx.value(), ElementsAre(0u, 2u, 3u)); +} diff --git a/src/hangar_sim/test/test_slip_aware_odom.cpp b/src/hangar_sim/test/test_slip_aware_odom.cpp new file mode 100644 index 000000000..7b1b4a5e1 --- /dev/null +++ b/src/hangar_sim/test/test_slip_aware_odom.cpp @@ -0,0 +1,260 @@ +// 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. + +#include +#include + +#include + +#include "hangar_sim/slip_aware_odom_logic.hpp" + +using slip_aware_odom::computeHoldStamp; +using slip_aware_odom::Pose2; +using slip_aware_odom::RelayParams; +using slip_aware_odom::RelayState; +using slip_aware_odom::updateRelay; +using ::testing::DoubleNear; + +namespace +{ +constexpr double kTol = 1e-6; +} // namespace + +// -------------------- updateRelay: first sample / continuity -------------------- + +TEST(UpdateRelay, FirstSampleSeedsOutputAtOriginRegardlessOfSourcePose) +{ + // GIVEN fresh state (out_ default-constructed at the origin, nothing relayed yet) + RelayState state; + const RelayParams params; + + // WHEN relaying the first sample, from an arbitrary non-origin source pose + const auto result = updateRelay(Pose2{ 1.0, 2.0, 0.5 }, /*t=*/10.0, /*yaw_rate=*/0.0, /*source=*/0, params, state); + + // THEN the output stream is seeded at the origin, not the raw source pose: with no prior output + // to preserve continuity from, the re-anchor (offset = compose(out=identity, invert(p))) makes + // compose(offset, p) collapse back to identity. This is the documented contract -- the published + // stream is re-anchored, not absolute -- and is why a single relayed message can never be trusted + // as an absolute pose. + EXPECT_THAT(result.pose.x, DoubleNear(0.0, kTol)); + EXPECT_THAT(result.pose.y, DoubleNear(0.0, kTol)); + EXPECT_THAT(result.pose.yaw, DoubleNear(0.0, kTol)); + EXPECT_EQ(state.active, 0); +} + +TEST(UpdateRelay, SameSourceContinuesWithoutReanchor) +{ + // GIVEN state anchored by an initial sample from source 0 + RelayState state; + const RelayParams params; + updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.0, 0.0, 0, params, state); + + // WHEN a second sample from the SAME source arrives, shifted by 2 m + const auto result = updateRelay(Pose2{ 2.0, 0.0, 0.0 }, 10.1, 0.0, 0, params, state); + + // THEN the output tracks the source's own delta directly (offset is unchanged) + EXPECT_THAT(result.pose.x, DoubleNear(2.0, kTol)); +} + +TEST(UpdateRelay, SourceSwitchReanchorsSeamlessly) +{ + // GIVEN a stream anchored at the origin by source 0, then driven to output x=5 + RelayState state; + const RelayParams params; + updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.0, 0.0, 0, params, state); + updateRelay(Pose2{ 5.0, 0.0, 0.0 }, 10.02, 0.0, 0, params, state); + + // WHEN source 1 takes over publishing its own pose (a different coordinate origin, e.g. x=0) + const auto result = updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.05, 0.0, /*source=*/1, params, state); + + // THEN the output continues from the last output pose (no jump) -- re-anchored at the switch + EXPECT_THAT(result.pose.x, DoubleNear(5.0, kTol)); + EXPECT_EQ(state.active, 1); +} + +TEST(UpdateRelay, StaleGapPastSourceGapReanchorsEvenOnSameSource) +{ + // GIVEN a stream anchored at the origin by source 0, then driven to output x=5 + RelayState state; + RelayParams params; + params.source_gap = 0.2; + updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.0, 0.0, 0, params, state); + updateRelay(Pose2{ 5.0, 0.0, 0.0 }, 10.02, 0.0, 0, params, state); + + // WHEN the SAME source resumes after a gap longer than source_gap, at a pose that jumped (e.g. the + // controller reset its own internal odom) + const auto result = updateRelay(Pose2{ 100.0, 0.0, 0.0 }, 10.02 + params.source_gap + 0.01, 0.0, 0, params, state); + + // THEN the output re-anchors instead of jumping to 100 + EXPECT_THAT(result.pose.x, DoubleNear(5.0, kTol)); +} + +TEST(UpdateRelay, GapAtExactlySourceGapDoesNotReanchor) +{ + // GIVEN a stream anchored at the origin by source 0, then driven to output x=5 + RelayState state; + RelayParams params; + params.source_gap = 0.2; + updateRelay(Pose2{ 0.0, 0.0, 0.0 }, 10.0, 0.0, 0, params, state); + updateRelay(Pose2{ 5.0, 0.0, 0.0 }, 10.02, 0.0, 0, params, state); + + // WHEN the same source resumes at EXACTLY the source_gap threshold (not longer than it) + const auto result = updateRelay(Pose2{ 105.0, 0.0, 0.0 }, 10.02 + params.source_gap, 0.0, 0, params, state); + + // THEN no re-anchor happens (the gap must be strictly greater than source_gap) -- output tracks + // the source's own delta through the unchanged offset: source moved +100 from its anchor pose + // (5,0,0), so output moves +100 from its own anchor (5,0,0) too. + EXPECT_THAT(result.pose.x, DoubleNear(105.0, kTol)); +} + +// -------------------- updateRelay: yaw-covariance ramp -------------------- + +TEST(UpdateRelay, NoYawRateKeepsCovarianceAtBase) +{ + // GIVEN fresh state + RelayState state; + const RelayParams params; + + // WHEN relaying several samples with zero yaw rate (driving straight) + auto result = updateRelay(Pose2{}, 10.0, 0.0, 0, params, state); + result = updateRelay(Pose2{}, 10.1, 0.0, 0, params, state); + result = updateRelay(Pose2{}, 10.2, 0.0, 0, params, state); + + // THEN the yaw covariance stays at base_cov + EXPECT_THAT(result.yaw_cov, DoubleNear(params.base_cov, kTol)); +} + +TEST(UpdateRelay, SustainedSpinRampsCovarianceTowardSpinCov) +{ + // GIVEN fresh state and a spin fast/long enough to fully saturate the leaky integral + // (|yaw_rate| * dt accumulated over many updates exceeds slip_full) + RelayState state; + const RelayParams params; + double t = 10.0; + + // WHEN relaying many samples at a sustained high yaw rate + auto result = updateRelay(Pose2{}, t, 0.0, 0, params, state); + for (int i = 0; i < 200; ++i) + { + t += 0.05; + result = updateRelay(Pose2{}, t, /*yaw_rate=*/5.0, 0, params, state); + } + + // THEN the covariance saturates at spin_cov (clamped, not overshooting) + EXPECT_THAT(result.yaw_cov, DoubleNear(params.spin_cov, 1e-3)); +} + +TEST(UpdateRelay, CovarianceDecaysAfterSpinStops) +{ + // GIVEN state that has ramped up to a high slip covariance from sustained spin + RelayState state; + const RelayParams params; + double t = 10.0; + auto result = updateRelay(Pose2{}, t, 0.0, 0, params, state); + for (int i = 0; i < 200; ++i) + { + t += 0.05; + result = updateRelay(Pose2{}, t, 5.0, 0, params, state); + } + const double spiking_cov = result.yaw_cov; + ASSERT_GT(spiking_cov, params.base_cov + 1.0); + + // WHEN the spin stops and several tau-scale gaps of straight driving follow + for (int i = 0; i < 20; ++i) + { + t += params.tau; + result = updateRelay(Pose2{}, t, 0.0, 0, params, state); + } + + // THEN the covariance decays back toward base_cov + EXPECT_THAT(result.yaw_cov, DoubleNear(params.base_cov, 1e-2)); +} + +TEST(UpdateRelay, MaxDtClampsGrowthButNotDecayOnStaleGap) +{ + // GIVEN state with a moderate slip level + RelayState state; + RelayParams params; + params.max_dt = 0.1; + double t = 10.0; + auto result = updateRelay(Pose2{}, t, 0.0, 0, params, state); + result = updateRelay(Pose2{}, t + 0.05, 5.0, 0, params, state); + const double slip_before = state.slip; + + // WHEN a stale gap much longer than max_dt arrives (e.g. a handoff silence) with a high yaw rate + result = updateRelay(Pose2{}, t + 5.05, /*yaw_rate=*/5.0, 0, params, state); + + // THEN the growth contribution is bounded by max_dt (slip cannot spike by yaw_rate * 5.0s), even + // though the decay term used the full 5.0s elapsed + const double max_possible_growth = 5.0 * params.max_dt; + EXPECT_LE(state.slip, slip_before * std::exp(-5.0 / params.tau) + max_possible_growth + kTol); +} + +// -------------------- computeHoldStamp -------------------- + +TEST(ComputeHoldStamp, NothingRelayedYetReturnsNullopt) +{ + // GIVEN no source has been relayed yet (active < 0) + // WHEN checking whether to hold + const auto held = computeHoldStamp(/*active=*/-1, /*last_t=*/0.0, /*gap=*/1.0, /*hold_gap=*/0.015); + + // THEN nothing is published + EXPECT_FALSE(held.has_value()); +} + +TEST(ComputeHoldStamp, GapBelowThresholdReturnsNullopt) +{ + // GIVEN an active source and a gap shorter than hold_gap + // WHEN checking whether to hold + const auto held = computeHoldStamp(/*active=*/0, /*last_t=*/10.0, /*gap=*/0.01, /*hold_gap=*/0.015); + + // THEN no held sample is published yet + EXPECT_FALSE(held.has_value()); +} + +TEST(ComputeHoldStamp, GapAtExactlyThresholdHolds) +{ + // GIVEN a gap exactly at hold_gap + // WHEN checking whether to hold + const auto held = computeHoldStamp(0, 10.0, /*gap=*/0.015, /*hold_gap=*/0.015); + + // THEN it holds (>= threshold triggers) + ASSERT_TRUE(held.has_value()); + EXPECT_THAT(held.value(), DoubleNear(10.015, kTol)); +} + +TEST(ComputeHoldStamp, GapPastThresholdAdvancesStampByFullGap) +{ + // GIVEN a source quiet well past hold_gap + // WHEN checking whether to hold + const auto held = computeHoldStamp(0, /*last_t=*/10.0, /*gap=*/0.5, /*hold_gap=*/0.015); + + // THEN the held stamp continues the relayed stream's clock base: last_t + gap + ASSERT_TRUE(held.has_value()); + EXPECT_THAT(held.value(), DoubleNear(10.5, kTol)); +} diff --git a/src/moveit_pro_clipseg b/src/moveit_pro_clipseg deleted file mode 160000 index 9d8d88222..000000000 --- a/src/moveit_pro_clipseg +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 9d8d882224c0373cc5ff92086f117bf1872ad84b