diff --git a/examples/cpp/navigation/global_navigation/include/global_navigation.hpp b/examples/cpp/navigation/global_navigation/include/global_navigation.hpp index e7f7444..17b8213 100644 --- a/examples/cpp/navigation/global_navigation/include/global_navigation.hpp +++ b/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: diff --git a/examples/cpp/navigation/local_navigation/include/local_navigation.hpp b/examples/cpp/navigation/local_navigation/include/local_navigation.hpp index 2231d29..85c2289 100644 --- a/examples/cpp/navigation/local_navigation/include/local_navigation.hpp +++ b/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: diff --git a/px4_ros2_cpp/CMakeLists.txt b/px4_ros2_cpp/CMakeLists.txt index ac46bda..59570e2 100644 --- a/px4_ros2_cpp/CMakeLists.txt +++ b/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 ) diff --git a/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp index b769d80..8c175d7 100644 --- a/px4_ros2_cpp/src/navigation/experimental/global_position_measurement_interface.cpp +++ b/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,18 +105,20 @@ 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; @@ -122,7 +126,7 @@ const 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; } diff --git a/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp index 78395ed..c63011d 100644 --- a/px4_ros2_cpp/src/navigation/experimental/local_position_measurement_interface.cpp +++ b/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,7 +132,8 @@ 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; } @@ -140,7 +141,7 @@ const (!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,7 +150,8 @@ 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; } @@ -157,7 +159,7 @@ const (!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,7 +196,7 @@ 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; @@ -202,23 +204,24 @@ const 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; @@ -226,30 +229,31 @@ const 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; } diff --git a/px4_ros2_cpp/test/integration/arming_check.cpp b/px4_ros2_cpp/test/integration/arming_check.cpp index e84c27b..ee0de36 100644 --- a/px4_ros2_cpp/test/integration/arming_check.cpp +++ b/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)); diff --git a/px4_ros2_cpp/test/integration/global_navigation.cpp b/px4_ros2_cpp/test/integration/global_navigation.cpp new file mode 100644 index 0000000..745f3ba --- /dev/null +++ b/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 +#include +#include +#include +#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(*_node); + ASSERT_TRUE(_global_navigation_interface->doRegister()) << + "Failed to register GlobalPositionMeasurementInterface."; + + // Subscribe to PX4 EKF estimator status flags + _subscriber = _node->create_subscription( + "/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 measurement, + const std::function & 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 _node; + std::shared_ptr _global_navigation_interface; + rclcpp::Subscription::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(); + 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; + }); +} diff --git a/px4_ros2_cpp/test/integration/local_navigation.cpp b/px4_ros2_cpp/test/integration/local_navigation.cpp new file mode 100644 index 0000000..d0f888b --- /dev/null +++ b/px4_ros2_cpp/test/integration/local_navigation.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +/** + * Summary: + * Tests that measurements sent via the interface are fused into PX4. + * - `LocalPositionInterfaceTest` fixture: + * - Set up: LocalPositionMeasurementInterface instance is configured with PoseFrame::LocalNED and VelocityFrame::LocalNED. + * - Test Cases: + * 1. FuseEvPos: Sends position measurements, expects cs_ev_pos and cs_ev_hgt flags to be set to true. + * 2. FuseEvVel: Sends velocity measurements, expects cs_ev_vel flag to be set to true. + * 3. FuseEvYaw: Sends attitude measurements, expects cs_ev_yaw flag to be set to true. + * 4. FuseAll: Sends measurements with all fields set. Expect all flags above to be true. + */ + +#include +#include +#include +#include +#include "util.hpp" + +using namespace std::chrono_literals; +using px4_ros2::LocalPositionMeasurement, px4_ros2::LocalPositionMeasurementInterface; +using px4_msgs::msg::EstimatorStatusFlags; + +class LocalPositionInterfaceTest : public BaseTest +{ +protected: + void SetUp() override + { + _node = initNode(); + + _local_navigation_interface = std::make_shared( + *_node, px4_ros2::PoseFrame::LocalNED, + px4_ros2::VelocityFrame::LocalNED); + ASSERT_TRUE(_local_navigation_interface->doRegister()) << + "Failed to register LocalPositionMeasurementInterface."; + + // Subscribe to PX4 EKF estimator status flags + _subscriber = _node->create_subscription( + "/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_ev_pos || _estimator_status_flags->cs_ev_hgt || + _estimator_status_flags->cs_ev_vel || _estimator_status_flags->cs_ev_yaw) + { + 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 measurement, + const std::function & 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( + _local_navigation_interface->update(*measurement) + ) << "Failed to send position measurement update via LocalPositionMeasurementInterface."; + + 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 _node; + std::shared_ptr _local_navigation_interface; + rclcpp::Subscription::SharedPtr _subscriber; + EstimatorStatusFlags::UniquePtr _estimator_status_flags; + + static constexpr std::chrono::seconds kTimeoutDuration = 60s; + static constexpr std::chrono::milliseconds kSleepInterval = 50ms; +}; + +TEST_F(LocalPositionInterfaceTest, fuseEvPos) { + auto measurement = std::make_unique(); + measurement->position_xy = Eigen::Vector2f {0.F, 0.F}; + measurement->position_xy_variance = Eigen::Vector2f {0.1F, 0.1F}; + measurement->position_z = 0.F; + measurement->position_z_variance = 0.01F; + + waitForMeasurementUpdate( + std::move(measurement), [](const EstimatorStatusFlags::UniquePtr & flags) { + return flags->cs_ev_pos && flags->cs_ev_hgt; + }); +} + +TEST_F(LocalPositionInterfaceTest, fuseEvVel) { + auto measurement = std::make_unique(); + measurement->velocity_xy = Eigen::Vector2f {0.F, 0.F}; + measurement->velocity_xy_variance = Eigen::Vector2f {0.1F, 0.1F}; + measurement->velocity_z = 0.0F; + measurement->velocity_z_variance = 0.1F; + + waitForMeasurementUpdate( + std::move(measurement), [](const EstimatorStatusFlags::UniquePtr & flags) { + return flags->cs_ev_vel; + }); +} + +TEST_F(LocalPositionInterfaceTest, fuseEvYaw) { + auto measurement = std::make_unique(); + measurement->attitude_quaternion = Eigen::Quaternionf {1.0F, 0.0F, 0.0F, 0.0F}; + measurement->attitude_variance = Eigen::Vector3f {0.1F, 0.1F, 0.1F}; + + waitForMeasurementUpdate( + std::move(measurement), [](const EstimatorStatusFlags::UniquePtr & flags) { + return flags->cs_ev_yaw; + }); +} + +TEST_F(LocalPositionInterfaceTest, fuseAll) { + auto measurement = std::make_unique(); + measurement->position_xy = Eigen::Vector2f {0.F, 0.F}; + measurement->position_xy_variance = Eigen::Vector2f {0.1F, 0.1F}; + measurement->position_z = 0.F; + measurement->position_z_variance = 0.1F; + measurement->velocity_xy = Eigen::Vector2f {0.F, 0.F}; + measurement->velocity_xy_variance = Eigen::Vector2f {0.1F, 0.1F}; + measurement->velocity_z = 0.0F; + measurement->velocity_z_variance = 0.1F; + measurement->attitude_quaternion = Eigen::Quaternionf {1.0F, 0.0F, 0.0F, 0.0F}; + measurement->attitude_variance = Eigen::Vector3f {0.1F, 0.1F, 0.1F}; + + waitForMeasurementUpdate( + std::move(measurement), [](const EstimatorStatusFlags::UniquePtr & flags) { + return flags->cs_ev_pos && flags->cs_ev_hgt && flags->cs_ev_vel && flags->cs_ev_yaw; + }); +} diff --git a/px4_ros2_cpp/test/integration/mode.cpp b/px4_ros2_cpp/test/integration/mode.cpp index 9d7bc27..b2c7928 100644 --- a/px4_ros2_cpp/test/integration/mode.cpp +++ b/px4_ros2_cpp/test/integration/mode.cpp @@ -271,7 +271,7 @@ void TestExecution::run() }); } -TEST_F(Tester, runModeTests) +TEST_F(ModesTest, runModeTests) { auto test_node = initNode(); ASSERT_TRUE(px4_ros2::waitForFMU(*test_node, 10s)); diff --git a/px4_ros2_cpp/test/integration/mode_executor.cpp b/px4_ros2_cpp/test/integration/mode_executor.cpp index f9f5bcf..a6feea0 100644 --- a/px4_ros2_cpp/test/integration/mode_executor.cpp +++ b/px4_ros2_cpp/test/integration/mode_executor.cpp @@ -214,7 +214,7 @@ void TestExecutionAutonomous::run() ASSERT_TRUE(_mode_executor->doRegister()); } -TEST_F(Tester, runExecutorAutonomous) +TEST_F(ModesTest, runExecutorAutonomous) { auto test_node = initNode(); ASSERT_TRUE(px4_ros2::waitForFMU(*test_node, 10s)); @@ -385,7 +385,7 @@ void TestExecutionInCharge::run() ASSERT_TRUE(_mode_executor->doRegister()); } -TEST_F(Tester, runExecutorInCharge) +TEST_F(ModesTest, runExecutorInCharge) { auto test_node = initNode(); ASSERT_TRUE(px4_ros2::waitForFMU(*test_node, 10s)); @@ -488,7 +488,7 @@ void TestExecutionFailsafe::run() ASSERT_TRUE(_mode_executor->doRegister()); } -TEST_F(Tester, runExecutorFailsafe) +TEST_F(ModesTest, runExecutorFailsafe) { auto test_node = initNode(); ASSERT_TRUE(px4_ros2::waitForFMU(*test_node, 10s)); diff --git a/px4_ros2_cpp/test/integration/overrides.cpp b/px4_ros2_cpp/test/integration/overrides.cpp index 485c924..a2e9685 100644 --- a/px4_ros2_cpp/test/integration/overrides.cpp +++ b/px4_ros2_cpp/test/integration/overrides.cpp @@ -268,7 +268,7 @@ void TestExecutionOverrides::run() ASSERT_TRUE(_mode_executor->doRegister()); } -TEST_F(Tester, runExecutorOverrides) +TEST_F(ModesTest, runExecutorOverrides) { auto test_node = initNode(); ASSERT_TRUE(px4_ros2::waitForFMU(*test_node, 10s)); diff --git a/px4_ros2_cpp/test/integration/util.hpp b/px4_ros2_cpp/test/integration/util.hpp index 0880134..74bbd0e 100644 --- a/px4_ros2_cpp/test/integration/util.hpp +++ b/px4_ros2_cpp/test/integration/util.hpp @@ -13,7 +13,7 @@ std::shared_ptr initNode(); -class Tester : public ::testing::Test +class BaseTest : public ::testing::Test { public: static void SetUpTestSuite() @@ -26,6 +26,10 @@ class Tester : public ::testing::Test } }; +class ModesTest : public BaseTest +{ +}; + class VehicleState { public: diff --git a/px4_ros2_cpp/test/unit/global_navigation.cpp b/px4_ros2_cpp/test/unit/global_navigation.cpp new file mode 100644 index 0000000..792cd5a --- /dev/null +++ b/px4_ros2_cpp/test/unit/global_navigation.cpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +/** + * Summary: + * - `GlobalPositionInterfaceTest` fixture: + * - Test Cases: + * 1. MeasurementEmpty: Sends a measurement with only a timestamp. Expects exception. + * 2. TimestampMissing: Sends a measurement with missing timestamp. Expects exception. + * 3. VarianceInvalid: Sends measurements with missing variance fields. Expects exception. + * 4. ContainsNAN: Sends measurements with NaN values. Expects exception. + */ + +#include +#include +#include + +using px4_ros2::GlobalPositionMeasurement, px4_ros2::NavigationInterfaceInvalidArgument; + + +class GlobalPositionInterfaceTest : public testing::Test +{ +protected: + void SetUp() override + { + _node = std::make_shared("test_node"); + + // Suppress logging for exception handling tests + auto ret = rcutils_logging_set_logger_level( + _node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL); + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR( + _node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + + _global_navigation_interface = std::make_shared( + *_node); + } + + std::shared_ptr _node; + std::shared_ptr _global_navigation_interface; +}; + +// Tests interface when position measurement is empty +TEST_F(GlobalPositionInterfaceTest, MeasurementEmpty) { + GlobalPositionMeasurement measurement{}; + + // Send measurement with only a timestamp + measurement.timestamp_sample = _node->get_clock()->now(); + EXPECT_THROW( + _global_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for empty measurement, but none was thrown."; +} + +// Tests interface when measurement timestamp is missing +TEST_F(GlobalPositionInterfaceTest, TimestampMissing) { + GlobalPositionMeasurement measurement{}; + + measurement.lat_lon = Eigen::Vector2d {12.34567, 23.45678}; + measurement.horizontal_variance = 0.1F; + EXPECT_THROW( + _global_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for missing timestamp, but none was thrown."; +} + +// Tests interface when a variance is missing +TEST_F(GlobalPositionInterfaceTest, VarianceInvalid) { + GlobalPositionMeasurement measurement{}; + + // Send lat lon without variance + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.lat_lon = Eigen::Vector2d {12.34567, 23.45678}; + EXPECT_THROW( + _global_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for missing variance (lat_lon), but none was thrown."; + + // Send altitude without variance + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.altitude_msl = 123.F; + EXPECT_THROW( + _global_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for missing variance (altitude), but none was thrown."; +} + +// Tests interface when measurement contains NAN +TEST_F(GlobalPositionInterfaceTest, ContainsNAN) { + GlobalPositionMeasurement measurement{}; + + // Send lat lon with NAN + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.lat_lon = Eigen::Vector2d {NAN, 23.45678}; + measurement.horizontal_variance = 0.1F; + EXPECT_THROW( + _global_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (lat_lon), but none was thrown."; + + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.lat_lon = Eigen::Vector2d {12.34567, 23.45678}; + measurement.horizontal_variance = NAN; + EXPECT_THROW( + _global_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (horizontal_variance), but none was thrown."; + + // Send altitude with NAN + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.altitude_msl = NAN; + measurement.vertical_variance = 0.1F; + EXPECT_THROW( + _global_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (altitude), but none was thrown."; + + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.altitude_msl = 123.F; + measurement.vertical_variance = NAN; + EXPECT_THROW( + _global_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (vertical_variance), but none was thrown."; +} diff --git a/px4_ros2_cpp/test/unit/local_navigation.cpp b/px4_ros2_cpp/test/unit/local_navigation.cpp new file mode 100644 index 0000000..32811fa --- /dev/null +++ b/px4_ros2_cpp/test/unit/local_navigation.cpp @@ -0,0 +1,358 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +/** + * Summary: + * - `LocalPositionInterfaceTest` fixture: + * - Set up: LocalPositionMeasurementInterface instance is configured with PoseFrame::LocalNED and VelocityFrame::LocalNED. + * - Test Cases: + * 1. MeasurementEmpty: Sends a measurement with only a timestamp. Expects exception. + * 2. TimestampMissing: Sends a measurement with missing timestamp. Expects exception. + * 3. VarianceInvalid: Sends measurements with missing variance fields. Expects exception. + * 4. ContainsNAN: Sends measurements with NaN values. Expects exception. + * + * - `LocalPositionInterfacePoseTest` fixture: + * - Set up: LocalPositionMeasurementInterface instance is configured with PoseFrame::LocalNED and VelocityFrame::Unknown. + * - Test Cases: + * 1. PoseFrameUnknown: Sends a measurement with a position, and a measurement with a velocity. + * Expects success for position measurements, exception for velocity measurements. + * + * - `LocalPositionInterfaceVelocityTest` fixture: + * - Set up: LocalPositionMeasurementInterface instance is configured with PoseFrame::Unknown and VelocityFrame::LocalNED. + * - Test Cases: + * 1. VelocityFrameUnknown: Sends a measurement with a position, and a measurement with a velocity. + * Expects success for velocity measurements, exception for position measurements. + */ + +#include +#include +#include + +using px4_ros2::LocalPositionMeasurement, px4_ros2::NavigationInterfaceInvalidArgument; + +class LocalPositionInterfaceTestBase : public testing::Test +{ +protected: + void SetUp() override + { + _node = std::make_shared("test_node"); + + // Suppress logging for exception handling tests + auto ret = rcutils_logging_set_logger_level( + _node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL); + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR( + _node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + + std::shared_ptr _node; + std::shared_ptr _local_navigation_interface; +}; + +// Configure interface with PoseFrame::LocalNED and VelocityFrame::LocalNED +class LocalPositionInterfaceTest : public LocalPositionInterfaceTestBase +{ +protected: + void SetUp() override + { + LocalPositionInterfaceTestBase::SetUp(); + _local_navigation_interface = std::make_shared( + *_node, + px4_ros2::PoseFrame::LocalNED, + px4_ros2::VelocityFrame::LocalNED); + } +}; + +// Configure interface with PoseFrame::LocalNED and VelocityFrame::Unknown +class LocalPositionInterfacePoseTest : public LocalPositionInterfaceTestBase +{ +protected: + void SetUp() override + { + LocalPositionInterfaceTestBase::SetUp(); + _local_navigation_interface = std::make_shared( + *_node, + px4_ros2::PoseFrame::LocalNED, + px4_ros2::VelocityFrame::Unknown); + } +}; + +// Configure interface with PoseFrame::Unknown and VelocityFrame::LocalNED +class LocalPositionInterfaceVelocityTest : public + LocalPositionInterfaceTestBase +{ +protected: + void SetUp() override + { + LocalPositionInterfaceTestBase::SetUp(); + _local_navigation_interface = std::make_shared( + *_node, + px4_ros2::PoseFrame::Unknown, + px4_ros2::VelocityFrame::LocalNED); + } +}; + +// Tests interface when position measurement is empty +TEST_F(LocalPositionInterfaceTest, MeasurementEmpty) { + LocalPositionMeasurement measurement{}; + + // Send measurement with only a timestamp + measurement.timestamp_sample = _node->get_clock()->now(); + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for empty measurement, but none was thrown."; +} + +// Tests interface when measurement timestamp is missing +TEST_F(LocalPositionInterfaceTest, TimestampMissing) { + LocalPositionMeasurement measurement{}; + + measurement.position_xy = Eigen::Vector2f {1.F, 2.F}; + measurement.position_xy_variance = Eigen::Vector2f {0.2F, 0.1F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for missing timestamp, but none was thrown."; +} + +// Tests interface when a variance is missing +TEST_F(LocalPositionInterfaceTest, VarianceInvalid) { + LocalPositionMeasurement measurement{}; + + // Send position_xy without variance + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.position_xy = Eigen::Vector2f {1.F, 2.F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for missing variance (position_xy), but none was thrown."; + + // Send position_z without variance + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.position_z = 12.3F; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for missing variance (position_z), but none was thrown."; + + // Send velocity_xy without variance + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.velocity_xy = Eigen::Vector2f {1.F, 2.F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for missing variance (velocity_xy), but none was thrown."; + + // Send velocity_z without variance + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.velocity_z = 12.3F; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for missing variance (velocity_z), but none was thrown."; + + // Send attitude without variance + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.attitude_quaternion = Eigen::Quaternionf {0.1F, -0.2F, 0.3F, 0.25F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for missing variance (attitude), but none was thrown."; +} + +// Tests interface when measurement contains NAN +TEST_F(LocalPositionInterfaceTest, ContainsNAN) { + LocalPositionMeasurement measurement{}; + + // Send position_xy with NAN + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.position_xy = Eigen::Vector2f {NAN, 2.F}; + measurement.position_xy_variance = Eigen::Vector2f {0.2F, 0.1F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (position_xy), but none was thrown."; + + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.position_xy = Eigen::Vector2f {1.F, 2.F}; + measurement.position_xy_variance = Eigen::Vector2f {NAN, 0.1F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (position_xy_variance), but none was thrown."; + + // Send position_z with NAN + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.position_z = NAN; + measurement.position_z_variance = 0.33F; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (position_z), but none was thrown."; + + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.position_z = 12.3F; + measurement.position_z_variance = NAN; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (position_z_variance), but none was thrown."; + + // Send velocity_xy with NAN + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.velocity_xy = Eigen::Vector2f {NAN, 2.F}; + measurement.velocity_xy_variance = Eigen::Vector2f {0.3F, 0.4F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (velocity_xy), but none was thrown."; + + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.velocity_xy = Eigen::Vector2f {1.F, 2.F}; + measurement.velocity_xy_variance = Eigen::Vector2f {NAN, 0.4F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (velocity_xy_variance), but none was thrown."; + + // Send velocity_z with NAN + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.velocity_z = NAN; + measurement.velocity_z_variance = 0.33F; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (velocity_z), but none was thrown."; + + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.velocity_z = 12.3F; + measurement.velocity_z_variance = NAN; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (velocity_z_variance), but none was thrown."; + + // Send attitude with NAN + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.attitude_quaternion = Eigen::Quaternionf {NAN, -0.2F, 0.3F, 0.25F}; + measurement.attitude_variance = Eigen::Vector3f {0.2F, 0.1F, 0.05F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (attitude_quaternion), but none was thrown."; + + measurement = {}; + 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 {NAN, 0.1F, 0.05F}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for NAN value (attitude_variance), but none was thrown."; +} + +// Tests interface when velocity reference frame is unknown +TEST_F(LocalPositionInterfacePoseTest, PoseFrameUnknown) { + LocalPositionMeasurement measurement{}; + + // Send position_xy and variance + // Expects success + 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}; + EXPECT_NO_THROW(_local_navigation_interface->update(measurement)) << + "Failed to send position update (position_xy) with known pose frame and unknown velocity frame."; + + // Send position_z and variance + // Expects success + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.position_z = 12.3F; + measurement.position_z_variance = 0.33F; + EXPECT_NO_THROW(_local_navigation_interface->update(measurement)) << + "Failed to send position update (position_z) with known pose frame and unknown velocity frame."; + + // Send velocity_xy and variance + // Expects exception + measurement = {}; + 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}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for velocity update (velocity_xy) in unknown velocity frame, but none was thrown."; + + // Send velocity_z and variance + // Expects exception + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.velocity_z = 12.3F; + measurement.velocity_z_variance = 0.33F; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for velocity update (velocity_z) in unknown velocity frame, but none was thrown."; +} + +// Tests interface when pose reference frame is unknown +TEST_F(LocalPositionInterfaceVelocityTest, VelocityFrameUnknown) { + LocalPositionMeasurement measurement{}; + + // Send position_xy and variance + // Expects exception + 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}; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for position update (position_xy) in unknown pose frame, but none was thrown."; + + // Send position_z and variance + // Expects exception + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.position_z = 12.3F; + measurement.position_z_variance = 0.33F; + EXPECT_THROW( + _local_navigation_interface->update(measurement), + NavigationInterfaceInvalidArgument) << + "Expected exception for position update (position_z) in unknown pose frame, but none was thrown."; + + // Send velocity_xy and variance + // Expects success + measurement = {}; + 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}; + EXPECT_NO_THROW(_local_navigation_interface->update(measurement)) << + "Failed to send velocity update (velocity_xy) with known velocity frame and unknown pose frame."; + + // Send velocity_z and variance + // Expects success + measurement = {}; + measurement.timestamp_sample = _node->get_clock()->now(); + measurement.velocity_z = 12.3F; + measurement.velocity_z_variance = 0.33F; + EXPECT_NO_THROW(_local_navigation_interface->update(measurement)) << + "Failed to send velocity update (velocity_z) with known velocity frame and unknown pose frame."; +}