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;
+}