added escalating failsafe service test

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)

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)

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

test/control_manager/escalating_failsafe_service/escalating_failsafe_service.test

@@ -0,0 +1,36 @@
+<launch>
+
+  <arg name="this_path" default="$(dirname)" />
+
+  <arg name="UAV_NAME" default="uav1" />
+  <arg name="UAV_TYPE" default="x500" />
+
+    <!-- automatically deduce the test name -->
+  <arg name="test_name" default="$(eval arg('this_path').split('/')[-1])" />
+
+    <!-- automatically deduce the package name -->
+  <arg name="import_eval" default="eval('_' + '_import_' + '_')"/>
+  <arg name="package_eval" default="eval(arg('import_eval') + '(\'rospkg\')').get_package_name(arg('this_path'))" />
+  <arg name="package" default="$(eval eval(arg('package_eval')))" />
+
+  <include file="$(find mrs_uav_testing)/launch/mrs_simulator.launch">
+    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
+  </include>
+
+  <include file="$(find mrs_multirotor_simulator)/launch/hw_api.launch">
+    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
+  </include>
+
+  <include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
+    <arg name="automatic_start" default="false" />
+    <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
+    <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
+    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
+  </include>
+
+  <test pkg="$(arg package)" type="test_$(arg test_name)" test-name="$(arg test_name)" time-limit="60.0">
+    <param name="test" value="$(arg test_name)" />
+    <param name="uav_name" value="$(arg UAV_NAME)" />
+  </test>
+
+</launch>

test/control_manager/escalating_failsafe_service/test.cpp

@@ -0,0 +1,223 @@
+#include <gtest/gtest.h>
+
+#include <mrs_uav_testing/test_generic.h>
+
+/* class Tester //{ */
+
+class Tester : public mrs_uav_testing::TestGeneric {
+
+public:
+  bool test();
+
+  Tester();
+
+  mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_esc_failsafe_;
+
+  std::tuple<bool, std::string> escalatingFailsafe();
+};
+
+Tester::Tester() {
+
+  sch_esc_failsafe_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + _uav_name_ + "/control_manager/failsafe_escalating");
+}
+
+std::tuple<bool, std::string> 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();
+}