From cc0259baec2333dcdc1dc2b10bc418cc81c21438 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sat, 27 Jun 2026 15:05:55 -0400 Subject: [PATCH] fix(nav): strict priority-preference source arbitration NavigationSensors::updateLatest iterated sources in priority order but, when the highest-priority source was fresh yet had no sample newer than the global high-water mark, it fell through to the next source. A faster lower-priority source (FCU ~10 Hz) therefore out-voted a fresh but slower primary (SBG INS ~4 Hz), so position never followed the configured primary. Stop at the first fresh source in priority order: it owns the output and lower-priority sources are used only when higher-priority ones go stale past sensor_timeout. Extracted the decision into a pure, unit-tested selectNavigationSource() (new navigation_source_selection.hpp) with 9 gtest cases covering the regression, failover, recovery and boundaries. Validated live on BizzyBoat: nav/active_sensor/position flips fcu -> sbg with no base_link jump (~6 mm). Ref: unh_echoboats_project11#339 Co-Authored-By: Claude Opus 4.8 --- mru_transform/CMakeLists.txt | 10 ++ .../mru_transform/navigation_sensors.hpp | 52 +++++---- .../navigation_source_selection.hpp | 56 +++++++++ .../test/test_nav_source_selection.cpp | 106 ++++++++++++++++++ 4 files changed, 202 insertions(+), 22 deletions(-) create mode 100644 mru_transform/include/mru_transform/navigation_source_selection.hpp create mode 100644 mru_transform/test/test_nav_source_selection.cpp diff --git a/mru_transform/CMakeLists.txt b/mru_transform/CMakeLists.txt index 7a3b54a..b991305 100644 --- a/mru_transform/CMakeLists.txt +++ b/mru_transform/CMakeLists.txt @@ -360,6 +360,16 @@ if(BUILD_TESTING) target_include_directories(test_datum_config PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/include" ) + + # unh_echoboats_project11#339: strict priority-preference nav-source + # arbitration (highest-priority fresh source wins, not the newest sample). + ament_add_gtest(test_nav_source_selection test/test_nav_source_selection.cpp) + target_link_libraries(test_nav_source_selection + rclcpp::rclcpp + ) + target_include_directories(test_nav_source_selection PRIVATE + "${CMAKE_CURRENT_SOURCE_DIR}/include" + ) endif() ament_package() diff --git a/mru_transform/include/mru_transform/navigation_sensors.hpp b/mru_transform/include/mru_transform/navigation_sensors.hpp index 321857b..b3662f8 100644 --- a/mru_transform/include/mru_transform/navigation_sensors.hpp +++ b/mru_transform/include/mru_transform/navigation_sensors.hpp @@ -4,6 +4,7 @@ #include "mru_transform/position_sensor.hpp" #include "mru_transform/velocity_sensor.hpp" #include "mru_transform/orientation_sensor.hpp" +#include "mru_transform/navigation_source_selection.hpp" #include "std_msgs/msg/string.hpp" #include "rclcpp/node_interfaces/node_interfaces.hpp" @@ -40,29 +41,36 @@ class NavigationSensors /// @return True if a valid value was found template bool updateLatest(T &value, const SensorVectorT& sensors, const rclcpp::Time& now) { - for(auto s: sensors){ - rclcpp::Time sensor_time = s->latest_value().header.stamp; - rclcpp::Time value_time = value.header.stamp; - auto msg_age = now - sensor_time; - if(msg_age < sensor_timeout_){ - if(sensor_time > value_time){ - value = s->latest_value(); - if(active_sensor_pubs_.find(s->sensor_type) != active_sensor_pubs_.end()) - { - std_msgs::msg::String active; - active.data = s->name(); - active_sensor_pubs_[s->sensor_type]->publish(active); - } - return true; - } - else{ - RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 1000, "skipping message with time " << (value_time - sensor_time).seconds() << " seconds behind last value from sensor " << s->name()); - } - }else{ - RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5000, "sensor " << s->name() << "'s value is stale, age: " << msg_age.seconds() << " seconds"); - } + // Strict priority-preference: the highest-priority *fresh* source owns the + // output; lower-priority sources are used only when higher-priority ones go + // stale (see selectNavigationSource / unh_echoboats_project11#339). + std::vector sensor_stamps; + sensor_stamps.reserve(sensors.size()); + for(auto s: sensors) + sensor_stamps.push_back(s->latest_value().header.stamp); + + // Diagnostics: warn (throttled) about stale sources we skip past before + // reaching the source that owns arbitration (the first fresh one). + for(std::size_t i = 0; i < sensors.size(); ++i){ + auto msg_age = now - sensor_stamps[i]; + if(msg_age < sensor_timeout_) + break; + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5000, "sensor " << sensors[i]->name() << "'s value is stale, age: " << msg_age.seconds() << " seconds"); } - return false; + + int idx = selectNavigationSource(sensor_stamps, value.header.stamp, now, sensor_timeout_); + if(idx < 0) + return false; + + auto s = sensors[idx]; + value = s->latest_value(); + if(active_sensor_pubs_.find(s->sensor_type) != active_sensor_pubs_.end()) + { + std_msgs::msg::String active; + active.data = s->name(); + active_sensor_pubs_[s->sensor_type]->publish(active); + } + return true; } void positionCallback(const PositionSensor::ValueType &position); diff --git a/mru_transform/include/mru_transform/navigation_source_selection.hpp b/mru_transform/include/mru_transform/navigation_source_selection.hpp new file mode 100644 index 0000000..7741d05 --- /dev/null +++ b/mru_transform/include/mru_transform/navigation_source_selection.hpp @@ -0,0 +1,56 @@ +#ifndef MRU_TRANSFORM_NAVIGATION_SOURCE_SELECTION_HPP +#define MRU_TRANSFORM_NAVIGATION_SOURCE_SELECTION_HPP + +#include +#include + +#include +#include + +namespace mru_transform +{ + +/// @brief Choose which navigation source should drive the fused output, using +/// strict priority-preference. +/// +/// @p sensor_stamps holds the latest sample time of each source, in descending +/// priority order. The first source whose latest sample is fresh +/// (age < @p timeout, measured against @p now) is THE source: lower-priority +/// sources are consulted only when every higher-priority source is stale. The +/// selected source updates the output only when its latest sample is newer than +/// @p last_value_time (the stamp of the value last published); otherwise there +/// is no new data to emit on this call. +/// +/// @return index into @p sensor_stamps to adopt, or -1 if no source should +/// update on this call. +/// +/// This replaces the earlier "newest fresh sample wins" arbitration, where a +/// faster lower-priority source (e.g. the FCU at ~10 Hz) out-voted a fresh but +/// slower primary (e.g. the SBG INS at ~4 Hz): the priority loop fell through +/// whenever the primary carried no sample newer than a global high-water mark, +/// handing the slot to the next source. Stopping at the first fresh source +/// keeps the primary in control while it is healthy and fails over only on a +/// genuine dropout past @p timeout. See unh_echoboats_project11#339. +inline int selectNavigationSource( + const std::vector &sensor_stamps, + const rclcpp::Time &last_value_time, + const rclcpp::Time &now, + const rclcpp::Duration &timeout) +{ + for(std::size_t i = 0; i < sensor_stamps.size(); ++i) + { + if(now - sensor_stamps[i] < timeout) + { + // Highest-priority fresh source: it owns the output. Adopt only when it + // carries a sample we have not published yet; never fall through to a + // lower-priority source while this one is fresh. + return sensor_stamps[i] > last_value_time ? static_cast(i) : -1; + } + // Stale: fall through to the next-priority source. + } + return -1; +} + +} // namespace mru_transform + +#endif diff --git a/mru_transform/test/test_nav_source_selection.cpp b/mru_transform/test/test_nav_source_selection.cpp new file mode 100644 index 0000000..07456fb --- /dev/null +++ b/mru_transform/test/test_nav_source_selection.cpp @@ -0,0 +1,106 @@ +// Unit tests for selectNavigationSource() — strict priority-preference nav-source +// arbitration (unh_echoboats_project11#339). +// +// The regression this guards: a fresh but slower primary (SBG INS, ~4 Hz) was +// being out-voted by a faster lower-priority source (FCU, ~10 Hz) because the +// old arbitration adopted the newest fresh sample rather than the highest- +// priority fresh source. The fix stops at the first fresh source in priority +// order and never falls through to a lower-priority one while it is fresh. + +#include + +#include +#include +#include + +#include "mru_transform/navigation_source_selection.hpp" + +using mru_transform::selectNavigationSource; + +namespace +{ +// Build a ROS-time stamp from seconds (all stamps share RCL_ROS_TIME so the +// comparisons/subtractions match what rclcpp::Time(header.stamp) produces). +rclcpp::Time t(double seconds) +{ + return rclcpp::Time(static_cast(seconds * 1e9), RCL_ROS_TIME); +} + +const rclcpp::Duration kTimeout = rclcpp::Duration::from_seconds(0.5); +// Priority order: index 0 = SBG (primary), index 1 = FCU (fallback). +} // namespace + +// The core fix: when the FCU just published (now == FCU stamp) but the primary +// SBG is fresh, the FCU must NOT be selected even though it is newer. +TEST(NavSourceSelection, FreshPrimaryBlocksFasterFallback) +{ + // SBG sample 0.1 s old (fresh); FCU just arrived; we already published SBG's + // 99.9 sample, so it carries no new data this tick. + std::vector stamps{t(99.9), t(100.0)}; + EXPECT_EQ(selectNavigationSource(stamps, /*last_value=*/t(99.9), /*now=*/t(100.0), kTimeout), -1); +} + +// A new SBG sample is adopted at index 0. +TEST(NavSourceSelection, NewPrimarySampleSelected) +{ + std::vector stamps{t(100.0), t(99.95)}; + EXPECT_EQ(selectNavigationSource(stamps, t(99.9), t(100.0), kTimeout), 0); +} + +// Primary stale past the timeout -> fail over to the fresh fallback. +TEST(NavSourceSelection, FailoverToFallbackWhenPrimaryStale) +{ + // SBG 1.0 s old (stale, > 0.5 s); FCU just arrived. + std::vector stamps{t(99.0), t(100.0)}; + EXPECT_EQ(selectNavigationSource(stamps, t(99.0), t(100.0), kTimeout), 1); +} + +// Primary recovers: its first fresh sample after a failover is adopted again, +// even though the fallback had been publishing newer stamps meanwhile. +TEST(NavSourceSelection, PrimaryRecoversAfterFailover) +{ + std::vector stamps{t(101.0), t(100.95)}; + EXPECT_EQ(selectNavigationSource(stamps, /*last_value=*/t(100.95), t(101.0), kTimeout), 0); +} + +// Fallback fresh but with no new sample (already published) -> no update. +TEST(NavSourceSelection, StalePrimaryFreshFallbackNoNewData) +{ + std::vector stamps{t(99.0), t(99.95)}; + EXPECT_EQ(selectNavigationSource(stamps, /*last_value=*/t(99.95), t(100.0), kTimeout), -1); +} + +// Everything stale -> nothing selected. +TEST(NavSourceSelection, AllStale) +{ + std::vector stamps{t(99.0), t(99.4)}; + EXPECT_EQ(selectNavigationSource(stamps, t(99.0), t(100.0), kTimeout), -1); +} + +// Boundary: a sample exactly at the timeout age counts as stale (age < timeout +// is the freshness test), so the loop falls through. +TEST(NavSourceSelection, TimeoutBoundaryIsStale) +{ + std::vector stamps{t(99.5)}; // exactly 0.5 s old + EXPECT_EQ(selectNavigationSource(stamps, t(99.0), t(100.0), kTimeout), -1); +} + +// Single fresh source with new data is adopted (default/legacy single-sensor case). +TEST(NavSourceSelection, SingleFreshSource) +{ + std::vector stamps{t(100.0)}; + EXPECT_EQ(selectNavigationSource(stamps, t(99.0), t(100.0), kTimeout), 0); +} + +// No sources configured -> nothing selected. +TEST(NavSourceSelection, EmptyList) +{ + std::vector stamps{}; + EXPECT_EQ(selectNavigationSource(stamps, t(99.0), t(100.0), kTimeout), -1); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}