From 4a7cb9c89411365cde93a9f08521bd4e3bf79925 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Sun, 17 Dec 2023 17:54:11 +0100 Subject: [PATCH] added max height check test --- src/uav_manager.cpp | 2 +- test/uav_manager/CMakeLists.txt | 2 + .../max_height_check/CMakeLists.txt | 16 ++++ .../config/custom_config.yaml | 24 ++++++ .../max_height_check/config/world_config.yaml | 34 ++++++++ .../max_height_check/max_height_check.test | 37 +++++++++ test/uav_manager/max_height_check/test.cpp | 83 +++++++++++++++++++ 7 files changed, 197 insertions(+), 1 deletion(-) create mode 100644 test/uav_manager/max_height_check/CMakeLists.txt create mode 100644 test/uav_manager/max_height_check/config/custom_config.yaml create mode 100644 test/uav_manager/max_height_check/config/world_config.yaml create mode 100644 test/uav_manager/max_height_check/max_height_check.test create mode 100644 test/uav_manager/max_height_check/test.cpp diff --git a/src/uav_manager.cpp b/src/uav_manager.cpp index 6cfb7dc4..3dd9bd61 100644 --- a/src/uav_manager.cpp +++ b/src/uav_manager.cpp @@ -880,7 +880,7 @@ void UavManager::timerMaxHeight(const ros::TimerEvent& event) { auto control_manager_diag = sh_control_manager_diag_.getMsg(); - if (!fixing_min_height_ && !control_manager_diag->flying_normally) { + if (!fixing_max_height_ && !control_manager_diag->flying_normally) { return; } diff --git a/test/uav_manager/CMakeLists.txt b/test/uav_manager/CMakeLists.txt index 6402fc4d..0696b399 100644 --- a/test/uav_manager/CMakeLists.txt +++ b/test/uav_manager/CMakeLists.txt @@ -5,3 +5,5 @@ add_subdirectory(landing) add_subdirectory(land_home) add_subdirectory(min_height_check) + +add_subdirectory(max_height_check) diff --git a/test/uav_manager/max_height_check/CMakeLists.txt b/test/uav_manager/max_height_check/CMakeLists.txt new file mode 100644 index 00000000..bf8f9853 --- /dev/null +++ b/test/uav_manager/max_height_check/CMakeLists.txt @@ -0,0 +1,16 @@ +get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) + +catkin_add_executable_with_gtest(test_${TEST_NAME} + test.cpp + ) + +target_link_libraries(test_${TEST_NAME} + ${catkin_LIBRARIES} + ) + +add_dependencies(test_${TEST_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +add_rostest(${TEST_NAME}.test) diff --git a/test/uav_manager/max_height_check/config/custom_config.yaml b/test/uav_manager/max_height_check/config/custom_config.yaml new file mode 100644 index 00000000..94c1f818 --- /dev/null +++ b/test/uav_manager/max_height_check/config/custom_config.yaml @@ -0,0 +1,24 @@ +# GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: +## -------------------------------------------------------------- +## | rosrun mrs_uav_core get_public_params.py # +## -------------------------------------------------------------- + +mrs_uav_managers: + + estimation_manager: + + # loaded state estimator plugins + state_estimators: [ + "gps_garmin", + ] + + initial_state_estimator: "gps_garmin" # will be used as the first state estimator + agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) + + uav_manager: + + max_height_checking: + + enabled: true + rate: 10.0 # [Hz] + safety_height_offset: 0.25 # how much lower to descend below the max height diff --git a/test/uav_manager/max_height_check/config/world_config.yaml b/test/uav_manager/max_height_check/config/world_config.yaml new file mode 100644 index 00000000..00e01eef --- /dev/null +++ b/test/uav_manager/max_height_check/config/world_config.yaml @@ -0,0 +1,34 @@ +world_origin: + + units: "LATLON" # {"UTM, "LATLON"} + + origin_x: 47.397743 + origin_y: 8.545594 + +safety_area: + + enabled: false + + horizontal: + + # the frame of reference in which the points are expressed + frame_name: "world_origin" + + # polygon + # + # x, y [m] for any frame_name except latlon_origin + # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" + points: [ + -50, -50, + 50, -50, + 50, 50, + -50, 50, + ] + + vertical: + + # the frame of reference in which the max&min z is expressed + frame_name: "world_origin" + + max_z: 15.0 + min_z: 0.5 diff --git a/test/uav_manager/max_height_check/max_height_check.test b/test/uav_manager/max_height_check/max_height_check.test new file mode 100644 index 00000000..36898b00 --- /dev/null +++ b/test/uav_manager/max_height_check/max_height_check.test @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/uav_manager/max_height_check/test.cpp b/test/uav_manager/max_height_check/test.cpp new file mode 100644 index 00000000..74bf4230 --- /dev/null +++ b/test/uav_manager/max_height_check/test.cpp @@ -0,0 +1,83 @@ +#include + +#include + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); +}; + +bool Tester::test() { + + { + auto [success, message] = activateMidAir(); + + if (!success) { + ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | --------------- goto to violate min height --------------- | + + { + auto [success, message] = this->gotoAbs(0, 0, 100, 0); + + if (success) { + ROS_ERROR("[%s]: goto should fail", ros::this_node::getName().c_str()); + return false; + } + } + + // | --------- wait till we are flying normally again --------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (this->isFlyingNormally()) { + break; + } + } + + // | ------------------- check the altitude ------------------- | + + auto height = this->getHeightAgl(); + + // TODO substitude with subscribed max agl height + double max_height_agl = 40; + + if (height) { + if (height.value() < max_height_agl) { + return true; + } + } + + return false; +} + + +TEST(TESTSuite, test) { + + Tester tester; + + bool result = tester.test(); + + if (result) { + GTEST_SUCCEED(); + } else { + GTEST_FAIL(); + } +} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { + + ros::init(argc, argv, "test"); + + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +}