added failsafe tests

CMakeLists.txt

@@ -11,6 +11,7 @@ set(CATKIN_DEPENDENCIES
   pluginlib
   roscpp
   rospy
+  mrs_uav_testing
   )
 
 find_package(catkin REQUIRED COMPONENTS

package.xml

@@ -21,6 +21,7 @@
   <depend>pluginlib</depend>
   <depend>roscpp</depend>
   <depend>rospy</depend>
+  <depend>mrs_uav_testing</depend>
 
   <export>
     <mrs_uav_managers plugin="${prefix}/plugins.xml" />

test/CMakeLists.txt

@@ -1,3 +1,6 @@
 find_package(rostest REQUIRED)
+find_package(mrs_uav_testing REQUIRED)
 
-add_subdirectory(modalities)
+# add_subdirectory(modalities)
+
+add_subdirectory(failsafe_controller)

test/all_tests.sh

@@ -5,14 +5,8 @@ set -e
 trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG
 trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR
 
-PACKAGE="mrs_uav_controllers"
-VERBOSE=1
-
-[ "$VERBOSE" = "0" ] && TEXT_OUTPUT=""
-[ "$VERBOSE" = "1" ] && TEXT_OUTPUT="-t"
-
 # build the package
-catkin build $PACKAGE # it has to be fully built normally before building with --catkin-make-args tests
-catkin build $PACKAGE --no-deps --catkin-make-args tests
+catkin build --this # it has to be fully built normally before building with --catkin-make-args tests
+catkin build --this --no-deps --catkin-make-args tests
 
-catkin test --this -i
+catkin test --this -i -p 1 -s

test/coverage.sh

@@ -15,8 +15,8 @@ cd build
 
 lcov --capture --directory . --output-file coverage.info
 lcov --remove coverage.info "*/test/*" --output-file coverage.info.removed
-lcov --extract coverage.info.removed "*/src/*${PACKAGE_NAME}/*" --output-file coverage.info.cleaned
-genhtml --title "${PACKAGE_NAME} - Test coverage report" --demangle-cpp --legend --frames --show-details -o coverage_html coverage.info.cleaned | tee /tmp/genhtml.log
+lcov --extract coverage.info.removed "*/mrs_uav_autostart/*" --output-file coverage.info.cleaned
+genhtml -o coverage_html coverage.info.cleaned | tee /tmp/genhtml.log
 
 COVERAGE_PCT=`cat /tmp/genhtml.log | tail -n 1 | awk '{print $2}'`
 

test/failsafe_controller/CMakeLists.txt

@@ -0,0 +1,37 @@
+get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME)
+
+set(MODALITIES
+  actuators
+  control_group
+  attitude_rate
+  attitude
+  # acceleration_hdg_rate
+  # acceleration_hdg
+  # velocity_hdg_rate
+  # velocity_hdg
+  # position
+  )
+
+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}
+  )
+
+foreach(MODALITY ${MODALITIES})
+
+  add_rostest(${TEST_NAME}.test
+    ARGS
+    package_name:=${PROJECT_NAME}
+    test_name:=${TEST_NAME}
+    modality:=${MODALITY}
+    )
+
+endforeach()

test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         false
+  attitude_rate:         false
+  attitude:              false
+  acceleration_hdg_rate: false
+  acceleration_hdg:      true
+  velocity_hdg_rate:     false
+  velocity_hdg:          false
+  position:              false

test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg_rate.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         false
+  attitude_rate:         false
+  attitude:              false
+  acceleration_hdg_rate: true
+  acceleration_hdg:      false
+  velocity_hdg_rate:     false
+  velocity_hdg:          false
+  position:              false

test/failsafe_controller/config/hw_api/hw_api_actuators.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             true
+  control_group:         false
+  attitude_rate:         false
+  attitude:              false
+  acceleration_hdg_rate: false
+  acceleration_hdg:      false
+  velocity_hdg_rate:     false
+  velocity_hdg:          false
+  position:              false

test/failsafe_controller/config/hw_api/hw_api_attitude.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         false
+  attitude_rate:         false
+  attitude:              true
+  acceleration_hdg_rate: false
+  acceleration_hdg:      false
+  velocity_hdg_rate:     false
+  velocity_hdg:          false
+  position:              false

test/failsafe_controller/config/hw_api/hw_api_attitude_rate.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         false
+  attitude_rate:         true
+  attitude:              false
+  acceleration_hdg_rate: false
+  acceleration_hdg:      false
+  velocity_hdg_rate:     false
+  velocity_hdg:          false
+  position:              false

test/failsafe_controller/config/hw_api/hw_api_control_group.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         true
+  attitude_rate:         false
+  attitude:              false
+  acceleration_hdg_rate: false
+  acceleration_hdg:      false
+  velocity_hdg_rate:     false
+  velocity_hdg:          false
+  position:              false

