tests: integration test navigation interface

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

px4_ros2_cpp/test/integration/global_navigation.cpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+/**
+ * Summary:
+ *  Tests that the GlobalPositionMeasurementInterface successfully publishes valid measurements to the expected ROS 2 topic.
+ */
+
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <px4_ros2/navigation/experimental/global_position_measurement_interface.hpp>
+#include "util.hpp"
+
+using px4_ros2::GlobalPositionMeasurement, px4_ros2::GlobalPositionMeasurementInterface;
+
+class GlobalPositionMeasurementInterfaceTest : public Tester
+{
+protected:
+  void SetUp() override
+  {
+    _node = initNode();
+
+    _global_navigation_interface = std::make_shared<GlobalPositionMeasurementInterface>(*_node);
+    ASSERT_TRUE(_global_navigation_interface->doRegister()) <<
+      "Failed to register GlobalPositionMeasurementInterface.";
+
+    _subscriber = _node->create_subscription<px4_msgs::msg::VehicleGlobalPosition>(
+      "/fmu/in/aux_global_position", 10,
+      [this](const px4_msgs::msg::VehicleGlobalPosition::SharedPtr msg) {
+        _received_measurement = msg;
+      });
+  }
+
+  void resetMeasurement()
+  {
+    _received_measurement = nullptr;
+  }
+
+  void sendGlobalPositionMeasurement(
+    std::unique_ptr<GlobalPositionMeasurement> measurement,
+    const std::string & test_subname = "")
+  {
+    ASSERT_NO_THROW(_global_navigation_interface->update(*measurement)) <<
+      "Failed to send position measurement update via GlobalPositionMeasurementInterface.";
+
+    constexpr auto kTimeoutDuration = std::chrono::milliseconds(200);
+    constexpr auto kSleepInterval = std::chrono::milliseconds(5);
+
+    auto start_time = std::chrono::steady_clock::now();
+
+    // Wait for measurement update to be received
+    while (_received_measurement == nullptr) {
+      rclcpp::spin_some(_node);
+
+      auto current_time = std::chrono::steady_clock::now();
+      auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(
+        current_time - start_time);
+
+      if (elapsed_time >= kTimeoutDuration) {
+        EXPECT_NE(_received_measurement, nullptr) <<
+          "Position measurement update " << test_subname << " was not received on ROS 2 topic.";
+        break;
+      }
+
+      std::this_thread::sleep_for(kSleepInterval);
+    }
+
+    resetMeasurement();
+  }
+
+  std::shared_ptr<rclcpp::Node> _node;
+  std::shared_ptr<GlobalPositionMeasurementInterface> _global_navigation_interface;
+  rclcpp::Subscription<px4_msgs::msg::VehicleGlobalPosition>::SharedPtr _subscriber;
+  px4_msgs::msg::VehicleGlobalPosition::SharedPtr _received_measurement;
+};
+
+// Tests that measurements are successfully published to ROS 2
+TEST_F(GlobalPositionMeasurementInterfaceTest, Success) {
+  // Send lat lon and variance
+  auto measurement = std::make_unique<GlobalPositionMeasurement>();
+  measurement->timestamp_sample = _node->get_clock()->now();
+  measurement->lat_lon = Eigen::Vector2d {12.34567, 23.45678};
+  measurement->horizontal_variance = 0.1F;
+  sendGlobalPositionMeasurement(std::move(measurement), "lat_lon");
+
+  // Send altitude and variance
+  measurement = std::make_unique<GlobalPositionMeasurement>();
+  measurement->timestamp_sample = _node->get_clock()->now();
+  measurement->altitude_msl = 123.F;
+  measurement->vertical_variance = 0.3F;
+  sendGlobalPositionMeasurement(std::move(measurement), "altitude");
+
+  // Send lat lon altitude and variances
+  measurement = std::make_unique<GlobalPositionMeasurement>();
+  measurement->timestamp_sample = _node->get_clock()->now();
+  measurement->lat_lon = Eigen::Vector2d {12.34567, 23.45678};
+  measurement->horizontal_variance = 0.1F;
+  measurement->altitude_msl = 123.F;
+  measurement->vertical_variance = 0.3F;
+  sendGlobalPositionMeasurement(std::move(measurement), "all");
+}

