added tests for takeoff failure

src/uav_manager.cpp

@@ -397,6 +397,11 @@ void UavManager::initialize() {
   param_loader.loadParam(yaml_prefix + "takeoff/during_takeoff/tracker", _takeoff_tracker_name_);
   param_loader.loadParam(yaml_prefix + "takeoff/takeoff_height", _takeoff_height_);
 
+  if (_takeoff_height_ < 0.5 || _takeoff_height_ > 10.0) {
+    ROS_ERROR("[UavManager]: the takeoff height (%.2f m) has to be between 0.5 and 10 meters", _takeoff_height_);
+    ros::shutdown();
+  }
+
   param_loader.loadParam(yaml_prefix + "landing/rate", _landing_timer_rate_);
   param_loader.loadParam(yaml_prefix + "landing/landing_tracker", _landing_tracker_name_);
   param_loader.loadParam(yaml_prefix + "landing/landing_controller", _landing_controller_name_);
@@ -1458,7 +1463,8 @@ bool UavManager::callbackTakeoff([[maybe_unused]] std_srvs::Trigger::Request& re
       res.success = false;
       res.message = ss.str();
 
-      switchControllerSrv(old_controller);
+      toggleControlOutput(false);
+      disarmSrv();
 
       return true;
     }
@@ -1478,7 +1484,8 @@ bool UavManager::callbackTakeoff([[maybe_unused]] std_srvs::Trigger::Request& re
       res.success = false;
       res.message = ss.str();
 
-      switchTrackerSrv(old_tracker);
+      toggleControlOutput(false);
+      disarmSrv();
 
       return true;
     }

test/include/failed_takeoff_test.h

@@ -0,0 +1,117 @@
+#include <mrs_uav_testing/test_generic.h>
+
+class FailedTakeoffTest : public mrs_uav_testing::TestGeneric {
+
+public:
+  bool test();
+
+  FailedTakeoffTest();
+
+private:
+  mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_takeoff_;
+};
+
+FailedTakeoffTest::FailedTakeoffTest() : mrs_uav_testing::TestGeneric() {
+
+  sch_takeoff_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + _uav_name_ + "/uav_manager/takeoff");
+}
+
+bool FailedTakeoffTest::test() {
+
+  // | ---------------- wait for ready to takeoff --------------- |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return false;
+    }
+
+    ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", name_.c_str());
+
+    if (mrsSystemReady()) {
+      ROS_INFO("[%s]: MRS UAV System is ready", name_.c_str());
+      break;
+    }
+
+    sleep(0.01);
+  }
+
+  // | ---------------------- arm the drone --------------------- |
+
+  ROS_INFO("[%s]: arming the drone", name_.c_str());
+
+  {
+    std_srvs::SetBool srv;
+    srv.request.data = true;
+
+    {
+      bool service_call = sch_arming_.call(srv);
+
+      if (!service_call || !srv.response.success) {
+        return false;
+      }
+    }
+  }
+
+  // | ---------------------- wait a second --------------------- |
+
+  sleep(2.0);
+
+  // | --------------------- check if armed --------------------- |
+
+  if (!sh_hw_api_status_.getMsg()->armed) {
+    return false;
+  }
+
+  // | ------------------- switch to offboard ------------------- |
+
+  {
+    std_srvs::Trigger srv;
+
+    {
+      bool service_call = sch_offboard_.call(srv);
+
+      if (!service_call || !srv.response.success) {
+        return false;
+      }
+    }
+  }
+
+  // | -------------------------- wait -------------------------- |
+
+  sleep(2.0);
+
+  // | ------------------ check if in offboard ------------------ |
+
+  if (!sh_hw_api_status_.getMsg()->offboard) {
+    return false;
+  }
+
+  sleep(2.0);
+
+  // | ------------------------- takeoff ------------------------ |
+
+  {
+    std_srvs::Trigger srv;
+
+    {
+      bool service_call = sch_takeoff_.call(srv);
+
+      if (service_call && srv.response.success) {
+        ROS_ERROR("[%s]: takeoff call success, this should not happen", ros::this_node::getName().c_str());
+        return false;
+      }
+    }
+  }
+
+  // | -------------------- check if disarmed ------------------- |
+
+  sleep(0.1);
+
+  if (this->sh_hw_api_status_.getMsg()->armed) {
+    ROS_ERROR("[%s]: the uav is still armed", ros::this_node::getName().c_str());
+    return false;
+  }
+
+  return true;
+}