test/failsafe_controller/config/hw_api/hw_api_position.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         false
+  attitude_rate:         false
+  attitude:              false
+  acceleration_hdg_rate: false
+  acceleration_hdg:      false
+  velocity_hdg_rate:     false
+  velocity_hdg:          false
+  position:              true

test/failsafe_controller/config/hw_api/hw_api_velocity_hdg.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         false
+  attitude_rate:         false
+  attitude:              false
+  acceleration_hdg_rate: false
+  acceleration_hdg:      false
+  velocity_hdg_rate:     false
+  velocity_hdg:          true
+  position:              false

test/failsafe_controller/config/hw_api/hw_api_velocity_hdg_rate.yaml

@@ -0,0 +1,10 @@
+input_mode:
+  actuators:             false
+  control_group:         false
+  attitude_rate:         false
+  attitude:              false
+  acceleration_hdg_rate: false
+  acceleration_hdg:      false
+  velocity_hdg_rate:     true
+  velocity_hdg:          false
+  position:              false

test/failsafe_controller/failsafe_controller.test

@@ -0,0 +1,35 @@
+<launch>
+
+  <arg name="UAV_NAME" default="uav1" />
+  <arg name="UAV_TYPE" default="x500" />
+
+  <!-- these will be filled in from the CMakeLists -->
+  <arg name="path" default="$(find mrs_uav_controllers)/test/failsafe_controller" />
+  <arg name="test_name" default="" />
+  <arg name="package_name" default="" />
+
+  <!-- params from the CMakeLists build matrix -->
+  <arg name="modality" default="" />
+
+  <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)" />
+    <arg name="custom_config" default="$(arg path)/config/hw_api/hw_api_$(arg modality).yaml" />
+  </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="UAV_NAME" default="$(arg UAV_NAME)" />
+  </include>
+
+  <test pkg="$(arg package_name)" 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)" />
+    <param name="modality" value="$(arg modality)" />
+  </test>
+
+</launch>

test/failsafe_controller/test.cpp

@@ -0,0 +1,109 @@
+#include <gtest/gtest.h>
+
+#include <mrs_uav_testing/test_generic.h>
+
+class Tester : public mrs_uav_testing::TestGeneric {
+
+public:
+  bool test();
+
+  Tester();
+
+  mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_failsafe_;
+
+  std::string _modality_;
+};
+
+Tester::Tester() : mrs_uav_testing::TestGeneric() {
+
+  pl_->loadParam("modality", _modality_, std::string());
+
+  sch_failsafe_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + _uav_name_ + "/control_manager/failsafe");
+}
+
+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;
+    }
+  }
+
+  // | -------------------- trigger failsafe -------------------- |
+
+  {
+    std_srvs::Trigger srv;
+
+    {
+      bool service_call = sch_failsafe_.call(srv);
+
+      if (!service_call || !srv.response.success) {
+        return false;
+      }
+    }
+  }
+
+  // | -------- wait till the right controller is active -------- |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return false;
+    }
+
+    if (_modality_ == "position") {
+      if (sh_control_manager_diag_.getMsg()->active_controller == "EmergencyController") {
+        break;
+      }
+    } else {
+      if (sh_control_manager_diag_.getMsg()->active_controller == "FailsafeController") {
+        break;
+      }
+    }
+
+    sleep(0.1);
+  }
+
+  // | ----------------- wait till the UAV lands ---------------- |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return false;
+    }
+
+    if (!isOutputEnabled()) {
+      return true;
+    }
+
+    sleep(0.1);
+  }
+
+  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) {
+
+  testing::InitGoogleTest(&argc, argv);
+
+  ros::init(argc, argv, "test");
+
+  return RUN_ALL_TESTS();
+}

test/modalities/CMakeLists.txt

@@ -1,3 +1,5 @@
+get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME)
+
 set(MODALITIES
   actuators
   control_group
@@ -15,15 +17,15 @@ set(CONTROLLERS
   mpc_controller
   )
 
-catkin_add_executable_with_gtest(test_modalities
+catkin_add_executable_with_gtest(test_${TEST_NAME}
   test.cpp
   )
 
-target_link_libraries(test_modalities
+target_link_libraries(test_${TEST_NAME}
   ${catkin_LIBRARIES}
   )
 
-add_dependencies(test_modalities
+add_dependencies(test_${TEST_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
   ${catkin_EXPORTED_TARGETS}
   )
@@ -32,7 +34,13 @@ foreach(CONTROLLER ${CONTROLLERS})
 
   foreach(MODALITY ${MODALITIES})
 
-    add_rostest(modalities.test ARGS controller:=${CONTROLLER} modality:=${MODALITY})
+    add_rostest(${TEST_NAME}.test
+      ARGS
+      package_name:=${PROJECT_NAME}
+      test_name:=${TEST_NAME}
+      controller:=${CONTROLLER}
+      modality:=${MODALITY}
+      )
 
   endforeach()