diff --git a/test/control_manager/CMakeLists.txt b/test/control_manager/CMakeLists.txt index cfcfeef..dc4bcb1 100644 --- a/test/control_manager/CMakeLists.txt +++ b/test/control_manager/CMakeLists.txt @@ -7,3 +7,5 @@ add_subdirectory(eland_innovation) add_subdirectory(failsafe_control_error) add_subdirectory(eland_service) + +add_subdirectory(escalating_failsafe_service) diff --git a/test/control_manager/escalating_failsafe_service/CMakeLists.txt b/test/control_manager/escalating_failsafe_service/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/control_manager/escalating_failsafe_service/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/control_manager/escalating_failsafe_service/config/custom_config.yaml b/test/control_manager/escalating_failsafe_service/config/custom_config.yaml new file mode 100644 index 0000000..3d6eb11 --- /dev/null +++ b/test/control_manager/escalating_failsafe_service/config/custom_config.yaml @@ -0,0 +1,29 @@ +# 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_baro", + ] + + initial_state_estimator: "gps_baro" # will be used as the first state estimator + agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback) + + control_manager: + + safety: + + escalating_failsafe: + + # how often does it allow to escalate + timeout: 2.0 # [s] + + ehover: true + eland: true + failsafe: true diff --git a/test/control_manager/escalating_failsafe_service/escalating_failsafe_service.test b/test/control_manager/escalating_failsafe_service/escalating_failsafe_service.test new file mode 100644 index 0000000..c17dfb0 --- /dev/null +++ b/test/control_manager/escalating_failsafe_service/escalating_failsafe_service.test @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/escalating_failsafe_service/test.cpp b/test/control_manager/escalating_failsafe_service/test.cpp new file mode 100644 index 0000000..87d461b --- /dev/null +++ b/test/control_manager/escalating_failsafe_service/test.cpp @@ -0,0 +1,223 @@ +#include + +#include + +/* class Tester //{ */ + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); + + Tester(); + + mrs_lib::ServiceClientHandler sch_esc_failsafe_; + + std::tuple escalatingFailsafe(); +}; + +Tester::Tester() { + + sch_esc_failsafe_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/control_manager/failsafe_escalating"); +} + +std::tuple Tester::escalatingFailsafe() { + + { + std_srvs::Trigger srv; + + { + bool service_call = sch_esc_failsafe_.call(srv); + + if (!service_call || !srv.response.success) { + return {false, "escalating failsafe service call failed"}; + } + } + } + + return {true, "service called"}; +} + +//} + +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 higher ---------------------- | + + { + auto [success, message] = this->gotoAbs(0, 0, 10.0, 0); + + if (!success) { + ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | ------------------- trigger failsage #1 ------------------ | + + { + auto [success, message] = escalatingFailsafe(); + + if (!success) { + ROS_ERROR("[%s]: escalating failsafe call #1 failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + sleep(0.5); + + // | ----------- try again quickly, this should fail ---------- | + + { + auto [success, message] = escalatingFailsafe(); + + if (success) { + ROS_ERROR("[%s]: escalating failsafe call #1.5 succeeded with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | -------------------- check for ehover -------------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!isFlyingNormally() && getActiveController() == "EmergencyController" && getActiveTracker() == "LandoffTracker") { + break; + } + + sleep(0.01); + } + + sleep(3.0); + + // check if we have not moved + + if (!isAtPosition(0, 0, 10.0, 0, 0.5)) { + ROS_ERROR("[%s]: we moved after ehover", ros::this_node::getName().c_str()); + return false; + } + + // | ------------------- trigger failsage #2 ------------------ | + + { + auto [success, message] = escalatingFailsafe(); + + if (!success) { + ROS_ERROR("[%s]: escalating failsafe call #2 failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + sleep(0.5); + + // | ----------- try again quickly, this should fail ---------- | + + { + auto [success, message] = escalatingFailsafe(); + + if (success) { + ROS_ERROR("[%s]: escalating failsafe call #2.5 succeeded with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | -------------------- wait few seconds -------------------- | + + sleep(1.5); + + // | ---------------- check if we are elanding ---------------- | + + auto uav_state = this->sh_uav_state_.getMsg(); + + if (!(!isFlyingNormally() && getActiveController() == "EmergencyController" && getActiveTracker() == "LandoffTracker" && + uav_state->velocity.linear.z < -0.3)) { + ROS_ERROR("[%s]: we are not elanding", ros::this_node::getName().c_str()); + return false; + } + + // | ------------------- trigger failsage #3 ------------------ | + + { + auto [success, message] = escalatingFailsafe(); + + if (!success) { + ROS_ERROR("[%s]: escalating failsafe call #3 failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + sleep(0.5); + + // | ----------- try again quickly, this should fail ---------- | + + { + auto [success, message] = escalatingFailsafe(); + + if (success) { + ROS_ERROR("[%s]: escalating failsafe call #3.5 succeeded with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | ------------ wait for the failsafe to trigger ------------ | + + sleep(1.5); + + if (!(!isFlyingNormally() && getActiveController() == "FailsafeController" && uav_state->velocity.linear.z < -0.3)) { + ROS_ERROR("[%s]: we are not in failsafe", ros::this_node::getName().c_str()); + return false; + } + + // | -------------------- wait for landing -------------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!this->isOutputEnabled()) { + return true; + } + + sleep(0.01); + } + + 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(); +}