generalized escalating failsafe test

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}
   )
 

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)

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)

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

test/control_manager/escalating_failsafe_rc/escalating_failsafe_rc.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="$(dirname)/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_rc/launch/hw_api.launch

@@ -0,0 +1,66 @@
+<launch>
+
+  <!-- args corresponding to environment variables -->
+  <arg name="UAV_NAME" default="$(optenv UAV_NAME)" />
+  <arg name="LOGGER_DEBUG" default="$(optenv LOGGER_DEBUG false)" />
+  <arg name="RUN_TYPE" default="$(optenv RUN_TYPE uav)" />
+
+    <!-- other args -->
+  <arg name="debug" default="false" />
+  <arg name="bond" default="$(optenv BOND true)" />
+  <arg name="nodelet_manager_name" default="$(optenv NODELET_MANAGER)" />
+  <arg name="custom_config" default="" />
+
+  <arg if="$(eval arg('nodelet_manager_name') != '')" name="standalone" value="false" />
+  <arg if="$(eval arg('nodelet_manager_name') == '')" name="standalone" value="true" />
+
+  <env name="ROSCONSOLE_CONFIG_FILE" if="$(eval arg('LOGGER_DEBUG'))" value="$(find mrs_uav_hw_api)/config/debug_verbosity.yaml" />
+
+  <arg     if="$(arg bond)" name="bond_suffix" value="" />
+  <arg unless="$(arg bond)" name="bond_suffix" value="--no-bond" />
+
+  <arg     if="$(eval arg('standalone') or arg('debug'))" name="nodelet" value="standalone" />
+  <arg unless="$(eval arg('standalone') or arg('debug'))" name="nodelet" value="load" />
+  <arg     if="$(eval arg('standalone') or arg('debug'))" name="nodelet_manager" value="" />
+  <arg unless="$(eval arg('standalone') or arg('debug'))" name="nodelet_manager" value="$(arg nodelet_manager_name)" />
+
+  <arg     if="$(arg debug)" name="launch_prefix" value="debug_roslaunch" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+
+  <group ns="$(arg UAV_NAME)">
+
+    <node pkg="nodelet" type="nodelet" name="hw_api" args="$(arg nodelet) mrs_uav_hw_api/HwApiManager $(arg nodelet_manager) $(arg bond_suffix)" output="screen" launch-prefix="$(arg launch_prefix)">
+
+      <rosparam file="$(find mrs_uav_hw_api)/config/hw_api.yaml" />
+      <rosparam file="$(find mrs_multirotor_simulator)/config/hw_api.yaml" />
+
+        <!-- Load a user param file -->
+      <rosparam if="$(eval not arg('custom_config') == '')" file="$(arg custom_config)" />
+
+        <!-- Parameters -->
+      <param name="topic_prefix" type="string" value="/$(arg UAV_NAME)/" />
+      <param name="uav_name" type="string" value="$(arg UAV_NAME)" />
+      <param name="simulation" type="bool" value="$(eval arg('RUN_TYPE') == 'simulation')" />
+
+        <!-- custom remaps -->
+      <remap from="~simulator_imu_in" to="/multirotor_simulator/$(arg UAV_NAME)/imu" />
+      <remap from="~simulator_odom_in" to="/multirotor_simulator/$(arg UAV_NAME)/odom" />
+      <remap from="~simulator_rangefinder_in" to="/multirotor_simulator/$(arg UAV_NAME)/rangefinder" />
+      <remap from="~simulator_actuators_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/actuators_cmd" />
+      <remap from="~simulator_control_group_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/control_group_cmd" />
+      <remap from="~simulator_attitude_rate_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/attitude_rate_cmd" />
+      <remap from="~simulator_attitude_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/attitude_cmd" />
+      <remap from="~simulator_acceleration_hdg_rate_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/acceleration_hdg_rate_cmd" />
+      <remap from="~simulator_acceleration_hdg_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/acceleration_hdg_cmd" />
+      <remap from="~simulator_velocity_hdg_rate_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/velocity_hdg_rate_cmd" />
+      <remap from="~simulator_velocity_hdg_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/velocity_hdg_cmd" />
+      <remap from="~simulator_position_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/position_cmd" />
+      <remap from="~simulator_tracker_cmd_out" to="/multirotor_simulator/$(arg UAV_NAME)/tracker_cmd" />
+
+      <remap from="~rc_channels" to="~rc_channels_unused" />
+
+    </node>
+
+  </group>
+
+</launch>

test/control_manager/escalating_failsafe_rc/test.cpp

@@ -0,0 +1,89 @@
+#include <gtest/gtest.h>
+
+#include <escalating_failsafe_test.h>
+
+#include <mrs_msgs/HwApiRcChannels.h>
+
+class Tester : public EscalatingFailsafeTest {
+
+public:
+  Tester();
+
+  mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels> ph_rc_channels_;
+
+  std::optional<std::tuple<bool, std::string>> 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<mrs_msgs::HwApiRcChannels>(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<std::tuple<bool, std::string>> 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();
+}