tests: external navigation interface

examples/cpp/navigation/global_navigation/include/global_navigation.hpp

@@ -34,8 +34,16 @@ class GlobalNavigationTest : public px4_ros2::GlobalPositionMeasurementInterface
     global_position_measurement.altitude_msl = 12.4F;
     global_position_measurement.vertical_variance = 0.2F;
 
-    update(global_position_measurement);
-    RCLCPP_DEBUG(_node.get_logger(), "Successfully sent position update to navigation interface.");
+    try {
+      update(global_position_measurement);
+      RCLCPP_DEBUG(
+        _node.get_logger(),
+        "Successfully sent position update to navigation interface.");
+    } catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) {
+      RCLCPP_ERROR_THROTTLE(
+        _node.get_logger(),
+        *_node.get_clock(), 1000, "Exception caught: %s", e.what());
+    }
   }
 
 private:

examples/cpp/navigation/local_navigation/include/local_navigation.hpp

@@ -38,8 +38,16 @@ class LocalNavigationTest : public px4_ros2::LocalPositionMeasurementInterface
     local_position_measurement.attitude_quaternion = Eigen::Quaternionf {0.1, -0.2, 0.3, 0.25};
     local_position_measurement.attitude_variance = Eigen::Vector3f {0.2, 0.1, 0.05};
 
-    update(local_position_measurement);
-    RCLCPP_DEBUG(_node.get_logger(), "Successfully sent position update to navigation interface.");
+    try {
+      update(local_position_measurement);
+      RCLCPP_DEBUG(
+        _node.get_logger(),
+        "Successfully sent position update to navigation interface.");
+    } catch (const px4_ros2::NavigationInterfaceInvalidArgument & e) {
+      RCLCPP_ERROR_THROTTLE(
+        _node.get_logger(),
+        *_node.get_clock(), 1000, "Exception caught: %s", e.what());
+    }
   }
 
 private:

px4_ros2_cpp/CMakeLists.txt

@@ -98,6 +98,8 @@ if(BUILD_TESTING)
 
     ament_add_gtest(integration_tests
             test/integration/arming_check.cpp
+            test/integration/global_navigation.cpp
+            test/integration/local_navigation.cpp
             test/integration/mode.cpp
             test/integration/mode_executor.cpp
             test/integration/overrides.cpp
@@ -110,6 +112,8 @@ if(BUILD_TESTING)
 
     # Unit tests
     ament_add_gtest(${PROJECT_NAME}_unit_tests
+            test/unit/global_navigation.cpp
+            test/unit/local_navigation.cpp
             test/unit/main.cpp
             test/unit/modes.cpp
     )

px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp

@@ -79,13 +79,15 @@ const
   if (measurement.lat_lon.has_value() &&
     (!measurement.horizontal_variance.has_value() || measurement.horizontal_variance.value() <= 0))
   {
-    RCLCPP_ERROR(_node.get_logger(), "Measurement lat_lon has an invalid variance value.");
+    RCLCPP_ERROR_ONCE(_node.get_logger(), "Measurement lat_lon has an invalid variance value.");
     return false;
   }
   if (measurement.altitude_msl.has_value() &&
     (!measurement.vertical_variance.has_value() || measurement.vertical_variance.value() <= 0))
   {
-    RCLCPP_ERROR(_node.get_logger(), "Measurement altitude_msl has an invalid variance value.");
+    RCLCPP_ERROR_ONCE(
+      _node.get_logger(),
+      "Measurement altitude_msl has an invalid variance value.");
     return false;
   }
 
@@ -103,26 +105,28 @@ bool GlobalPositionMeasurementInterface::isValueNotNAN(
 const
 {
   if (measurement.lat_lon.has_value() && measurement.lat_lon.value().hasNaN()) {
-    RCLCPP_ERROR(_node.get_logger(), "Measurement value lat_lon is defined but contains a NAN.");
+    RCLCPP_ERROR_ONCE(
+      _node.get_logger(),
+      "Measurement value lat_lon is defined but contains a NAN.");
     return false;
   }
   if (measurement.horizontal_variance.has_value() &&
     std::isnan(measurement.horizontal_variance.value()))
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(), "Measurement value horizontal_variance is defined but contains a NAN.");
     return false;
   }
   if (measurement.altitude_msl.has_value() && std::isnan(measurement.altitude_msl.value())) {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(),
       "Measurement value altitude_msl is defined but contains a NAN.");
     return false;
   }
   if (measurement.vertical_variance.has_value() &&
     std::isnan(measurement.vertical_variance.value()))
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(), "Measurement value vertical_variance is defined but contains a NAN.");
     return false;
   }

px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp

@@ -123,7 +123,7 @@ const
     (!measurement.position_xy_variance.has_value() ||
     (measurement.position_xy_variance.value().array() <= 0).any()))
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(),
       "Measurement value position_xy has an invalid variance value.");
     return false;
@@ -132,15 +132,16 @@ const
   if (measurement.position_z.has_value() &&
     (!measurement.position_z_variance.has_value() || measurement.position_z_variance <= 0))
   {
-    RCLCPP_ERROR(_node.get_logger(), "Measurement value position_z has an invalid variance value.");
+    RCLCPP_ERROR_ONCE(
+      _node.get_logger(), "Measurement value position_z has an invalid variance value.");
     return false;
   }
 
   if (measurement.velocity_xy.has_value() &&
     (!measurement.velocity_xy_variance.has_value() ||
     (measurement.velocity_xy_variance.value().array() <= 0).any()))
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(),
       "Measurement value velocity_xy has an invalid variance value.");
     return false;
@@ -149,15 +150,16 @@ const
   if (measurement.velocity_z.has_value() &&
     (!measurement.velocity_z_variance.has_value() || measurement.velocity_z_variance <= 0))
   {
-    RCLCPP_ERROR(_node.get_logger(), "Measurement value velocity_z has an invalid variance value.");
+    RCLCPP_ERROR_ONCE(
+      _node.get_logger(), "Measurement value velocity_z has an invalid variance value.");
     return false;
   }
 
   if (measurement.attitude_quaternion.has_value() &&
     (!measurement.attitude_variance.has_value() ||
     (measurement.attitude_variance.value().array() <= 0).any()))
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(),
       "Measurement value attitude_quaternion has an invalid variance value.");
     return false;
@@ -172,7 +174,7 @@ const
   if ((measurement.position_xy.has_value() || measurement.position_z.has_value()) &&
     _pose_frame == AuxLocalPosition::POSE_FRAME_UNKNOWN)
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(),
       "Position measurement has unknown pose frame.");
     return false;
@@ -181,7 +183,7 @@ const
   if ((measurement.velocity_xy.has_value() || measurement.velocity_z.has_value()) &&
     _velocity_frame == AuxLocalPosition::VELOCITY_FRAME_UNKNOWN)
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(),
       "Velocity measurement has unknown velocity frame.");
     return false;
@@ -194,62 +196,64 @@ bool LocalPositionMeasurementInterface::isValueNotNAN(const LocalPositionMeasure
 const
 {
   if (measurement.position_xy.has_value() && measurement.position_xy.value().hasNaN()) {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(),
       "Measurement value position_xy is defined but contains a NAN.");
     return false;
   }
   if (measurement.position_xy_variance.has_value() &&
     measurement.position_xy_variance.value().hasNaN())
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(), "Measurement value position_xy_variance is defined but contains a NAN.");
     return false;
   }
   if (measurement.position_z.has_value() && std::isnan(measurement.position_z.value())) {
-    RCLCPP_ERROR(_node.get_logger(), "Measurement value position_z is defined but contains a NAN.");
+    RCLCPP_ERROR_ONCE(
+      _node.get_logger(), "Measurement value position_z is defined but contains a NAN.");
     return false;
   }
   if (measurement.position_z_variance.has_value() &&
     std::isnan(measurement.position_z_variance.value()))
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(), "Measurement value position_z_variance is defined but contains a NAN.");
     return false;
   }
   if (measurement.velocity_xy.has_value() && measurement.velocity_xy.value().hasNaN()) {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(),
       "Measurement value velocity_xy is defined but contains a NAN.");
     return false;
   }
   if (measurement.velocity_xy_variance.has_value() &&
     measurement.velocity_xy_variance.value().hasNaN())
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(), "Measurement value velocity_xy_variance is defined but contains a NAN.");
     return false;
   }
   if (measurement.velocity_z.has_value() && std::isnan(measurement.velocity_z.value())) {
-    RCLCPP_ERROR(_node.get_logger(), "Measurement value velocity_z is defined but contains a NAN.");
+    RCLCPP_ERROR_ONCE(
+      _node.get_logger(), "Measurement value velocity_z is defined but contains a NAN.");
     return false;
   }
   if (measurement.velocity_z_variance.has_value() &&
     std::isnan(measurement.velocity_z_variance.value()))
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(), "Measurement value velocity_z_variance is defined but contains a NAN.");
     return false;
   }
   if (measurement.attitude_quaternion.has_value() &&
     measurement.attitude_quaternion.value().coeffs().hasNaN())
   {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(), "Measurement value attitude_quaternion is defined but contains a NAN.");
     return false;
   }
   if (measurement.attitude_variance.has_value() && measurement.attitude_variance.value().hasNaN()) {
-    RCLCPP_ERROR(
+    RCLCPP_ERROR_ONCE(
       _node.get_logger(), "Measurement value attitude_variance is defined but contains a NAN.");
     return false;
   }

px4_ros2_cpp/test/integration/arming_check.cpp

@@ -106,7 +106,7 @@ void TestExecution::run()
 }
 
 