test/uav_manager/CMakeLists.txt

@@ -1,5 +1,9 @@
 add_subdirectory(takeoff)
 
+add_subdirectory(failed_takeoff_controller)
+
+add_subdirectory(failed_takeoff_tracker)
+
 add_subdirectory(landing)
 
 add_subdirectory(land_home)

test/uav_manager/failed_takeoff_controller/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/uav_manager/failed_takeoff_controller/config/automatic_start.yaml

@@ -0,0 +1 @@
+handle_takeoff: false

test/uav_manager/failed_takeoff_controller/config/custom_config.yaml

@@ -0,0 +1,24 @@
+# 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_garmin",
+    ]
+
+    initial_state_estimator: "gps_garmin" # will be used as the first state estimator
+    agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback)
+
+  uav_manager:
+
+    takeoff:
+
+      during_takeoff:
+        # some nonexisting controller
+        controller: "WrongController"

test/uav_manager/failed_takeoff_controller/config/mrs_simulator.yaml

@@ -0,0 +1,3 @@
+# turning off for the takeoff test
+individual_takeoff_platform:
+  enabled: false

test/uav_manager/failed_takeoff_controller/failed_takeoff_controller.test

@@ -0,0 +1,38 @@
+<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)" />
+    <arg name="mrs_simulator_config" default="$(dirname)/config/mrs_simulator.yaml" />
+  </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="true" />
+    <arg name="automatic_start_config" default="$(dirname)/config/automatic_start.yaml" />
+    <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/uav_manager/failed_takeoff_controller/test.cpp

@@ -0,0 +1,32 @@
+#include <gtest/gtest.h>
+
+#include <failed_takeoff_test.h>
+
+class Tester : public FailedTakeoffTest {
+
+public:
+  Tester() : FailedTakeoffTest(){};
+};
+
+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();
+}
+

test/uav_manager/failed_takeoff_tracker/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/uav_manager/failed_takeoff_tracker/config/automatic_start.yaml

@@ -0,0 +1 @@
+handle_takeoff: false

test/uav_manager/failed_takeoff_tracker/config/custom_config.yaml

@@ -0,0 +1,24 @@
+# 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_garmin",
+    ]
+
+    initial_state_estimator: "gps_garmin" # will be used as the first state estimator
+    agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback)
+
+  uav_manager:
+
+    takeoff:
+
+      during_takeoff:
+        # some nonexisting tracker
+        tracker: "WrongTracker

test/uav_manager/failed_takeoff_tracker/config/mrs_simulator.yaml

@@ -0,0 +1,3 @@
+# turning off for the takeoff test
+individual_takeoff_platform:
+  enabled: false

test/uav_manager/failed_takeoff_tracker/failed_takeoff_tracker.test

@@ -0,0 +1,38 @@
+<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)" />
+    <arg name="mrs_simulator_config" default="$(dirname)/config/mrs_simulator.yaml" />
+  </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="true" />
+    <arg name="automatic_start_config" default="$(dirname)/config/automatic_start.yaml" />
+    <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/uav_manager/failed_takeoff_tracker/test.cpp

@@ -0,0 +1,32 @@
+#include <gtest/gtest.h>
+
+#include <failed_takeoff_test.h>
+
+class Tester : public FailedTakeoffTest {
+
+public:
+  Tester() : FailedTakeoffTest(){};
+};
+
+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();
+}
+