Field import: mru_transform (2026-06-27)

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()

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<typename T, typename SensorVectorT> 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<rclcpp::Time> 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);

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 <cstddef>
+#include <vector>
+
+#include <rclcpp/time.hpp>
+#include <rclcpp/duration.hpp>
+
+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<rclcpp::Time> &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<int>(i) : -1;
+    }
+    // Stale: fall through to the next-priority source.
+  }
+  return -1;
+}
+
+} // namespace mru_transform
+
+#endif

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 <vector>
+
+#include <gtest/gtest.h>
+#include <rclcpp/time.hpp>
+#include <rclcpp/duration.hpp>
+
+#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<int64_t>(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<rclcpp::Time> 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<rclcpp::Time> 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<rclcpp::Time> 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<rclcpp::Time> 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<rclcpp::Time> 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<rclcpp::Time> 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<rclcpp::Time> 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<rclcpp::Time> 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<rclcpp::Time> 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();
+}