-TEST_F(Tester, denyArming)
+TEST_F(ModesTest, denyArming)
 {
   auto test_node = initNode();
   ASSERT_TRUE(px4_ros2::waitForFMU(*test_node, 10s));

px4_ros2_cpp/test/integration/global_navigation.cpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+/**
+ * Summary:
+ *  Tests that measurements sent via the interface are fused into PX4.
+ * - `GlobalPositionInterfaceTest` fixture:
+ *   - Test Cases:
+ *     1. FuseAll: Sends measurements with all fields set. Expects cs_aux_gpos to be set to true.
+ */
+
+
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/estimator_status_flags.hpp>
+#include <px4_ros2/navigation/experimental/global_position_measurement_interface.hpp>
+#include "util.hpp"
+
+using namespace std::chrono_literals;
+using px4_ros2::GlobalPositionMeasurement, px4_ros2::GlobalPositionMeasurementInterface;
+using px4_msgs::msg::EstimatorStatusFlags;
+
+class GlobalPositionInterfaceTest : public BaseTest
+{
+protected:
+  void SetUp() override
+  {
+    _node = initNode();
+
+    _global_navigation_interface = std::make_shared<GlobalPositionMeasurementInterface>(*_node);
+    ASSERT_TRUE(_global_navigation_interface->doRegister()) <<
+      "Failed to register GlobalPositionMeasurementInterface.";
+
+    // Subscribe to PX4 EKF estimator status flags
+    _subscriber = _node->create_subscription<EstimatorStatusFlags>(
+      "/fmu/out/estimator_status_flags", rclcpp::QoS(10).best_effort(),
+      [this](EstimatorStatusFlags::UniquePtr msg) {
+        _estimator_status_flags = std::move(msg);
+      });
+  }
+
+  void waitUntilFlagsReset()
+  {
+    auto start_time = _node->now();
+
+    // Wait for PX4 to stop fusing external measurements
+    while (_estimator_status_flags->cs_aux_gpos) {
+      rclcpp::spin_some(_node);
+      rclcpp::sleep_for(kSleepInterval);
+
+      const auto elapsed_time = _node->now() - start_time;
+
+      if (elapsed_time >= kTimeoutDuration) {
+        FAIL() << "Timeout while waiting for PX4 estimator flags to reset.";
+        break;
+      }
+    }
+
+    _estimator_status_flags.reset();
+  }
+
+  void waitForMeasurementUpdate(
+    std::unique_ptr<GlobalPositionMeasurement> measurement,
+    const std::function<bool(const EstimatorStatusFlags::UniquePtr &)> & is_fused_getter
+  )
+  {
+    auto start_time = _node->now();
+
+    // Wait for PX4 estimator flags to confirm proper measurement fusion
+    while (!_estimator_status_flags || !is_fused_getter(_estimator_status_flags)) {
+
+      // Send measurement
+      measurement->timestamp_sample = _node->get_clock()->now();
+      ASSERT_NO_THROW(
+        _global_navigation_interface->update(*measurement)
+      ) << "Failed to send position measurement update via GlobalPositionMeasurementInterface.";
+
+      rclcpp::spin_some(_node);
+
+      // Check timeout
+      const auto elapsed_time = _node->now() - start_time;
+
+      if (elapsed_time >= kTimeoutDuration) {
+        ASSERT_NE(_estimator_status_flags, nullptr) <<
+          "Missing feedback from PX4: no estimator status flags published over /fmu/out/estimator_status_flags.";
+        EXPECT_TRUE(is_fused_getter(_estimator_status_flags)) <<
+          "Position measurement update was not fused into the PX4 EKF.";
+        break;
+      }
+
+      rclcpp::sleep_for(kSleepInterval);
+    }
+
+    waitUntilFlagsReset();
+  }
+
+  std::shared_ptr<rclcpp::Node> _node;
+  std::shared_ptr<GlobalPositionMeasurementInterface> _global_navigation_interface;
+  rclcpp::Subscription<EstimatorStatusFlags>::SharedPtr _subscriber;
+  EstimatorStatusFlags::UniquePtr _estimator_status_flags;
+
+  static constexpr std::chrono::seconds kTimeoutDuration = 60s;
+  static constexpr std::chrono::milliseconds kSleepInterval = 50ms;
+};
+
+TEST_F(GlobalPositionInterfaceTest, fuseAll) {
+  auto measurement = std::make_unique<GlobalPositionMeasurement>();
+  measurement->lat_lon = Eigen::Vector2d {12.34567, 23.45678};
+  measurement->horizontal_variance = 0.01F;
+  measurement->altitude_msl = 123.F;
+  measurement->vertical_variance = 0.01F;
+  waitForMeasurementUpdate(
+    std::move(measurement), [](const EstimatorStatusFlags::UniquePtr & flags) {
+      return flags->cs_aux_gpos;
+    });
+}