diff --git a/src/uav_manager.cpp b/src/uav_manager.cpp index 9b9fbdb..b1748f6 100644 --- a/src/uav_manager.cpp +++ b/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; } diff --git a/test/include/failed_takeoff_test.h b/test/include/failed_takeoff_test.h new file mode 100644 index 0000000..fdff4d4 --- /dev/null +++ b/test/include/failed_takeoff_test.h @@ -0,0 +1,117 @@ +#include + +class FailedTakeoffTest : public mrs_uav_testing::TestGeneric { + +public: + bool test(); + + FailedTakeoffTest(); + +private: + mrs_lib::ServiceClientHandler sch_takeoff_; +}; + +FailedTakeoffTest::FailedTakeoffTest() : mrs_uav_testing::TestGeneric() { + + sch_takeoff_ = mrs_lib::ServiceClientHandler(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; +} diff --git a/test/uav_manager/CMakeLists.txt b/test/uav_manager/CMakeLists.txt index 7d0c563..1227e29 100644 --- a/test/uav_manager/CMakeLists.txt +++ b/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) diff --git a/test/uav_manager/failed_takeoff_controller/CMakeLists.txt b/test/uav_manager/failed_takeoff_controller/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/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) diff --git a/test/uav_manager/failed_takeoff_controller/config/automatic_start.yaml b/test/uav_manager/failed_takeoff_controller/config/automatic_start.yaml new file mode 100644 index 0000000..f7893c1 --- /dev/null +++ b/test/uav_manager/failed_takeoff_controller/config/automatic_start.yaml @@ -0,0 +1 @@ +handle_takeoff: false diff --git a/test/uav_manager/failed_takeoff_controller/config/custom_config.yaml b/test/uav_manager/failed_takeoff_controller/config/custom_config.yaml new file mode 100644 index 0000000..304b7f1 --- /dev/null +++ b/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" diff --git a/test/uav_manager/failed_takeoff_controller/config/mrs_simulator.yaml b/test/uav_manager/failed_takeoff_controller/config/mrs_simulator.yaml new file mode 100644 index 0000000..7704c54 --- /dev/null +++ b/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 diff --git a/test/uav_manager/failed_takeoff_controller/failed_takeoff_controller.test b/test/uav_manager/failed_takeoff_controller/failed_takeoff_controller.test new file mode 100644 index 0000000..894197d --- /dev/null +++ b/test/uav_manager/failed_takeoff_controller/failed_takeoff_controller.test @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/uav_manager/failed_takeoff_controller/test.cpp b/test/uav_manager/failed_takeoff_controller/test.cpp new file mode 100644 index 0000000..d03b94e --- /dev/null +++ b/test/uav_manager/failed_takeoff_controller/test.cpp @@ -0,0 +1,32 @@ +#include + +#include + +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(); +} + diff --git a/test/uav_manager/failed_takeoff_tracker/CMakeLists.txt b/test/uav_manager/failed_takeoff_tracker/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/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) diff --git a/test/uav_manager/failed_takeoff_tracker/config/automatic_start.yaml b/test/uav_manager/failed_takeoff_tracker/config/automatic_start.yaml new file mode 100644 index 0000000..f7893c1 --- /dev/null +++ b/test/uav_manager/failed_takeoff_tracker/config/automatic_start.yaml @@ -0,0 +1 @@ +handle_takeoff: false diff --git a/test/uav_manager/failed_takeoff_tracker/config/custom_config.yaml b/test/uav_manager/failed_takeoff_tracker/config/custom_config.yaml new file mode 100644 index 0000000..8d7bfba --- /dev/null +++ b/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 diff --git a/test/uav_manager/failed_takeoff_tracker/config/mrs_simulator.yaml b/test/uav_manager/failed_takeoff_tracker/config/mrs_simulator.yaml new file mode 100644 index 0000000..7704c54 --- /dev/null +++ b/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 diff --git a/test/uav_manager/failed_takeoff_tracker/failed_takeoff_tracker.test b/test/uav_manager/failed_takeoff_tracker/failed_takeoff_tracker.test new file mode 100644 index 0000000..894197d --- /dev/null +++ b/test/uav_manager/failed_takeoff_tracker/failed_takeoff_tracker.test @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/uav_manager/failed_takeoff_tracker/test.cpp b/test/uav_manager/failed_takeoff_tracker/test.cpp new file mode 100644 index 0000000..d03b94e --- /dev/null +++ b/test/uav_manager/failed_takeoff_tracker/test.cpp @@ -0,0 +1,32 @@ +#include + +#include + +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(); +} +