px4_ros2_cpp/test/integration/local_navigation.cpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+/**
+ * Summary:
+ *  Tests that the LocalPositionMeasurementInterface successfully publishes valid measurements to the expected ROS 2 topic.
+ */
+
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <px4_ros2/navigation/experimental/local_position_measurement_interface.hpp>
+#include "util.hpp"
+
+using px4_ros2::LocalPositionMeasurement, px4_ros2::LocalPositionMeasurementInterface;
+
+class LocalPositionMeasurementInterfaceTest : public Tester
+{
+protected:
+  void SetUp() override
+  {
+    _node = initNode();
+
+    _local_navigation_interface = std::make_shared<LocalPositionMeasurementInterface>(
+      *_node, px4_ros2::PoseFrame::LocalNED,
+      px4_ros2::VelocityFrame::LocalNED);
+    ASSERT_TRUE(_local_navigation_interface->doRegister()) <<
+      "Failed to register LocalPositionMeasurementInterface.";
+
+    _subscriber = _node->create_subscription<px4_ros2::AuxLocalPosition>(
+      "/fmu/in/vehicle_visual_odometry", 10,
+      [this](const px4_ros2::AuxLocalPosition::SharedPtr msg) {
+        _received_measurement = msg;
+      });
+  }
+
+  void resetMeasurement()
+  {
+    _received_measurement = nullptr;
+  }
+
+  void sendLocalPositionMeasurement(
+    std::unique_ptr<LocalPositionMeasurement> measurement,
+    const std::string & test_subname = "")
+  {
+    ASSERT_NO_THROW(_local_navigation_interface->update(*measurement)) <<
+      "Failed to send position measurement update via LocalPositionMeasurementInterface.";
+
+    constexpr auto kTimeoutDuration = std::chrono::milliseconds(200);
+    constexpr auto kSleepInterval = std::chrono::milliseconds(5);
+
+    auto start_time = std::chrono::steady_clock::now();
+
+    // Wait for measurement update to be received
+    while (_received_measurement == nullptr) {
+      rclcpp::spin_some(_node);
+
+      auto current_time = std::chrono::steady_clock::now();
+      auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(
+        current_time - start_time);
+
+      if (elapsed_time >= kTimeoutDuration) {
+        EXPECT_NE(_received_measurement, nullptr) <<
+          "Position measurement update " << test_subname << " was not received on ROS 2 topic.";
+        break;
+      }
+
+      std::this_thread::sleep_for(kSleepInterval);
+    }
+
+    resetMeasurement();
+  }
+
+  std::shared_ptr<rclcpp::Node> _node;
+  std::shared_ptr<LocalPositionMeasurementInterface> _local_navigation_interface;
+  rclcpp::Subscription<px4_ros2::AuxLocalPosition>::SharedPtr _subscriber;
+  px4_ros2::AuxLocalPosition::SharedPtr _received_measurement;
+};
+
+// Tests that measurements are successfully published to ROS 2
+TEST_F(LocalPositionMeasurementInterfaceTest, Success) {
+  // Send position_xy and variance
+  auto measurement = std::make_unique<LocalPositionMeasurement>();
+  measurement->timestamp_sample = _node->get_clock()->now();
+  measurement->position_xy = Eigen::Vector2f {1.F, 2.F};
+  measurement->position_xy_variance = Eigen::Vector2f {0.2F, 0.1F};
+  sendLocalPositionMeasurement(std::move(measurement), "position_xy");
+
+  // Send position_z and variance
+  measurement = std::make_unique<LocalPositionMeasurement>();
+  measurement->timestamp_sample = _node->get_clock()->now();
+  measurement->position_z = 12.3F;
+  measurement->position_z_variance = 0.33F;
+  sendLocalPositionMeasurement(std::move(measurement), "position_z");
+
+  // Send velocity_xy and variance
+  measurement = std::make_unique<LocalPositionMeasurement>();
+  measurement->timestamp_sample = _node->get_clock()->now();
+  measurement->velocity_xy = Eigen::Vector2f {1.F, 2.F};
+  measurement->velocity_xy_variance = Eigen::Vector2f {0.3F, 0.4F};
+  sendLocalPositionMeasurement(std::move(measurement), "velocity_xy");
+
+  // Send velocity_z and variance
+  measurement = std::make_unique<LocalPositionMeasurement>();
+  measurement->timestamp_sample = _node->get_clock()->now();
+  measurement->velocity_z = 12.3F;
+  measurement->velocity_z_variance = 0.33F;
+  sendLocalPositionMeasurement(std::move(measurement), "velocity_z");
+
+  // Send attitude and variance
+  measurement = std::make_unique<LocalPositionMeasurement>();
+  measurement->timestamp_sample = _node->get_clock()->now();
+  measurement->attitude_quaternion = Eigen::Quaternionf {0.1F, -0.2F, 0.3F, 0.25F};
+  measurement->attitude_variance = Eigen::Vector3f {0.2F, 0.1F, 0.05F};
+  _local_navigation_interface->update(*measurement);
+  sendLocalPositionMeasurement(std::move(measurement), "attitude");
+
+  // Send all measurements
+  measurement = std::make_unique<LocalPositionMeasurement>();
+  measurement->timestamp_sample = _node->get_clock()->now();
+  measurement->position_xy = Eigen::Vector2f {1.F, 2.F};
+  measurement->position_xy_variance = Eigen::Vector2f {0.2F, 0.1F};
+  measurement->position_z = 12.3F;
+  measurement->position_z_variance = 0.33F;
+  measurement->velocity_xy = Eigen::Vector2f {1.F, 2.F};
+  measurement->velocity_xy_variance = Eigen::Vector2f {0.3F, 0.4F};
+  measurement->velocity_z = 12.3F;
+  measurement->velocity_z_variance = 0.33F;
+  measurement->attitude_quaternion = Eigen::Quaternionf {0.1F, -0.2F, 0.3F, 0.25F};
+  measurement->attitude_variance = Eigen::Vector3f {0.2F, 0.1F, 0.05F};
+  sendLocalPositionMeasurement(std::move(measurement), "all");
+}