diff --git a/CMakeLists.txt b/CMakeLists.txt index b0cbd00a..06d83d04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ set(CATKIN_DEPENDENCIES tf2 tf2_ros tf2_geometry_msgs + mrs_uav_testing ) find_package(catkin REQUIRED COMPONENTS @@ -24,7 +25,14 @@ find_package(catkin REQUIRED COMPONENTS set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") +add_definitions(-Wall) +add_definitions(-Wextra) + +if(COVERAGE) + message(WARNING "building with --coverage, the performance might be limited") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --coverage") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage") +endif() set(LIBRARIES MrsUavManagers_GainManager @@ -163,9 +171,6 @@ target_link_libraries(MrsUavManagers_TransformManager if(CATKIN_ENABLE_TESTING) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --coverage") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage") - add_subdirectory(test) endif() diff --git a/package.xml b/package.xml index d045fa7f..cf6ad0b3 100644 --- a/package.xml +++ b/package.xml @@ -26,6 +26,9 @@ tf2_ros tf2_geometry_msgs rostest + mrs_uav_testing + + mrs_uav_gazebo_simulation diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp index b141c1d2..6e3319a3 100644 --- a/src/control_manager/control_manager.cpp +++ b/src/control_manager/control_manager.cpp @@ -2869,16 +2869,10 @@ void ControlManager::timerEland(const ros::TimerEvent& event) { return; } - double throttle = 0; - - if (std::holds_alternative(last_control_output.control_output.value())) { - throttle = std::get(last_control_output.control_output.value()).throttle; - } else if (std::holds_alternative(last_control_output.control_output.value())) { - throttle = std::get(last_control_output.control_output.value()).throttle; - } else if (std::holds_alternative(last_control_output.control_output.value())) { - throttle = std::get(last_control_output.control_output.value()).throttle; - } else { - ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement eland timer for this control mode"); + auto throttle = extractThrottle(last_control_output); + + if (!throttle) { + ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement landing detection mechanism for the current control modality"); return; } @@ -2888,6 +2882,14 @@ void ControlManager::timerEland(const ros::TimerEvent& event) { } else if (current_state_landing_ == LANDING_STATE) { + // -------------------------------------------------------------- + // | TODO | + // This section needs work. The throttle landing detection | + // mechanism should be extracted and other mechanisms, such | + // as velocity-based detection should be used for high | + // modalities | + // -------------------------------------------------------------- + if (!last_control_output.control_output) { ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning"); ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered"); @@ -2895,7 +2897,7 @@ void ControlManager::timerEland(const ros::TimerEvent& event) { } // recalculate the mass based on the throttle - throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle) / common_handlers_->g; + throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g; ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_); // condition for automatic motor turn off @@ -2977,6 +2979,14 @@ void ControlManager::timerFailsafe(const ros::TimerEvent& event) { return; } + // -------------------------------------------------------------- + // | TODO | + // This section needs work. The throttle landing detection | + // mechanism should be extracted and other mechanisms, such | + // as velocity-based detection should be used for high | + // modalities | + // -------------------------------------------------------------- + double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g; ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_); @@ -7259,6 +7269,7 @@ std::tuple ControlManager::eland(void) { /* failsafe() //{ */ std::tuple ControlManager::failsafe(void) { + // copy member variables auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_); auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_); @@ -7288,6 +7299,10 @@ std::tuple ControlManager::failsafe(void) { return std::tuple(true, "RC emergency handoff is ON, disabling output"); } + if (getLowestOuput(_hw_api_inputs_) == POSITION) { + return eland(); + } + if (_parachute_enabled_) { auto [success, message] = deployParachute(); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index aadf38fc..c3782ba4 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,127 +1,5 @@ find_package(rostest REQUIRED) -# takeoff test +add_subdirectory(control_manager) -add_rostest_gtest(MrsUavManagers_UavManagerTakeoffTest - uav_manager/takeoff/uav_manager_takeoff.test - uav_manager/takeoff/takeoff_test.cpp - ) - -target_link_libraries( - MrsUavManagers_UavManagerTakeoffTest - ${catkin_LIBRARIES} - ) - -add_dependencies( - MrsUavManagers_UavManagerTakeoffTest - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -# landing test - -add_rostest_gtest(MrsUavManagers_UavManagerLandingTest - uav_manager/landing/uav_manager_landing.test - uav_manager/landing/landing_test.cpp - ) - -target_link_libraries( - MrsUavManagers_UavManagerLandingTest - ${catkin_LIBRARIES} - ) - -add_dependencies( - MrsUavManagers_UavManagerLandingTest - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -# goto service test - -add_rostest_gtest(MrsUavManagers_ControlManagerGotoServiceTest - control_manager/goto_service/control_manager_goto_service.test - control_manager/goto_service/goto_service_test.cpp - ) - -target_link_libraries( - MrsUavManagers_ControlManagerGotoServiceTest - ${catkin_LIBRARIES} - ) - -add_dependencies( - MrsUavManagers_ControlManagerGotoServiceTest - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -# eland innovation test - -add_rostest_gtest(MrsUavManagers_ControlManagerElandInnovationTest - control_manager/eland_innovation/control_manager_eland_innovation.test - control_manager/eland_innovation/eland_innovation_test.cpp - ) - -target_link_libraries( - MrsUavManagers_ControlManagerElandInnovationTest - ${catkin_LIBRARIES} - ) - -add_dependencies( - MrsUavManagers_ControlManagerElandInnovationTest - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -# eland controlerror test - -add_rostest_gtest(MrsUavManagers_ControlManagerElandControlErrorTest - control_manager/eland_control_error/control_manager_eland_control_error.test - control_manager/eland_control_error/eland_control_error_test.cpp - ) - -target_link_libraries( - MrsUavManagers_ControlManagerElandControlErrorTest - ${catkin_LIBRARIES} - ) - -add_dependencies( - MrsUavManagers_ControlManagerElandControlErrorTest - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -# failsafe controlerror test - -add_rostest_gtest(MrsUavManagers_ControlManagerFailsafeControlErrorTest - control_manager/failsafe_control_error/control_manager_failsafe_control_error.test - control_manager/failsafe_control_error/failsafe_control_error_test.cpp - ) - -target_link_libraries( - MrsUavManagers_ControlManagerFailsafeControlErrorTest - ${catkin_LIBRARIES} - ) - -add_dependencies( - MrsUavManagers_ControlManagerFailsafeControlErrorTest - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -# eland service test - -add_rostest_gtest(MrsUavManagers_ControlManagerElandServiceTest - control_manager/eland_service/control_manager_eland_service.test - control_manager/eland_service/eland_service_test.cpp - ) - -target_link_libraries( - MrsUavManagers_ControlManagerElandServiceTest - ${catkin_LIBRARIES} - ) - -add_dependencies( - MrsUavManagers_ControlManagerElandServiceTest - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) +add_subdirectory(uav_manager) diff --git a/test/all_tests.sh b/test/all_tests.sh index 8a3e2f24..6614ec31 100755 --- a/test/all_tests.sh +++ b/test/all_tests.sh @@ -5,31 +5,30 @@ 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_managers" -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 - -TEST_FILES=$(find . -name "*.test" -type f -printf "%f\n") +ORIGINAL_PATH=`pwd` + +while [ ! -e ".catkin_tools" ]; do + cd .. + if [[ `pwd` == "/" ]]; then + # we reached the root and didn't find the build/COLCON_IGNORE file - that's a fail! + echo "$0: could not find the root of the current workspace". + return 1 + fi +done -for TEST_FILE in `echo $TEST_FILES`; do +cd build - echo "$0: running test '$TEST_FILE'" +OLD_FILES=$(find . -name "*.gcda") - # folder for test results - TEST_RESULT_PATH=$(realpath /tmp/$RANDOM) - mkdir -p $TEST_RESULT_PATH +for FILE in $OLD_FILES; do + echo "$0: removing old coverage file '$FILE'" + rm $FILE +done - # run the test - rostest $PACKAGE $TEST_FILE $TEXT_OUTPUT --results-filename=$PACKAGE.test --results-base-dir="$TEST_RESULT_PATH" +cd $ORIGINAL_PATH - # evaluate the test results - echo test result path is $TEST_RESULT_PATH - catkin_test_results "$TEST_RESULT_PATH" +# build the package +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 -done +catkin test --this -i -p 1 -s diff --git a/test/control_manager/CMakeLists.txt b/test/control_manager/CMakeLists.txt new file mode 100644 index 00000000..17b34a55 --- /dev/null +++ b/test/control_manager/CMakeLists.txt @@ -0,0 +1,7 @@ +add_subdirectory(goto_service) + +add_subdirectory(eland_control_error) + +add_subdirectory(eland_innovation) + +add_subdirectory(failsafe_control_error) diff --git a/test/control_manager/eland_control_error/CMakeLists.txt b/test/control_manager/eland_control_error/CMakeLists.txt new file mode 100644 index 00000000..bf8f9853 --- /dev/null +++ b/test/control_manager/eland_control_error/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/control_manager/eland_control_error/config/custom_config.yaml b/test/control_manager/eland_control_error/config/custom_config.yaml index 3b685fac..c7d83d50 100644 --- a/test/control_manager/eland_control_error/config/custom_config.yaml +++ b/test/control_manager/eland_control_error/config/custom_config.yaml @@ -9,10 +9,10 @@ mrs_uav_managers: # loaded state estimator plugins state_estimators: [ - "passthrough", + "gps_baro", ] - initial_state_estimator: "passthrough" # will be used as the first state estimator + 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: diff --git a/test/control_manager/eland_control_error/config/network_config.yaml b/test/control_manager/eland_control_error/config/network_config.yaml deleted file mode 100644 index 08f370d6..00000000 --- a/test/control_manager/eland_control_error/config/network_config.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# 1. This list is used by NimbroNetwork for mutual communication of the UAVs -# The names of the robots have to match hostnames described in /etc/hosts. -# -# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. -# The names should match the true "UAV_NAMES" (the topic prefixes). -# -# network_config:=~/config/network_config.yaml -# -# to the core.launch and nimbro.launch. - -network: - - robot_names: [ - uav1, - ] diff --git a/test/control_manager/eland_control_error/config/simulator.yaml b/test/control_manager/eland_control_error/config/simulator.yaml deleted file mode 100644 index 7bdd82f3..00000000 --- a/test/control_manager/eland_control_error/config/simulator.yaml +++ /dev/null @@ -1,20 +0,0 @@ -simulation_rate: 250.0 -clock_rate: 250.0 -realtime_factor: 1.0 - -iterate_without_input: true - -individual_takeoff_platform: - enabled: true - -uav_names: [ - "uav1", -] - -uav1: - type: "x500" - spawn: - x: 5.0 - y: 5.0 - z: 2.0 - heading: 0 diff --git a/test/control_manager/eland_control_error/config/world_config.yaml b/test/control_manager/eland_control_error/config/world_config.yaml deleted file mode 100644 index 00e01eef..00000000 --- a/test/control_manager/eland_control_error/config/world_config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -world_origin: - - units: "LATLON" # {"UTM, "LATLON"} - - origin_x: 47.397743 - origin_y: 8.545594 - -safety_area: - - enabled: false - - horizontal: - - # the frame of reference in which the points are expressed - frame_name: "world_origin" - - # polygon - # - # x, y [m] for any frame_name except latlon_origin - # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" - points: [ - -50, -50, - 50, -50, - 50, 50, - -50, 50, - ] - - vertical: - - # the frame of reference in which the max&min z is expressed - frame_name: "world_origin" - - max_z: 15.0 - min_z: 0.5 diff --git a/test/control_manager/eland_control_error/control_manager_eland_control_error.test b/test/control_manager/eland_control_error/control_manager_eland_control_error.test deleted file mode 100644 index 9cddd763..00000000 --- a/test/control_manager/eland_control_error/control_manager_eland_control_error.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/control_manager/eland_control_error/eland_control_error.test b/test/control_manager/eland_control_error/eland_control_error.test new file mode 100644 index 00000000..c17dfb02 --- /dev/null +++ b/test/control_manager/eland_control_error/eland_control_error.test @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/eland_control_error/eland_control_error_test.cpp b/test/control_manager/eland_control_error/eland_control_error_test.cpp deleted file mode 100644 index d1d7cdd9..00000000 --- a/test/control_manager/eland_control_error/eland_control_error_test.cpp +++ /dev/null @@ -1,224 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include - -/* class Tester //{ */ - -class Tester { - -public: - Tester(); - - bool test(); - -private: - ros::NodeHandle nh_; - std::shared_ptr spinner_; - - mrs_lib::SubscribeHandler sh_control_manager_diag_; - mrs_lib::SubscribeHandler sh_estim_manager_diag_; - - mrs_lib::ServiceClientHandler sch_arming_; - mrs_lib::ServiceClientHandler sch_midair_; - - mrs_lib::ServiceClientHandler sch_goto_; -}; - -//} - -/* Tester() //{ */ - -Tester::Tester() { - - // | ------------------ initialize test node ------------------ | - - nh_ = ros::NodeHandle("~"); - - ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); - - ros::Time::waitForValid(); - - spinner_ = std::make_shared(4); - spinner_->start(); - - std::string uav_name = "uav1"; - - // | ----------------------- subscribers ---------------------- | - - mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "Test"; - shopts.no_message_timeout = mrs_lib::no_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - - sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); - sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/estimation_manager/diagnostics"); - - ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str()); - - // | --------------------- service clients -------------------- | - - sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/arming"); - sch_midair_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/uav_manager/midair_activation"); - - sch_goto_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/control_manager/goto_relative"); - - ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); -} - -//} - -/* test() //{ */ - -bool Tester::test() { - - // | ---------------- wait for ready to takeoff --------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.hasMsg() && sh_estim_manager_diag_.hasMsg()) { - break; - } - - ros::Duration(0.01).sleep(); - } - - ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); - - ros::Duration(1.0).sleep(); - - // | ---------------------- arm the drone --------------------- | - - ROS_INFO("[%s]: arming the drone", ros::this_node::getName().c_str()); - - { - std_srvs::SetBool arming; - arming.request.data = true; - - { - bool service_call = sch_arming_.call(arming); - - if (!service_call || !arming.response.success) { - ROS_ERROR("[%s]: arming service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------------------- wait -------------------------- | - - ros::Duration(0.2).sleep(); - - // | -------------------- midair activation ------------------- | - - ROS_INFO("[%s]: activating the drone 'in mid air'", ros::this_node::getName().c_str()); - - { - std_srvs::Trigger trigger; - - { - bool service_call = sch_midair_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: midair activation service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ----------------- sleep before the action ---------------- | - - ros::Duration(1.0).sleep(); - - // | -------------------------- goto -------------------------- | - - { - mrs_msgs::Vec4 goto_cmd; - - goto_cmd.request.goal[0] = 5.0; - - ROS_INFO("[%s]: calling goto", ros::this_node::getName().c_str()); - - { - bool service_call = sch_goto_.call(goto_cmd); - - if (!service_call || !goto_cmd.response.success) { - ROS_ERROR("[%s]: goto service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------- wait for emergency controller ------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for EmergencyController", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->active_controller == "EmergencyController") { - break; - } - } - - // | -------------------- wait for landing -------------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for landing", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->output_enabled == false) { - break; - } - } - - return true; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, eland_control_error) { - - 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, "eland_control_error_test"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/control_manager/eland_control_error/launch/mrs_uav_system.launch b/test/control_manager/eland_control_error/launch/mrs_uav_system.launch deleted file mode 100644 index 1811aa12..00000000 --- a/test/control_manager/eland_control_error/launch/mrs_uav_system.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/control_manager/eland_control_error/test.cpp b/test/control_manager/eland_control_error/test.cpp new file mode 100644 index 00000000..d6338b36 --- /dev/null +++ b/test/control_manager/eland_control_error/test.cpp @@ -0,0 +1,81 @@ +#include + +#include + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); +}; + +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; + } + } + + { + auto [success, message] = this->gotoRel(2, 2, 2, 0); + + if (success) { + ROS_ERROR("[%s]: goto succeeded, that should not happen in this case: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | -------------- wait for the eland to trigger ------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!isFlyingNormally() && getActiveController() == "EmergencyController" && getActiveTracker() == "LandoffTracker") { + break; + } + } + + // | -------------------- wait for landing -------------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!this->isOutputEnabled()) { + return true; + } + } + + 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(); +} diff --git a/test/control_manager/eland_innovation/CMakeLists.txt b/test/control_manager/eland_innovation/CMakeLists.txt new file mode 100644 index 00000000..bf8f9853 --- /dev/null +++ b/test/control_manager/eland_innovation/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/control_manager/eland_innovation/config/network_config.yaml b/test/control_manager/eland_innovation/config/network_config.yaml deleted file mode 100644 index 08f370d6..00000000 --- a/test/control_manager/eland_innovation/config/network_config.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# 1. This list is used by NimbroNetwork for mutual communication of the UAVs -# The names of the robots have to match hostnames described in /etc/hosts. -# -# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. -# The names should match the true "UAV_NAMES" (the topic prefixes). -# -# network_config:=~/config/network_config.yaml -# -# to the core.launch and nimbro.launch. - -network: - - robot_names: [ - uav1, - ] diff --git a/test/control_manager/eland_innovation/config/simulator.yaml b/test/control_manager/eland_innovation/config/simulator.yaml deleted file mode 100644 index 7bdd82f3..00000000 --- a/test/control_manager/eland_innovation/config/simulator.yaml +++ /dev/null @@ -1,20 +0,0 @@ -simulation_rate: 250.0 -clock_rate: 250.0 -realtime_factor: 1.0 - -iterate_without_input: true - -individual_takeoff_platform: - enabled: true - -uav_names: [ - "uav1", -] - -uav1: - type: "x500" - spawn: - x: 5.0 - y: 5.0 - z: 2.0 - heading: 0 diff --git a/test/control_manager/eland_innovation/control_manager_eland_innovation.test b/test/control_manager/eland_innovation/control_manager_eland_innovation.test deleted file mode 100644 index 3c85ed37..00000000 --- a/test/control_manager/eland_innovation/control_manager_eland_innovation.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/control_manager/eland_innovation/eland_innovation.test b/test/control_manager/eland_innovation/eland_innovation.test new file mode 100644 index 00000000..54865e18 --- /dev/null +++ b/test/control_manager/eland_innovation/eland_innovation.test @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/eland_innovation/eland_innovation_test.cpp b/test/control_manager/eland_innovation/eland_innovation_test.cpp deleted file mode 100644 index e066b952..00000000 --- a/test/control_manager/eland_innovation/eland_innovation_test.cpp +++ /dev/null @@ -1,235 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include - -#define POS_JUMP_SIZE 3.0 - -/* class Tester //{ */ - -class Tester { - -public: - Tester(); - - bool test(); - -private: - ros::NodeHandle nh_; - std::shared_ptr spinner_; - - mrs_lib::SubscribeHandler sh_control_manager_diag_; - mrs_lib::SubscribeHandler sh_estim_manager_diag_; - mrs_lib::SubscribeHandler sh_hw_api_odom_; - - mrs_lib::ServiceClientHandler sch_arming_; - mrs_lib::ServiceClientHandler sch_midair_; - - mrs_lib::PublisherHandler ph_odometry_; - - void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg); - - std::atomic odom_jumped_ = false; -}; - -//} - -/* callbackOdometry() //{ */ - -void Tester::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) { - - nav_msgs::Odometry odom_out = *msg; - - if (odom_jumped_) { - odom_out.pose.pose.position.x += POS_JUMP_SIZE; - } - - ph_odometry_.publish(odom_out); -} - -//} - -/* Tester() //{ */ - -Tester::Tester() { - - // | ------------------ initialize test node ------------------ | - - nh_ = ros::NodeHandle("~"); - - ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); - - ros::Time::waitForValid(); - - spinner_ = std::make_shared(4); - spinner_->start(); - - std::string uav_name = "uav1"; - - // | ----------------------- subscribers ---------------------- | - - mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "Test"; - shopts.no_message_timeout = mrs_lib::no_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - - sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); - sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/estimation_manager/diagnostics"); - sh_hw_api_odom_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/hw_api/odometry_unchanged", &Tester::callbackOdometry, this); - - ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str()); - - // | --------------------- service clients -------------------- | - - sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/arming"); - sch_midair_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/uav_manager/midair_activation"); - - ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); - - // | ----------------------- publishers ----------------------- | - - ph_odometry_ = mrs_lib::PublisherHandler(nh_, "/" + uav_name + "/hw_api/odometry"); - - ROS_INFO("[%s]: publishers initialized", ros::this_node::getName().c_str()); -} - -//} - -/* test() //{ */ - -bool Tester::test() { - - // | ---------------- wait for ready to takeoff --------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.hasMsg() && sh_estim_manager_diag_.hasMsg()) { - break; - } - - ros::Duration(0.01).sleep(); - } - - ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); - - ros::Duration(1.0).sleep(); - - // | ---------------------- arm the drone --------------------- | - - ROS_INFO("[%s]: arming the drone", ros::this_node::getName().c_str()); - - { - std_srvs::SetBool arming; - arming.request.data = true; - - { - bool service_call = sch_arming_.call(arming); - - if (!service_call || !arming.response.success) { - ROS_ERROR("[%s]: arming service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------------------- wait -------------------------- | - - ros::Duration(0.2).sleep(); - - // | -------------------- midair activation ------------------- | - - ROS_INFO("[%s]: activating the drone 'in mid air'", ros::this_node::getName().c_str()); - - { - std_srvs::Trigger trigger; - - { - bool service_call = sch_midair_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: midair activation service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ----------------- sleep before the action ---------------- | - - ros::Duration(1.0).sleep(); - - // | ------------------- make odometry jump ------------------- | - - odom_jumped_ = true; - - // | -------------- wait for emergency controller ------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for EmergencyController", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->active_controller == "EmergencyController") { - break; - } - } - - // | -------------------- wait for landing -------------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for landing", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->output_enabled == false) { - break; - } - } - - return true; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, eland_innovation) { - - 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, "eland_innovation_test"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/control_manager/eland_innovation/launch/mrs_uav_system.launch b/test/control_manager/eland_innovation/launch/mrs_uav_system.launch deleted file mode 100644 index 675fe202..00000000 --- a/test/control_manager/eland_innovation/launch/mrs_uav_system.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/control_manager/eland_innovation/test.cpp b/test/control_manager/eland_innovation/test.cpp new file mode 100644 index 00000000..41af3c13 --- /dev/null +++ b/test/control_manager/eland_innovation/test.cpp @@ -0,0 +1,108 @@ +#include + +#include + +#include + +#define POS_JUMP_SIZE 4.0 + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); + + Tester(); + + mrs_lib::SubscribeHandler sh_hw_api_odom_; + + mrs_lib::PublisherHandler ph_odometry_; + + void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg); + + std::atomic odom_jumped_ = false; +}; + +Tester::Tester() : mrs_uav_testing::TestGeneric() { + + sh_hw_api_odom_ = mrs_lib::SubscribeHandler(shopts_, "/" + _uav_name_ + "/hw_api/odometry_unchanged", &Tester::callbackOdometry, this); + + ph_odometry_ = mrs_lib::PublisherHandler(nh_, "/" + _uav_name_ + "/hw_api/odometry"); +} + +void Tester::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) { + + nav_msgs::Odometry odom_out = *msg; + + if (odom_jumped_) { + odom_out.pose.pose.position.x += POS_JUMP_SIZE; + } + + ph_odometry_.publish(odom_out); +} + +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; + } + } + + sleep(1.0); + + odom_jumped_ = true; + + // | -------------- wait for the eland to trigger ------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!isFlyingNormally() && getActiveController() == "EmergencyController" && getActiveTracker() == "LandoffTracker") { + break; + } + } + + // | -------------------- wait for landing -------------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!this->isOutputEnabled()) { + return true; + } + } + + 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(); +} diff --git a/test/control_manager/eland_service/control_manager_eland_service.test b/test/control_manager/eland_service/control_manager_eland_service.test deleted file mode 100644 index c114f7b1..00000000 --- a/test/control_manager/eland_service/control_manager_eland_service.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/control_manager/eland_service/eland_service_test.cpp b/test/control_manager/eland_service/eland_service_test.cpp deleted file mode 100644 index f00afaaf..00000000 --- a/test/control_manager/eland_service/eland_service_test.cpp +++ /dev/null @@ -1,222 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include - -#define POS_JUMP_SIZE 3.0 - -/* class Tester //{ */ - -class Tester { - -public: - Tester(); - - bool test(); - -private: - ros::NodeHandle nh_; - std::shared_ptr spinner_; - - mrs_lib::SubscribeHandler sh_control_manager_diag_; - mrs_lib::SubscribeHandler sh_estim_manager_diag_; - - mrs_lib::ServiceClientHandler sch_arming_; - mrs_lib::ServiceClientHandler sch_midair_; - - mrs_lib::ServiceClientHandler sch_eland_; -}; - -//} - -/* Tester() //{ */ - -Tester::Tester() { - - // | ------------------ initialize test node ------------------ | - - nh_ = ros::NodeHandle("~"); - - ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); - - ros::Time::waitForValid(); - - spinner_ = std::make_shared(4); - spinner_->start(); - - std::string uav_name = "uav1"; - - // | ----------------------- subscribers ---------------------- | - - mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "Test"; - shopts.no_message_timeout = mrs_lib::no_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - - sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); - sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/estimation_manager/diagnostics"); - - ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str()); - - // | --------------------- service clients -------------------- | - - sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/arming"); - sch_midair_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/uav_manager/midair_activation"); - - sch_eland_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/control_manager/eland"); - - ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); -} - -//} - -/* test() //{ */ - -bool Tester::test() { - - // | ---------------- wait for ready to takeoff --------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.hasMsg() && sh_estim_manager_diag_.hasMsg()) { - break; - } - - ros::Duration(0.01).sleep(); - } - - ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); - - ros::Duration(1.0).sleep(); - - // | ---------------------- arm the drone --------------------- | - - ROS_INFO("[%s]: arming the edrone", ros::this_node::getName().c_str()); - - { - std_srvs::SetBool arming; - arming.request.data = true; - - { - bool service_call = sch_arming_.call(arming); - - if (!service_call || !arming.response.success) { - ROS_ERROR("[%s]: arming service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------------------- wait -------------------------- | - - ros::Duration(0.2).sleep(); - - // | -------------------- midair activation ------------------- | - - ROS_INFO("[%s]: activating the drone 'in mid air'", ros::this_node::getName().c_str()); - - { - std_srvs::Trigger trigger; - - { - bool service_call = sch_midair_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: midair activation service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ----------------- sleep before the action ---------------- | - - ros::Duration(1.0).sleep(); - - // | ----------------- call the eland service ----------------- | - - { - std_srvs::Trigger trigger; - - { - bool service_call = sch_eland_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: eland service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------- wait for emergency controller ------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for EmergencyController", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->active_controller == "EmergencyController") { - break; - } - } - - // | -------------------- wait for landing -------------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for landing", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->output_enabled == false) { - break; - } - } - - return true; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, eland_service) { - - 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, "eland_service_test"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/control_manager/failsafe_control_error/CMakeLists.txt b/test/control_manager/failsafe_control_error/CMakeLists.txt new file mode 100644 index 00000000..bf8f9853 --- /dev/null +++ b/test/control_manager/failsafe_control_error/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/control_manager/failsafe_control_error/config/custom_config.yaml b/test/control_manager/failsafe_control_error/config/custom_config.yaml index 7f4d9f91..185f2c96 100644 --- a/test/control_manager/failsafe_control_error/config/custom_config.yaml +++ b/test/control_manager/failsafe_control_error/config/custom_config.yaml @@ -9,15 +9,15 @@ mrs_uav_managers: # loaded state estimator plugins state_estimators: [ - "passthrough", + "gps_baro", ] - initial_state_estimator: "passthrough" # will be used as the first state estimator + 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: MpcController: eland_threshold: 0.0 # [m], position error triggering eland - failsafe_threshold: 0.1 # [m], position error triggering failsafe land + failsafe_threshold: 0.2 # [m], position error triggering failsafe land odometry_innovation_threshold: 0.0 # [m], position odometry innovation threshold diff --git a/test/control_manager/failsafe_control_error/config/network_config.yaml b/test/control_manager/failsafe_control_error/config/network_config.yaml deleted file mode 100644 index 08f370d6..00000000 --- a/test/control_manager/failsafe_control_error/config/network_config.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# 1. This list is used by NimbroNetwork for mutual communication of the UAVs -# The names of the robots have to match hostnames described in /etc/hosts. -# -# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. -# The names should match the true "UAV_NAMES" (the topic prefixes). -# -# network_config:=~/config/network_config.yaml -# -# to the core.launch and nimbro.launch. - -network: - - robot_names: [ - uav1, - ] diff --git a/test/control_manager/failsafe_control_error/config/simulator.yaml b/test/control_manager/failsafe_control_error/config/simulator.yaml deleted file mode 100644 index 7bdd82f3..00000000 --- a/test/control_manager/failsafe_control_error/config/simulator.yaml +++ /dev/null @@ -1,20 +0,0 @@ -simulation_rate: 250.0 -clock_rate: 250.0 -realtime_factor: 1.0 - -iterate_without_input: true - -individual_takeoff_platform: - enabled: true - -uav_names: [ - "uav1", -] - -uav1: - type: "x500" - spawn: - x: 5.0 - y: 5.0 - z: 2.0 - heading: 0 diff --git a/test/control_manager/failsafe_control_error/config/world_config.yaml b/test/control_manager/failsafe_control_error/config/world_config.yaml deleted file mode 100644 index 00e01eef..00000000 --- a/test/control_manager/failsafe_control_error/config/world_config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -world_origin: - - units: "LATLON" # {"UTM, "LATLON"} - - origin_x: 47.397743 - origin_y: 8.545594 - -safety_area: - - enabled: false - - horizontal: - - # the frame of reference in which the points are expressed - frame_name: "world_origin" - - # polygon - # - # x, y [m] for any frame_name except latlon_origin - # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" - points: [ - -50, -50, - 50, -50, - 50, 50, - -50, 50, - ] - - vertical: - - # the frame of reference in which the max&min z is expressed - frame_name: "world_origin" - - max_z: 15.0 - min_z: 0.5 diff --git a/test/control_manager/failsafe_control_error/control_manager_failsafe_control_error.test b/test/control_manager/failsafe_control_error/control_manager_failsafe_control_error.test deleted file mode 100644 index aa4ac32c..00000000 --- a/test/control_manager/failsafe_control_error/control_manager_failsafe_control_error.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/control_manager/failsafe_control_error/failsafe_control_error.test b/test/control_manager/failsafe_control_error/failsafe_control_error.test new file mode 100644 index 00000000..c17dfb02 --- /dev/null +++ b/test/control_manager/failsafe_control_error/failsafe_control_error.test @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/failsafe_control_error/failsafe_control_error_test.cpp b/test/control_manager/failsafe_control_error/failsafe_control_error_test.cpp deleted file mode 100644 index dbc35f85..00000000 --- a/test/control_manager/failsafe_control_error/failsafe_control_error_test.cpp +++ /dev/null @@ -1,224 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include - -/* class Tester //{ */ - -class Tester { - -public: - Tester(); - - bool test(); - -private: - ros::NodeHandle nh_; - std::shared_ptr spinner_; - - mrs_lib::SubscribeHandler sh_control_manager_diag_; - mrs_lib::SubscribeHandler sh_estim_manager_diag_; - - mrs_lib::ServiceClientHandler sch_arming_; - mrs_lib::ServiceClientHandler sch_midair_; - - mrs_lib::ServiceClientHandler sch_goto_; -}; - -//} - -/* Tester() //{ */ - -Tester::Tester() { - - // | ------------------ initialize test node ------------------ | - - nh_ = ros::NodeHandle("~"); - - ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); - - ros::Time::waitForValid(); - - spinner_ = std::make_shared(4); - spinner_->start(); - - std::string uav_name = "uav1"; - - // | ----------------------- subscribers ---------------------- | - - mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "Test"; - shopts.no_message_timeout = mrs_lib::no_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - - sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); - sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/estimation_manager/diagnostics"); - - ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str()); - - // | --------------------- service clients -------------------- | - - sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/arming"); - sch_midair_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/uav_manager/midair_activation"); - - sch_goto_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/control_manager/goto_relative"); - - ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); -} - -//} - -/* test() //{ */ - -bool Tester::test() { - - // | ---------------- wait for ready to takeoff --------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.hasMsg() && sh_estim_manager_diag_.hasMsg()) { - break; - } - - ros::Duration(0.01).sleep(); - } - - ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); - - ros::Duration(1.0).sleep(); - - // | ---------------------- arm the drone --------------------- | - - ROS_INFO("[%s]: arming the drone", ros::this_node::getName().c_str()); - - { - std_srvs::SetBool arming; - arming.request.data = true; - - { - bool service_call = sch_arming_.call(arming); - - if (!service_call || !arming.response.success) { - ROS_ERROR("[%s]: arming service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------------------- wait -------------------------- | - - ros::Duration(0.2).sleep(); - - // | -------------------- midair activation ------------------- | - - ROS_INFO("[%s]: activating the drone 'in mid air'", ros::this_node::getName().c_str()); - - { - std_srvs::Trigger trigger; - - { - bool service_call = sch_midair_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: midair activation service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ----------------- sleep before the action ---------------- | - - ros::Duration(1.0).sleep(); - - // | -------------------------- goto -------------------------- | - - { - mrs_msgs::Vec4 goto_cmd; - - goto_cmd.request.goal[0] = 5.0; - - ROS_INFO("[%s]: calling goto", ros::this_node::getName().c_str()); - - { - bool service_call = sch_goto_.call(goto_cmd); - - if (!service_call || !goto_cmd.response.success) { - ROS_ERROR("[%s]: goto service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ------------- waiting for failsafe controller ------------ | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for FailsafeController", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->active_controller == "FailsafeController") { - break; - } - } - - // | -------------------- wait for landing -------------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for landing", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->output_enabled == false) { - break; - } - } - - return true; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, failsafe_control_error) { - - 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, "failsafe_control_error_test"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/control_manager/failsafe_control_error/launch/mrs_uav_system.launch b/test/control_manager/failsafe_control_error/launch/mrs_uav_system.launch deleted file mode 100644 index f8354f47..00000000 --- a/test/control_manager/failsafe_control_error/launch/mrs_uav_system.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/control_manager/failsafe_control_error/test.cpp b/test/control_manager/failsafe_control_error/test.cpp new file mode 100644 index 00000000..7843b7a7 --- /dev/null +++ b/test/control_manager/failsafe_control_error/test.cpp @@ -0,0 +1,81 @@ +#include + +#include + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); +}; + +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; + } + } + + { + auto [success, message] = this->gotoRel(2, 2, 2, 0); + + if (success) { + ROS_ERROR("[%s]: goto succeeded, that should not happen in this case: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | ------------ wait for the failsafe to trigger ------------ | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!isFlyingNormally() && getActiveController() == "FailsafeController") { + break; + } + } + + // | -------------------- wait for landing -------------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!this->isOutputEnabled()) { + return true; + } + } + + 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(); +} diff --git a/test/control_manager/goto_service/CMakeLists.txt b/test/control_manager/goto_service/CMakeLists.txt new file mode 100644 index 00000000..bf8f9853 --- /dev/null +++ b/test/control_manager/goto_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) diff --git a/test/control_manager/goto_service/control_manager_goto_service.test b/test/control_manager/goto_service/control_manager_goto_service.test deleted file mode 100644 index f4689207..00000000 --- a/test/control_manager/goto_service/control_manager_goto_service.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/control_manager/goto_service/goto_service.test b/test/control_manager/goto_service/goto_service.test new file mode 100644 index 00000000..604b7439 --- /dev/null +++ b/test/control_manager/goto_service/goto_service.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/goto_service/goto_service_test.cpp b/test/control_manager/goto_service/goto_service_test.cpp deleted file mode 100644 index 5122700a..00000000 --- a/test/control_manager/goto_service/goto_service_test.cpp +++ /dev/null @@ -1,230 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include - -#define POS_JUMP_SIZE 3.0 - -/* class Tester //{ */ - -class Tester { - -public: - Tester(); - - bool test(); - -private: - ros::NodeHandle nh_; - std::shared_ptr spinner_; - - mrs_lib::SubscribeHandler sh_control_manager_diag_; - mrs_lib::SubscribeHandler sh_estim_manager_diag_; - - mrs_lib::ServiceClientHandler sch_arming_; - mrs_lib::ServiceClientHandler sch_midair_; - - mrs_lib::ServiceClientHandler sch_goto_; -}; - -//} - -/* Tester() //{ */ - -Tester::Tester() { - - // | ------------------ initialize test node ------------------ | - - nh_ = ros::NodeHandle("~"); - - ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); - - ros::Time::waitForValid(); - - spinner_ = std::make_shared(4); - spinner_->start(); - - std::string uav_name = "uav1"; - - // | ----------------------- subscribers ---------------------- | - - mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "Test"; - shopts.no_message_timeout = mrs_lib::no_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - - sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); - sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/estimation_manager/diagnostics"); - - ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str()); - - // | --------------------- service clients -------------------- | - - sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/arming"); - sch_midair_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/uav_manager/midair_activation"); - - sch_goto_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/control_manager/goto"); - - ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); -} - -//} - -/* test() //{ */ - -bool Tester::test() { - - // | ---------------- wait for ready to takeoff --------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.hasMsg() && sh_estim_manager_diag_.hasMsg()) { - break; - } - - ros::Duration(0.01).sleep(); - } - - ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); - - ros::Duration(1.0).sleep(); - - // | ---------------------- arm the drone --------------------- | - - ROS_INFO("[%s]: arming the drone", ros::this_node::getName().c_str()); - - { - std_srvs::SetBool arming; - arming.request.data = true; - - { - bool service_call = sch_arming_.call(arming); - - if (!service_call || !arming.response.success) { - ROS_ERROR("[%s]: arming service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------------------- wait -------------------------- | - - ros::Duration(0.2).sleep(); - - // | -------------------- midair activation ------------------- | - - ROS_INFO("[%s]: activating the drone 'in mid air'", ros::this_node::getName().c_str()); - - { - std_srvs::Trigger trigger; - - { - bool service_call = sch_midair_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: midair activation service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ----------------- sleep before the action ---------------- | - - ros::Duration(1.0).sleep(); - - // | ------------------------ test goto ----------------------- | - - mrs_msgs::Vec4 goto_cmd; - - { - goto_cmd.request.goal[0] = -10; - goto_cmd.request.goal[1] = -20; - goto_cmd.request.goal[2] = 5.5; - goto_cmd.request.goal[3] = 2.2; - - ROS_INFO("[%s]: calling goto", ros::this_node::getName().c_str()); - - { - bool service_call = sch_goto_.call(goto_cmd); - - if (!service_call || !goto_cmd.response.success) { - ROS_ERROR("[%s]: goto service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ------------------ check for the result ------------------ | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for goto", ros::this_node::getName().c_str()); - - auto diag = sh_estim_manager_diag_.getMsg(); - - auto hdg = mrs_lib::AttitudeConverter(diag->pose.orientation).getHeading(); - - auto flying_normally = sh_control_manager_diag_.getMsg()->flying_normally; - - if (std::abs(goto_cmd.request.goal[0] - diag->pose.position.x) < 1e-1 && std::abs(goto_cmd.request.goal[1] - diag->pose.position.y) < 1e-1 && - std::abs(goto_cmd.request.goal[2] - diag->pose.position.z) < 1e-1 && std::abs(goto_cmd.request.goal[3] - hdg) < 1e-1 && flying_normally) { - - ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); - - return true; - } - - ros::Duration(0.01).sleep(); - } - - return false; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, goto_service) { - - 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, "goto_service_test"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/control_manager/goto_service/test.cpp b/test/control_manager/goto_service/test.cpp new file mode 100644 index 00000000..c463a94b --- /dev/null +++ b/test/control_manager/goto_service/test.cpp @@ -0,0 +1,62 @@ +#include + +#include + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); +}; + +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; + } + } + + { + auto [success, message] = this->gotoAbs(0, 0, 2.0, 0); + + if (!success) { + ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + this->sleep(5.0); + + if (this->isFlyingNormally()) { + return true; + } else { + ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); + 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(); +} diff --git a/test/coverage.sh b/test/coverage.sh index 38093c5f..9e75c239 100755 --- a/test/coverage.sh +++ b/test/coverage.sh @@ -1,10 +1,5 @@ #!/bin/bash -PACKAGE_NAME=mrs_uav_managers - -# find the build folder of the current workspace -roscd $PACKAGE_NAME - while [ ! -e ".catkin_tools" ]; do cd .. if [[ `pwd` == "/" ]]; then @@ -14,11 +9,13 @@ while [ ! -e ".catkin_tools" ]; do fi done +WORKSPACE_NAME=${PWD##*/} + cd build lcov --capture --directory . --output-file coverage.info lcov --remove coverage.info "*/test/*" --output-file coverage.info.removed -lcov --extract coverage.info.removed "*/*workspace/*" --output-file coverage.info.cleaned +lcov --extract coverage.info.removed "*/${WORKSPACE_NAME}/src/*" --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}'` diff --git a/test/sessions/x500_in_mid_air/config/custom_config.yaml b/test/sessions/x500_in_mid_air/config/custom_config.yaml deleted file mode 100644 index 9f6bbb87..00000000 --- a/test/sessions/x500_in_mid_air/config/custom_config.yaml +++ /dev/null @@ -1,16 +0,0 @@ -# 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: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) diff --git a/test/sessions/x500_in_mid_air/config/network_config.yaml b/test/sessions/x500_in_mid_air/config/network_config.yaml deleted file mode 100644 index 08f370d6..00000000 --- a/test/sessions/x500_in_mid_air/config/network_config.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# 1. This list is used by NimbroNetwork for mutual communication of the UAVs -# The names of the robots have to match hostnames described in /etc/hosts. -# -# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. -# The names should match the true "UAV_NAMES" (the topic prefixes). -# -# network_config:=~/config/network_config.yaml -# -# to the core.launch and nimbro.launch. - -network: - - robot_names: [ - uav1, - ] diff --git a/test/sessions/x500_in_mid_air/config/simulator.yaml b/test/sessions/x500_in_mid_air/config/simulator.yaml deleted file mode 100644 index 196c2e8d..00000000 --- a/test/sessions/x500_in_mid_air/config/simulator.yaml +++ /dev/null @@ -1,20 +0,0 @@ -simulation_rate: 250.0 -clock_rate: 250.0 -realtime_factor: 1.0 - -iterate_without_input: true - -individual_takeoff_platform: - enabled: true - -uav_names: [ - "uav1", -] - -uav1: - type: "x500" - spawn: - x: 10.0 - y: 15.0 - z: 2.0 - heading: 0 diff --git a/test/sessions/x500_in_mid_air/config/world_config.yaml b/test/sessions/x500_in_mid_air/config/world_config.yaml deleted file mode 100644 index 9b550677..00000000 --- a/test/sessions/x500_in_mid_air/config/world_config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -world_origin: - - units: "LATLON" # {"UTM, "LATLON"} - - origin_x: 47.397743 - origin_y: 8.545594 - -safety_area: - - enabled: true - - horizontal: - - # the frame of reference in which the points are expressed - frame_name: "world_origin" - - # polygon - # - # x, y [m] for any frame_name except latlon_origin - # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" - points: [ - -50, -50, - 50, -50, - 50, 50, - -50, 50, - ] - - vertical: - - # the frame of reference in which the max&min z is expressed - frame_name: "world_origin" - - max_z: 15.0 - min_z: 0.5 diff --git a/test/sessions/x500_in_mid_air/launch/mrs_uav_system.launch b/test/sessions/x500_in_mid_air/launch/mrs_uav_system.launch deleted file mode 100644 index 83567506..00000000 --- a/test/sessions/x500_in_mid_air/launch/mrs_uav_system.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/single_test.sh b/test/single_test.sh deleted file mode 100755 index 4505f41c..00000000 --- a/test/single_test.sh +++ /dev/null @@ -1,27 +0,0 @@ -#!/bin/bash - -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_managers" -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 - -# folder for test results -TEST_RESULT_PATH=$(realpath /tmp/$RANDOM) -mkdir -p $TEST_RESULT_PATH - -# run the test -rostest $PACKAGE control_manager_eland_control_error.test $TEXT_OUTPUT --results-filename=$PACKAGE.test --results-base-dir="$TEST_RESULT_PATH" - -# evaluate the test results -echo test result path is $TEST_RESULT_PATH -catkin_test_results "$TEST_RESULT_PATH" diff --git a/test/uav_manager/CMakeLists.txt b/test/uav_manager/CMakeLists.txt new file mode 100644 index 00000000..5c1fda7a --- /dev/null +++ b/test/uav_manager/CMakeLists.txt @@ -0,0 +1,3 @@ +add_subdirectory(takeoff) + +add_subdirectory(landing) diff --git a/test/uav_manager/landing/CMakeLists.txt b/test/uav_manager/landing/CMakeLists.txt new file mode 100644 index 00000000..bf8f9853 --- /dev/null +++ b/test/uav_manager/landing/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/landing/landing.test b/test/uav_manager/landing/landing.test new file mode 100644 index 00000000..604b7439 --- /dev/null +++ b/test/uav_manager/landing/landing.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/uav_manager/landing/landing_test.cpp b/test/uav_manager/landing/landing_test.cpp deleted file mode 100644 index 33c41f78..00000000 --- a/test/uav_manager/landing/landing_test.cpp +++ /dev/null @@ -1,217 +0,0 @@ -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include - -/* class Tester //{ */ - -class Tester { - -public: - Tester(); - - bool test(); - -private: - ros::NodeHandle nh_; - std::shared_ptr spinner_; - - mrs_lib::SubscribeHandler sh_control_manager_diag_; - mrs_lib::SubscribeHandler sh_estim_manager_diag_; - - mrs_lib::ServiceClientHandler sch_arming_; - mrs_lib::ServiceClientHandler sch_midair_; - - mrs_lib::ServiceClientHandler sch_land_; -}; - -//} - -/* Tester() //{ */ - -Tester::Tester() { - - // | ------------------ initialize test node ------------------ | - - nh_ = ros::NodeHandle("~"); - - ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); - - ros::Time::waitForValid(); - - spinner_ = std::make_shared(4); - spinner_->start(); - - std::string uav_name = "uav1"; - - // | ----------------------- subscribers ---------------------- | - - mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "Test"; - shopts.no_message_timeout = mrs_lib::no_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - - sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); - sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/estimation_manager/diagnostics"); - - ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str()); - - // | --------------------- service clients -------------------- | - - sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/arming"); - sch_midair_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/uav_manager/midair_activation"); - - sch_land_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/uav_manager/land"); - - ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); -} - -//} - -/* test() //{ */ - -bool Tester::test() { - - // | ---------------- wait for ready to takeoff --------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.hasMsg() && sh_estim_manager_diag_.hasMsg()) { - break; - } - - ros::Duration(0.01).sleep(); - } - - ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); - - ros::Duration(1.0).sleep(); - - // | ---------------------- arm the drone --------------------- | - - ROS_INFO("[%s]: arming the edrone", ros::this_node::getName().c_str()); - - { - std_srvs::SetBool arming; - arming.request.data = true; - - { - bool service_call = sch_arming_.call(arming); - - if (!service_call || !arming.response.success) { - ROS_ERROR("[%s]: arming service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------------------- wait -------------------------- | - - ros::Duration(0.2).sleep(); - - // | -------------------- midair activation ------------------- | - - ROS_INFO("[%s]: activating the drone 'in mid air'", ros::this_node::getName().c_str()); - - { - std_srvs::Trigger trigger; - - { - bool service_call = sch_midair_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: midair activation service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ----------------- sleep before the action ---------------- | - - ros::Duration(1.0).sleep(); - - // | ---------------------- call landing ---------------------- | - - { - std_srvs::Trigger trigger; - - { - bool service_call = sch_land_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: land service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ----------------- wait for LandoffTracker ---------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for LandoffTracker", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->active_tracker == "LandoffTracker") { - ROS_INFO("[%s]: LandoffTracker detected", ros::this_node::getName().c_str()); - break; - } - } - - // | -------------------- wait for landing -------------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for landing", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->output_enabled == false) { - ROS_INFO("[%s]: landing detected", ros::this_node::getName().c_str()); - return true; - } - } - - return false; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, landing) { - - 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, "landing_test"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/uav_manager/landing/test.cpp b/test/uav_manager/landing/test.cpp new file mode 100644 index 00000000..ccc13a57 --- /dev/null +++ b/test/uav_manager/landing/test.cpp @@ -0,0 +1,100 @@ +#include + +#include + +#include + +#define POS_JUMP_SIZE 3.0 + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); + + Tester(); + + mrs_lib::ServiceClientHandler sch_land_; +}; + +Tester::Tester() : mrs_uav_testing::TestGeneric() { + + sch_land_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/uav_manager/land"); +} + +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; + } + } + + sleep(1.0); + + // | -------------- wait for the eland to trigger ------------- | + + { + std_srvs::Trigger srv; + + { + bool service_call = sch_land_.call(srv); + + if (!service_call || !srv.response.success) { + ROS_ERROR("[%s]: land service call failed", ros::this_node::getName().c_str()); + return false; + } + } + } + + sleep(0.5); + + // | -------------------- wait for landing -------------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!isOutputEnabled()) { + + return true; + + } else { + + // TODO this is not the right way to check if the landing actually happens as planned + if (getActiveTracker() != "LandoffTracker" && getActiveTracker() != "NullTracker") { + ROS_ERROR("[%s]: not landing anymore", ros::this_node::getName().c_str()); + return false; + } + } + } + + 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(); +} diff --git a/test/uav_manager/landing/uav_manager_landing.test b/test/uav_manager/landing/uav_manager_landing.test deleted file mode 100644 index 34ed5bfd..00000000 --- a/test/uav_manager/landing/uav_manager_landing.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/uav_manager/takeoff/CMakeLists.txt b/test/uav_manager/takeoff/CMakeLists.txt new file mode 100644 index 00000000..bf8f9853 --- /dev/null +++ b/test/uav_manager/takeoff/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/takeoff/config/custom_config.yaml b/test/uav_manager/takeoff/config/custom_config.yaml deleted file mode 100644 index 9f6bbb87..00000000 --- a/test/uav_manager/takeoff/config/custom_config.yaml +++ /dev/null @@ -1,16 +0,0 @@ -# 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: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) diff --git a/test/uav_manager/takeoff/config/mrs_simulator.yaml b/test/uav_manager/takeoff/config/mrs_simulator.yaml new file mode 100644 index 00000000..7704c54b --- /dev/null +++ b/test/uav_manager/takeoff/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/takeoff/config/network_config.yaml b/test/uav_manager/takeoff/config/network_config.yaml deleted file mode 100644 index 08f370d6..00000000 --- a/test/uav_manager/takeoff/config/network_config.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# 1. This list is used by NimbroNetwork for mutual communication of the UAVs -# The names of the robots have to match hostnames described in /etc/hosts. -# -# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. -# The names should match the true "UAV_NAMES" (the topic prefixes). -# -# network_config:=~/config/network_config.yaml -# -# to the core.launch and nimbro.launch. - -network: - - robot_names: [ - uav1, - ] diff --git a/test/uav_manager/takeoff/config/simulator.yaml b/test/uav_manager/takeoff/config/simulator.yaml deleted file mode 100644 index c8f10602..00000000 --- a/test/uav_manager/takeoff/config/simulator.yaml +++ /dev/null @@ -1,20 +0,0 @@ -simulation_rate: 250.0 -clock_rate: 250.0 -realtime_factor: 1.0 - -iterate_without_input: true - -individual_takeoff_platform: - enabled: false - -uav_names: [ - "uav1", -] - -uav1: - type: "x500" - spawn: - x: 10.0 - y: 15.0 - z: 2.0 - heading: 0 diff --git a/test/uav_manager/takeoff/config/world_config.yaml b/test/uav_manager/takeoff/config/world_config.yaml deleted file mode 100644 index 9b550677..00000000 --- a/test/uav_manager/takeoff/config/world_config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -world_origin: - - units: "LATLON" # {"UTM, "LATLON"} - - origin_x: 47.397743 - origin_y: 8.545594 - -safety_area: - - enabled: true - - horizontal: - - # the frame of reference in which the points are expressed - frame_name: "world_origin" - - # polygon - # - # x, y [m] for any frame_name except latlon_origin - # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" - points: [ - -50, -50, - 50, -50, - 50, 50, - -50, 50, - ] - - vertical: - - # the frame of reference in which the max&min z is expressed - frame_name: "world_origin" - - max_z: 15.0 - min_z: 0.5 diff --git a/test/uav_manager/takeoff/launch/mrs_uav_system.launch b/test/uav_manager/takeoff/launch/mrs_uav_system.launch deleted file mode 100644 index 4f0adef8..00000000 --- a/test/uav_manager/takeoff/launch/mrs_uav_system.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/uav_manager/takeoff/takeoff.test b/test/uav_manager/takeoff/takeoff.test new file mode 100644 index 00000000..4d31f7f3 --- /dev/null +++ b/test/uav_manager/takeoff/takeoff.test @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/uav_manager/takeoff/takeoff_test.cpp b/test/uav_manager/takeoff/takeoff_test.cpp deleted file mode 100644 index 5b740db4..00000000 --- a/test/uav_manager/takeoff/takeoff_test.cpp +++ /dev/null @@ -1,189 +0,0 @@ -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include - -/* class Tester //{ */ - -class Tester { - -public: - Tester(); - - bool test(); - -private: - ros::NodeHandle nh_; - std::shared_ptr spinner_; - - mrs_lib::SubscribeHandler sh_control_manager_diag_; - mrs_lib::SubscribeHandler sh_estim_manager_diag_; - mrs_lib::SubscribeHandler sh_can_takeoff; - - mrs_lib::ServiceClientHandler sch_arming_; - mrs_lib::ServiceClientHandler sch_offboard_; - - mrs_lib::ServiceClientHandler sch_land_; -}; - -//} - -/* Tester() //{ */ - -Tester::Tester() { - - // | ------------------ initialize test node ------------------ | - - nh_ = ros::NodeHandle("~"); - - ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); - - ros::Time::waitForValid(); - - spinner_ = std::make_shared(4); - spinner_->start(); - - std::string uav_name = "uav1"; - - // | ----------------------- subscribers ---------------------- | - - mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "Test"; - shopts.no_message_timeout = mrs_lib::no_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - - sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); - sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/estimation_manager/diagnostics"); - sh_can_takeoff = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/automatic_start/can_takeoff"); - - ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str()); - - // | --------------------- service clients -------------------- | - - sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/arming"); - sch_offboard_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/offboard"); - - ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); -} - -//} - -/* test() //{ */ - -bool Tester::test() { - - // | ---------------- wait for ready to takeoff --------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.hasMsg() && sh_estim_manager_diag_.hasMsg()) { - break; - } - - ros::Duration(0.01).sleep(); - } - - ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); - - ros::Duration(1.0).sleep(); - - // | ---------------------- arm the drone --------------------- | - - ROS_INFO("[%s]: arming the edrone", ros::this_node::getName().c_str()); - - { - std_srvs::SetBool arming; - arming.request.data = true; - - { - bool service_call = sch_arming_.call(arming); - - if (!service_call || !arming.response.success) { - ROS_ERROR("[%s]: arming service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------------------- wait -------------------------- | - - ros::Duration(0.2).sleep(); - - // | -------------------- trigger offboard -------------------- | - - ROS_INFO("[%s]: triggering offboard", ros::this_node::getName().c_str()); - - { - std_srvs::Trigger trigger; - - { - bool service_call = sch_offboard_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: offboard service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | -------------- wait for the takeoff finished ------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the takeoff to finish", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.getMsg()->flying_normally) { - - ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); - - return true; - } - - ros::Duration(0.01).sleep(); - } - - return false; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, takeoff) { - - 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, "takeoff_test"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/uav_manager/takeoff/test.cpp b/test/uav_manager/takeoff/test.cpp new file mode 100644 index 00000000..13b85997 --- /dev/null +++ b/test/uav_manager/takeoff/test.cpp @@ -0,0 +1,51 @@ +#include + +#include + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); +}; + +bool Tester::test() { + + auto [success, message] = takeoff(); + + if (!success) { + ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + + this->sleep(5.0); + + if (this->isFlyingNormally()) { + return true; + } else { + ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); + 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(); +} diff --git a/test/uav_manager/takeoff/uav_manager_takeoff.test b/test/uav_manager/takeoff/uav_manager_takeoff.test deleted file mode 100644 index 3d55db90..00000000 --- a/test/uav_manager/takeoff/uav_manager_takeoff.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - -