diff --git a/CMakeLists.txt b/CMakeLists.txt index 97ae98c..452763d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ set(CATKIN_DEPENDENCIES sensor_msgs std_msgs tf2_geometry_msgs + mrs_uav_testing ) find_package(catkin REQUIRED COMPONENTS diff --git a/package.xml b/package.xml index f66dbc6..6b45f30 100644 --- a/package.xml +++ b/package.xml @@ -27,6 +27,7 @@ sensor_msgs std_msgs tf2_geometry_msgs + mrs_uav_testing pluginlib tf2_ros diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 4dfdc5e..ac7795b 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,83 +1,9 @@ find_package(rostest REQUIRED) -# Corrections test +add_subdirectory(gps_baro) -## DOES NOT COMPILE -# add_rostest_gtest(CorrectionsTest -# corrections/corrections.test -# corrections/test_corrections.cpp -# ) +add_subdirectory(gps_garmin) -# target_link_libraries(CorrectionsTest -# mrs_lib -# ${catkin_LIBRARIES} -# ) +add_subdirectory(rtk) -# add_dependencies(CorrectionsTest -# ${${PROJECT_NAME}_EXPORTED_TARGETS} -# ${catkin_EXPORTED_TARGETS} -# ) - -# GpsBaro basic - -add_rostest_gtest(GpsBaro_basic - gps_baro/basic/gps_baro_basic.test - gps_baro/basic/test.cpp - ) - -target_link_libraries(GpsBaro_basic - ${catkin_LIBRARIES} - ) - -add_dependencies(GpsBaro_basic - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -# GpsGarmin basic - -add_rostest_gtest(GpsGarmin_basic - gps_garmin/basic/gps_garmin_basic.test - gps_garmin/basic/test.cpp - ) - -target_link_libraries(GpsGarmin_basic - ${catkin_LIBRARIES} - ) - -add_dependencies(GpsGarmin_basic - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -# Rtk basic - -add_rostest_gtest(Rtk_basic - rtk/basic/rtk_basic.test - rtk/basic/test.cpp - ) - -target_link_libraries(Rtk_basic - ${catkin_LIBRARIES} - ) - -add_dependencies(Rtk_basic - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -# RtkGarmin basic - -add_rostest_gtest(RtkGarmin_basic - rtk_garmin/basic/rtk_garmin_basic.test - rtk_garmin/basic/test.cpp - ) - -target_link_libraries(RtkGarmin_basic - ${catkin_LIBRARIES} - ) - -add_dependencies(RtkGarmin_basic - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) +add_subdirectory(rtk_garmin) diff --git a/test/all_tests.sh b/test/all_tests.sh index 9aef441..aa6ca4e 100755 --- a/test/all_tests.sh +++ b/test/all_tests.sh @@ -5,31 +5,8 @@ set -e trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR -PACKAGE="mrs_uav_state_estimators" -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") - -for TEST_FILE in `echo $TEST_FILES`; do - - echo "$0: running test '$TEST_FILE'" - - # folder for test results - TEST_RESULT_PATH=$(realpath /tmp/$RANDOM) - mkdir -p $TEST_RESULT_PATH - - # run the test - rostest $PACKAGE $TEST_FILE $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" +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/compile_tests.sh b/test/compile_tests.sh new file mode 100755 index 0000000..999699e --- /dev/null +++ b/test/compile_tests.sh @@ -0,0 +1,10 @@ +#!/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 + +# 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 diff --git a/test/coverage.sh b/test/coverage.sh deleted file mode 100755 index 8e71309..0000000 --- a/test/coverage.sh +++ /dev/null @@ -1,25 +0,0 @@ -#!/bin/bash - -PACKAGE_NAME=mrs_uav_state_estimators - -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 - -cd build - -lcov --capture --directory . --output-file coverage.info -lcov --remove coverage.info "*/test/*" --output-file coverage.info.removed -lcov --extract coverage.info.removed "*/${PACKAGE_NAME}/*" --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}'` - -echo "Coverage: $COVERAGE_PCT" - -xdg-open coverage_html/index.html diff --git a/test/gps_baro/CMakeLists.txt b/test/gps_baro/CMakeLists.txt new file mode 100644 index 0000000..322bd0f --- /dev/null +++ b/test/gps_baro/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(gps_baro_basic) diff --git a/test/gps_baro/basic/config/network_config.yaml b/test/gps_baro/basic/config/network_config.yaml deleted file mode 100644 index 08f370d..0000000 --- a/test/gps_baro/basic/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/gps_baro/basic/config/simulator.yaml b/test/gps_baro/basic/config/simulator.yaml deleted file mode 100644 index 196c2e8..0000000 --- a/test/gps_baro/basic/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/gps_baro/basic/gps_baro_basic.test b/test/gps_baro/basic/gps_baro_basic.test deleted file mode 100644 index c5c0685..0000000 --- a/test/gps_baro/basic/gps_baro_basic.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/gps_baro/basic/launch/mrs_uav_system.launch b/test/gps_baro/basic/launch/mrs_uav_system.launch deleted file mode 100644 index acc0d1a..0000000 --- a/test/gps_baro/basic/launch/mrs_uav_system.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/gps_baro/basic/test.cpp b/test/gps_baro/basic/test.cpp deleted file mode 100644 index c4d17da..0000000 --- a/test/gps_baro/basic/test.cpp +++ /dev/null @@ -1,219 +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_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 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(); - - // | ------------------------ test goto ----------------------- | - - mrs_msgs::Vec4 goto_cmd; - - { - goto_cmd.request.goal[0] = 0; - goto_cmd.request.goal[1] = 0; - 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; - } - } - } - - ros::Duration(1.0).sleep(); - - // | ------------------- check if we are ok ------------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: checking if we are still flying", ros::this_node::getName().c_str()); - - if (!sh_control_manager_diag_.getMsg()->flying_normally) { - ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); - return false; - } - - if (!sh_control_manager_diag_.getMsg()->tracker_status.have_goal) { - ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); - return true; - } - } - - return false; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, GpsBaro_basic) { - - 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, "GpsBaro_basic"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/gps_baro/gps_baro_basic/CMakeLists.txt b/test/gps_baro/gps_baro_basic/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/gps_baro/gps_baro_basic/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/gps_baro/basic/config/custom_config.yaml b/test/gps_baro/gps_baro_basic/config/custom_config.yaml similarity index 100% rename from test/gps_baro/basic/config/custom_config.yaml rename to test/gps_baro/gps_baro_basic/config/custom_config.yaml diff --git a/test/gps_baro/basic/config/hw_api.yaml b/test/gps_baro/gps_baro_basic/config/hw_api.yaml similarity index 100% rename from test/gps_baro/basic/config/hw_api.yaml rename to test/gps_baro/gps_baro_basic/config/hw_api.yaml diff --git a/test/gps_baro/gps_baro_basic/gps_baro_basic.test b/test/gps_baro/gps_baro_basic/gps_baro_basic.test new file mode 100644 index 0000000..dd26c61 --- /dev/null +++ b/test/gps_baro/gps_baro_basic/gps_baro_basic.test @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/gps_baro/gps_baro_basic/test.cpp b/test/gps_baro/gps_baro_basic/test.cpp new file mode 100644 index 0000000..50e094f --- /dev/null +++ b/test/gps_baro/gps_baro_basic/test.cpp @@ -0,0 +1,67 @@ +#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; + } + } + + if (getActiveEstimator() != "gps_baro") { + ROS_ERROR("[%s]: gps_baro estimator not active", ros::this_node::getName().c_str()); + return false; + } + + { + auto [success, message] = this->gotoRel(10, 2, 1.5, 1.5); + + 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/gps_garmin/CMakeLists.txt b/test/gps_garmin/CMakeLists.txt new file mode 100644 index 0000000..b346a14 --- /dev/null +++ b/test/gps_garmin/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(gps_garmin_basic) diff --git a/test/gps_garmin/basic/config/network_config.yaml b/test/gps_garmin/basic/config/network_config.yaml deleted file mode 100644 index 08f370d..0000000 --- a/test/gps_garmin/basic/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/gps_garmin/basic/config/simulator.yaml b/test/gps_garmin/basic/config/simulator.yaml deleted file mode 100644 index 196c2e8..0000000 --- a/test/gps_garmin/basic/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/gps_garmin/basic/gps_garmin_basic.test b/test/gps_garmin/basic/gps_garmin_basic.test deleted file mode 100644 index 71b4133..0000000 --- a/test/gps_garmin/basic/gps_garmin_basic.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/gps_garmin/basic/launch/mrs_uav_system.launch b/test/gps_garmin/basic/launch/mrs_uav_system.launch deleted file mode 100644 index 0a86b53..0000000 --- a/test/gps_garmin/basic/launch/mrs_uav_system.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/gps_garmin/basic/test.cpp b/test/gps_garmin/basic/test.cpp deleted file mode 100644 index 9e86ac2..0000000 --- a/test/gps_garmin/basic/test.cpp +++ /dev/null @@ -1,219 +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_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 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(); - - // | ------------------------ test goto ----------------------- | - - mrs_msgs::Vec4 goto_cmd; - - { - goto_cmd.request.goal[0] = 0; - goto_cmd.request.goal[1] = 0; - 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; - } - } - } - - ros::Duration(1.0).sleep(); - - // | ------------------- check if we are ok ------------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: checking if we are still flying", ros::this_node::getName().c_str()); - - if (!sh_control_manager_diag_.getMsg()->flying_normally) { - ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); - return false; - } - - if (!sh_control_manager_diag_.getMsg()->tracker_status.have_goal) { - ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); - return true; - } - } - - return false; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, GpsGarmin_basic) { - - 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, "GpsGarmin_basic"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/gps_garmin/gps_garmin_basic/CMakeLists.txt b/test/gps_garmin/gps_garmin_basic/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/gps_garmin/gps_garmin_basic/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/gps_garmin/basic/config/custom_config.yaml b/test/gps_garmin/gps_garmin_basic/config/custom_config.yaml similarity index 100% rename from test/gps_garmin/basic/config/custom_config.yaml rename to test/gps_garmin/gps_garmin_basic/config/custom_config.yaml diff --git a/test/gps_garmin/basic/config/hw_api.yaml b/test/gps_garmin/gps_garmin_basic/config/hw_api.yaml similarity index 100% rename from test/gps_garmin/basic/config/hw_api.yaml rename to test/gps_garmin/gps_garmin_basic/config/hw_api.yaml diff --git a/test/gps_garmin/gps_garmin_basic/gps_garmin_basic.test b/test/gps_garmin/gps_garmin_basic/gps_garmin_basic.test new file mode 100644 index 0000000..dd26c61 --- /dev/null +++ b/test/gps_garmin/gps_garmin_basic/gps_garmin_basic.test @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/gps_garmin/gps_garmin_basic/test.cpp b/test/gps_garmin/gps_garmin_basic/test.cpp new file mode 100644 index 0000000..bba13fa --- /dev/null +++ b/test/gps_garmin/gps_garmin_basic/test.cpp @@ -0,0 +1,67 @@ +#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; + } + } + + if (getActiveEstimator() != "gps_garmin") { + ROS_ERROR("[%s]: gps_garmin estimator not active", ros::this_node::getName().c_str()); + return false; + } + + { + auto [success, message] = this->gotoRel(10, 2, 1.5, 1.5); + + 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/rtk/CMakeLists.txt b/test/rtk/CMakeLists.txt new file mode 100644 index 0000000..cef3dba --- /dev/null +++ b/test/rtk/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(rtk_basic) diff --git a/test/rtk/basic/config/network_config.yaml b/test/rtk/basic/config/network_config.yaml deleted file mode 100644 index 08f370d..0000000 --- a/test/rtk/basic/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/rtk/basic/config/simulator.yaml b/test/rtk/basic/config/simulator.yaml deleted file mode 100644 index 196c2e8..0000000 --- a/test/rtk/basic/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/rtk/basic/config/world_config.yaml b/test/rtk/basic/config/world_config.yaml deleted file mode 100644 index 59f9c50..0000000 --- a/test/rtk/basic/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: -2.0 diff --git a/test/rtk/basic/launch/mrs_uav_system.launch b/test/rtk/basic/launch/mrs_uav_system.launch deleted file mode 100644 index 7117cbf..0000000 --- a/test/rtk/basic/launch/mrs_uav_system.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/rtk/basic/rtk_basic.test b/test/rtk/basic/rtk_basic.test deleted file mode 100644 index 03a9572..0000000 --- a/test/rtk/basic/rtk_basic.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/rtk/basic/test.cpp b/test/rtk/basic/test.cpp deleted file mode 100644 index 3ec9411..0000000 --- a/test/rtk/basic/test.cpp +++ /dev/null @@ -1,219 +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_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 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(); - - // | ------------------------ test goto ----------------------- | - - mrs_msgs::Vec4 goto_cmd; - - { - goto_cmd.request.goal[0] = 0; - goto_cmd.request.goal[1] = 0; - 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; - } - } - } - - ros::Duration(1.0).sleep(); - - // | ------------------- check if we are ok ------------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: checking if we are still flying", ros::this_node::getName().c_str()); - - if (!sh_control_manager_diag_.getMsg()->flying_normally) { - ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); - return false; - } - - if (!sh_control_manager_diag_.getMsg()->tracker_status.have_goal) { - ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); - return true; - } - } - - return false; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, Rtk_basic) { - - 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, "Rtk_basic"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/rtk/rtk_basic/CMakeLists.txt b/test/rtk/rtk_basic/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/rtk/rtk_basic/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/rtk/basic/config/custom_config.yaml b/test/rtk/rtk_basic/config/custom_config.yaml similarity index 100% rename from test/rtk/basic/config/custom_config.yaml rename to test/rtk/rtk_basic/config/custom_config.yaml diff --git a/test/rtk/basic/config/hw_api.yaml b/test/rtk/rtk_basic/config/hw_api.yaml similarity index 100% rename from test/rtk/basic/config/hw_api.yaml rename to test/rtk/rtk_basic/config/hw_api.yaml diff --git a/test/gps_garmin/basic/config/world_config.yaml b/test/rtk/rtk_basic/config/world_config.yaml similarity index 97% rename from test/gps_garmin/basic/config/world_config.yaml rename to test/rtk/rtk_basic/config/world_config.yaml index 9b55067..454e3a3 100644 --- a/test/gps_garmin/basic/config/world_config.yaml +++ b/test/rtk/rtk_basic/config/world_config.yaml @@ -31,4 +31,4 @@ safety_area: frame_name: "world_origin" max_z: 15.0 - min_z: 0.5 + min_z: -3.0 diff --git a/test/rtk/rtk_basic/rtk_basic.test b/test/rtk/rtk_basic/rtk_basic.test new file mode 100644 index 0000000..7f455e7 --- /dev/null +++ b/test/rtk/rtk_basic/rtk_basic.test @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/rtk/rtk_basic/test.cpp b/test/rtk/rtk_basic/test.cpp new file mode 100644 index 0000000..7785c71 --- /dev/null +++ b/test/rtk/rtk_basic/test.cpp @@ -0,0 +1,67 @@ +#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; + } + } + + if (getActiveEstimator() != "rtk") { + ROS_ERROR("[%s]: rtk estimator not active", ros::this_node::getName().c_str()); + return false; + } + + { + auto [success, message] = this->gotoRel(10, 2, 1.5, 1.5); + + 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/rtk_garmin/CMakeLists.txt b/test/rtk_garmin/CMakeLists.txt new file mode 100644 index 0000000..66e5895 --- /dev/null +++ b/test/rtk_garmin/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(rtk_garmin_basic) diff --git a/test/rtk_garmin/basic/config/network_config.yaml b/test/rtk_garmin/basic/config/network_config.yaml deleted file mode 100644 index 08f370d..0000000 --- a/test/rtk_garmin/basic/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/rtk_garmin/basic/config/simulator.yaml b/test/rtk_garmin/basic/config/simulator.yaml deleted file mode 100644 index 196c2e8..0000000 --- a/test/rtk_garmin/basic/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/rtk_garmin/basic/config/world_config.yaml b/test/rtk_garmin/basic/config/world_config.yaml deleted file mode 100644 index 9b55067..0000000 --- a/test/rtk_garmin/basic/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/rtk_garmin/basic/launch/mrs_uav_system.launch b/test/rtk_garmin/basic/launch/mrs_uav_system.launch deleted file mode 100644 index cb28f48..0000000 --- a/test/rtk_garmin/basic/launch/mrs_uav_system.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/rtk_garmin/basic/rtk_garmin_basic.test b/test/rtk_garmin/basic/rtk_garmin_basic.test deleted file mode 100644 index 6e27965..0000000 --- a/test/rtk_garmin/basic/rtk_garmin_basic.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/rtk_garmin/basic/test.cpp b/test/rtk_garmin/basic/test.cpp deleted file mode 100644 index 0518582..0000000 --- a/test/rtk_garmin/basic/test.cpp +++ /dev/null @@ -1,219 +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_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 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(); - - // | ------------------------ test goto ----------------------- | - - mrs_msgs::Vec4 goto_cmd; - - { - goto_cmd.request.goal[0] = 0; - goto_cmd.request.goal[1] = 0; - 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; - } - } - } - - ros::Duration(1.0).sleep(); - - // | ------------------- check if we are ok ------------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: checking if we are still flying", ros::this_node::getName().c_str()); - - if (!sh_control_manager_diag_.getMsg()->flying_normally) { - ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); - return false; - } - - if (!sh_control_manager_diag_.getMsg()->tracker_status.have_goal) { - ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); - return true; - } - } - - return false; -} - -//} - -std::shared_ptr tester_; - -TEST(TESTSuite, RtkGarmin_basic) { - - 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, "RtkGarmin_basic"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/rtk_garmin/rtk_garmin_basic/CMakeLists.txt b/test/rtk_garmin/rtk_garmin_basic/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/rtk_garmin/rtk_garmin_basic/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/rtk_garmin/basic/config/custom_config.yaml b/test/rtk_garmin/rtk_garmin_basic/config/custom_config.yaml similarity index 100% rename from test/rtk_garmin/basic/config/custom_config.yaml rename to test/rtk_garmin/rtk_garmin_basic/config/custom_config.yaml diff --git a/test/rtk_garmin/basic/config/hw_api.yaml b/test/rtk_garmin/rtk_garmin_basic/config/hw_api.yaml similarity index 100% rename from test/rtk_garmin/basic/config/hw_api.yaml rename to test/rtk_garmin/rtk_garmin_basic/config/hw_api.yaml diff --git a/test/gps_baro/basic/config/world_config.yaml b/test/rtk_garmin/rtk_garmin_basic/config/world_config.yaml similarity index 100% rename from test/gps_baro/basic/config/world_config.yaml rename to test/rtk_garmin/rtk_garmin_basic/config/world_config.yaml diff --git a/test/rtk_garmin/rtk_garmin_basic/rtk_garmin_basic.test b/test/rtk_garmin/rtk_garmin_basic/rtk_garmin_basic.test new file mode 100644 index 0000000..7f455e7 --- /dev/null +++ b/test/rtk_garmin/rtk_garmin_basic/rtk_garmin_basic.test @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/rtk_garmin/rtk_garmin_basic/test.cpp b/test/rtk_garmin/rtk_garmin_basic/test.cpp new file mode 100644 index 0000000..a53b9d9 --- /dev/null +++ b/test/rtk_garmin/rtk_garmin_basic/test.cpp @@ -0,0 +1,67 @@ +#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; + } + } + + if (getActiveEstimator() != "rtk_garmin") { + ROS_ERROR("[%s]: rtk_garmin estimator not active", ros::this_node::getName().c_str()); + return false; + } + + { + auto [success, message] = this->gotoRel(10, 2, 1.5, 1.5); + + 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/single_test.sh b/test/single_test.sh deleted file mode 100755 index 368f046..0000000 --- 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_state_estimators" -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 ./rtk_garmin/basic/rtk_garmin_basic.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"