diff --git a/CMakeLists.txt b/CMakeLists.txt index 06d83d0..a703aeb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,13 +45,13 @@ set(LIBRARIES ) catkin_package( - INCLUDE_DIRS include + INCLUDE_DIRS include test/include LIBRARIES ${LIBRARIES} CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} ) include_directories( - include + include test/include ${catkin_INCLUDE_DIRS} ) diff --git a/test/control_manager/CMakeLists.txt b/test/control_manager/CMakeLists.txt index dc4bcb1..725333e 100644 --- a/test/control_manager/CMakeLists.txt +++ b/test/control_manager/CMakeLists.txt @@ -9,3 +9,5 @@ add_subdirectory(failsafe_control_error) add_subdirectory(eland_service) add_subdirectory(escalating_failsafe_service) + +add_subdirectory(escalating_failsafe_rc) diff --git a/test/control_manager/escalating_failsafe_rc/CMakeLists.txt b/test/control_manager/escalating_failsafe_rc/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/control_manager/escalating_failsafe_rc/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_rc/config/custom_config.yaml b/test/control_manager/escalating_failsafe_rc/config/custom_config.yaml new file mode 100644 index 0000000..3d6eb11 --- /dev/null +++ b/test/control_manager/escalating_failsafe_rc/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_rc/escalating_failsafe_rc.test b/test/control_manager/escalating_failsafe_rc/escalating_failsafe_rc.test new file mode 100644 index 0000000..41389f0 --- /dev/null +++ b/test/control_manager/escalating_failsafe_rc/escalating_failsafe_rc.test @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/escalating_failsafe_rc/launch/hw_api.launch b/test/control_manager/escalating_failsafe_rc/launch/hw_api.launch new file mode 100644 index 0000000..5e0f968 --- /dev/null +++ b/test/control_manager/escalating_failsafe_rc/launch/hw_api.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/escalating_failsafe_rc/test.cpp b/test/control_manager/escalating_failsafe_rc/test.cpp new file mode 100644 index 0000000..512c4ff --- /dev/null +++ b/test/control_manager/escalating_failsafe_rc/test.cpp @@ -0,0 +1,89 @@ +#include + +#include + +#include + +class Tester : public EscalatingFailsafeTest { + +public: + Tester(); + + mrs_lib::PublisherHandler ph_rc_channels_; + + std::optional> escalatingFailsafe(); + + ros::Timer timer_rc_; + void timerRc(const ros::TimerEvent& event); + + mrs_msgs::HwApiRcChannels rc_; + std::mutex mutex_rc_; +}; + +Tester::Tester() { + + ph_rc_channels_ = mrs_lib::PublisherHandler(nh_, "/" + _uav_name_ + "/hw_api/rc_channels"); + + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + + timer_rc_ = nh_.createTimer(ros::Rate(100.0), &Tester::timerRc, this, false, true); +} + +void Tester::timerRc([[maybe_unused]] const ros::TimerEvent& event) { + + { + std::scoped_lock lock(mutex_rc_); + + ph_rc_channels_.publish(rc_); + } +} + +std::optional> Tester::escalatingFailsafe() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[7] = 1.0; + } + + sleep(0.2); + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[7] = 0.0; + } + + return {}; +} + +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(); +} diff --git a/test/control_manager/escalating_failsafe_service/test.cpp b/test/control_manager/escalating_failsafe_service/test.cpp index 87d461b..fedddd6 100644 --- a/test/control_manager/escalating_failsafe_service/test.cpp +++ b/test/control_manager/escalating_failsafe_service/test.cpp @@ -1,19 +1,15 @@ #include -#include +#include -/* class Tester //{ */ - -class Tester : public mrs_uav_testing::TestGeneric { +class Tester : public EscalatingFailsafeTest { public: - bool test(); - Tester(); mrs_lib::ServiceClientHandler sch_esc_failsafe_; - std::tuple escalatingFailsafe(); + std::optional> escalatingFailsafe(); }; Tester::Tester() { @@ -21,7 +17,7 @@ Tester::Tester() { sch_esc_failsafe_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/control_manager/failsafe_escalating"); } -std::tuple Tester::escalatingFailsafe() { +std::optional> Tester::escalatingFailsafe() { { std_srvs::Trigger srv; @@ -30,176 +26,14 @@ std::tuple Tester::escalatingFailsafe() { bool service_call = sch_esc_failsafe_.call(srv); if (!service_call || !srv.response.success) { - return {false, "escalating failsafe service call failed"}; + return {{false, "escalating failsafe service call failed"}}; } } } - return {true, "service called"}; + 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; diff --git a/test/include/escalating_failsafe_test.h b/test/include/escalating_failsafe_test.h new file mode 100644 index 0000000..76be291 --- /dev/null +++ b/test/include/escalating_failsafe_test.h @@ -0,0 +1,218 @@ +#include + +class EscalatingFailsafeTest : public mrs_uav_testing::TestGeneric { + +public: + bool test(); + + EscalatingFailsafeTest() : mrs_uav_testing::TestGeneric(){}; + + virtual std::optional> escalatingFailsafe() = 0; +}; + +bool EscalatingFailsafeTest::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 failsafe #1 ------------------ | + + { + auto result = escalatingFailsafe(); + + if (result) { + + auto [success, message] = result.value(); + + 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 result = escalatingFailsafe(); + + if (result) { + + auto [success, message] = result.value(); + + if (success) { + ROS_ERROR("[%s]: escalating failsafe call #1.5 suceeded 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 failsafe #2 ------------------ | + + { + auto result = escalatingFailsafe(); + + if (result) { + + auto [success, message] = result.value(); + + 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 result = escalatingFailsafe(); + + if (result) { + + auto [success, message] = result.value(); + + if (success) { + ROS_ERROR("[%s]: escalating failsafe call #2.5 suceeded 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 failsafe #3 ------------------ | + + { + auto result = escalatingFailsafe(); + + if (result) { + + auto [success, message] = result.value(); + + 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 result = escalatingFailsafe(); + + if (result) { + + auto [success, message] = result.value(); + + if (success) { + ROS_ERROR("[%s]: escalating failsafe call #3.5 suceeded 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; + } + + sleep(0.5); + + // | ----------- try again quickly, this should fail ---------- | + + { + auto result = escalatingFailsafe(); + + if (result) { + + auto [success, message] = result.value(); + + if (success) { + ROS_ERROR("[%s]: escalating failsafe call #4 suceeded with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + } + + // | -------------------- wait for landing -------------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!this->isOutputEnabled()) { + return true; + } + + sleep(0.01); + } + + return false; +}