From 92e6a42f66ee129a309a5bf923fe31dfbfb94420 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Wed, 22 Nov 2023 08:11:15 +0100 Subject: [PATCH] updated tests --- src/control_manager/control_manager.cpp | 1 + test/CMakeLists.txt | 18 ++ test/control_manager/goto/test.cpp | 53 +++-- test/run_test.sh | 27 +++ test/uav_manager/landing/test.cpp | 198 ++++++++++++++++++ .../landing/uav_manager_landing.test | 7 + test/uav_manager/takeoff/test.cpp | 44 ++-- 7 files changed, 314 insertions(+), 34 deletions(-) create mode 100755 test/run_test.sh create mode 100644 test/uav_manager/landing/test.cpp create mode 100644 test/uav_manager/landing/uav_manager_landing.test diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp index 103a9a55..7085b3ac 100644 --- a/src/control_manager/control_manager.cpp +++ b/src/control_manager/control_manager.cpp @@ -7869,6 +7869,7 @@ void ControlManager::ungripSrv(void) { /* toggleOutput() //{ */ void ControlManager::toggleOutput(const bool& input) { + if (input == output_enabled_) { ROS_WARN_THROTTLE(0.1, "[ControlManager]: output is already %s", input ? "ON" : "OFF"); return; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a03072d4..d32505ea 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -18,6 +18,24 @@ add_dependencies( ${catkin_EXPORTED_TARGETS} ) +# landing test + +add_rostest_gtest(MrsUavManagers_UavManagerLandingTest + uav_manager/landing/uav_manager_landing.test + uav_manager/landing/test.cpp + ) + +target_link_libraries( + MrsUavManagers_UavManagerLandingTest + ${catkin_LIBRARIES} + ) + +add_dependencies( + MrsUavManagers_UavManagerLandingTest + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + # goto test add_rostest_gtest(MrsUavManagers_ControlManagerGotoTest diff --git a/test/control_manager/goto/test.cpp b/test/control_manager/goto/test.cpp index 338dee2f..c5f3bc1c 100644 --- a/test/control_manager/goto/test.cpp +++ b/test/control_manager/goto/test.cpp @@ -16,12 +16,8 @@ #include #include -/* TEST(TESTSuite, goto) //{ */ - TEST(TESTSuite, goto) { - int result = 0; - // | ------------------ initialize test node ------------------ | ros::NodeHandle nh = ros::NodeHandle("~"); @@ -30,6 +26,9 @@ TEST(TESTSuite, goto) { ros::Time::waitForValid(); + ros::AsyncSpinner spinner(2); + spinner.start(); + std::string uav_name = "uav1"; // | ----------------------- subscribers ---------------------- | @@ -70,8 +69,7 @@ TEST(TESTSuite, goto) { break; } - ros::spinOnce(); - ros::Duration(0.1).sleep(); + ros::Duration(0.01).sleep(); } ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); @@ -80,12 +78,19 @@ TEST(TESTSuite, goto) { // | ---------------------- arm the drone --------------------- | - ROS_INFO("[%s]: arming th edrone", ros::this_node::getName().c_str()); + ROS_INFO("[%s]: arming the edrone", ros::this_node::getName().c_str()); std_srvs::SetBool arming; arming.request.data = true; - sch_arming.call(arming); + { + 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()); + GTEST_FAIL(); + } + } // | -------------------------- wait -------------------------- | @@ -97,7 +102,14 @@ TEST(TESTSuite, goto) { std_srvs::Trigger midair; - sch_midair.call(midair); + { + bool service_call = sch_midair.call(midair); + + if (!service_call || !midair.response.success) { + ROS_ERROR("[%s]: midair activation service call failed", ros::this_node::getName().c_str()); + GTEST_FAIL(); + } + } // | ----------------- sleep before the action ---------------- | @@ -116,7 +128,14 @@ TEST(TESTSuite, goto) { ROS_INFO("[%s]: calling goto", ros::this_node::getName().c_str()); - sch_goto.call(goto_cmd); + { + 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()); + GTEST_FAIL(); + } + } // | ------------------ check for the result ------------------ | @@ -132,21 +151,21 @@ TEST(TESTSuite, goto) { 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) { - result = 1; + + ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); + + GTEST_SUCCEED(); + return; + break; } - ros::spinOnce(); ros::Duration(0.01).sleep(); } - ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); - - EXPECT_TRUE(result); + GTEST_FAIL(); } -//} - int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { ros::init(argc, argv, "goto_test"); diff --git a/test/run_test.sh b/test/run_test.sh new file mode 100755 index 00000000..d92a57d4 --- /dev/null +++ b/test/run_test.sh @@ -0,0 +1,27 @@ +#!/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 --catkin-make-args tests + +# folder for test results +TEST_RESULT_PATH=$(realpath /tmp/$RANDOM) +mkdir -p $TEST_RESULT_PATH + +# run the test +rostest $PACKAGE uav_manager_landing.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/landing/test.cpp b/test/uav_manager/landing/test.cpp new file mode 100644 index 00000000..58717ba4 --- /dev/null +++ b/test/uav_manager/landing/test.cpp @@ -0,0 +1,198 @@ +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +TEST(TESTSuite, landing) { + + // | ------------------ initialize test node ------------------ | + + ros::NodeHandle nh = ros::NodeHandle("~"); + + ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); + + ros::Time::waitForValid(); + + ros::AsyncSpinner spinner(2); + 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(); + + mrs_lib::SubscribeHandler sh_control_manager_diag = + mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); + + mrs_lib::SubscribeHandler 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 -------------------- | + + mrs_lib::ServiceClientHandler sch_arming = mrs_lib::ServiceClientHandler(nh, "/" + uav_name + "/hw_api/arming"); + + mrs_lib::ServiceClientHandler sch_midair = + mrs_lib::ServiceClientHandler(nh, "/" + uav_name + "/uav_manager/midair_activation"); + + mrs_lib::ServiceClientHandler sch_land = mrs_lib::ServiceClientHandler(nh, "/" + uav_name + "/uav_manager/land"); + + mrs_lib::ServiceClientHandler sch_goto = mrs_lib::ServiceClientHandler(nh, "/" + uav_name + "/control_manager/goto"); + + ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); + + // | ---------------- 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()); + GTEST_FAIL(); + } + } + + // | -------------------------- 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 midair; + + { + bool service_call = sch_midair.call(midair); + + if (!service_call || !midair.response.success) { + ROS_ERROR("[%s]: midair activation service call failed", ros::this_node::getName().c_str()); + GTEST_FAIL(); + } + } + + // | ----------------- sleep before the action ---------------- | + + ros::Duration(1.0).sleep(); + + // | -------------------------- goto -------------------------- | + + mrs_msgs::Vec4 goto_cmd; + + goto_cmd.request.goal[0] = 5; + goto_cmd.request.goal[1] = 5; + goto_cmd.request.goal[2] = 8; + goto_cmd.request.goal[3] = 1; + + 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()); + GTEST_FAIL(); + } + } + + ros::Duration(1.0).sleep(); + + // | ------------ check for the result of the goto ------------ | + + while (ros::ok()) { + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for goto", ros::this_node::getName().c_str()); + + if (!sh_control_manager_diag.getMsg()->tracker_status.have_goal) { + + ROS_INFO("[%s]: goto finished", ros::this_node::getName().c_str()); + break; + } + + ros::Duration(0.01).sleep(); + } + + // | ------------------ initiate the landing ------------------ | + + ROS_INFO("[%s]: calling for landing", ros::this_node::getName().c_str()); + + std_srvs::Trigger landing; + + { + bool service_call = sch_land.call(landing); + + if (!service_call || !landing.response.success) { + ROS_ERROR("[%s]: landing service call failed", ros::this_node::getName().c_str()); + GTEST_FAIL(); + } + } + + // | ------------ waiting for the landing to finish ----------- | + + while (ros::ok()) { + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the landing to finish", ros::this_node::getName().c_str()); + + if (!sh_control_manager_diag.getMsg()->output_enabled) { + + ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); + + GTEST_SUCCEED(); + return; + } + + ros::Duration(0.01).sleep(); + } + + GTEST_FAIL(); +} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { + + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "takeoff_test"); + + return RUN_ALL_TESTS(); +} diff --git a/test/uav_manager/landing/uav_manager_landing.test b/test/uav_manager/landing/uav_manager_landing.test new file mode 100644 index 00000000..92576826 --- /dev/null +++ b/test/uav_manager/landing/uav_manager_landing.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/test/uav_manager/takeoff/test.cpp b/test/uav_manager/takeoff/test.cpp index f3e0ff24..2f92ce0d 100644 --- a/test/uav_manager/takeoff/test.cpp +++ b/test/uav_manager/takeoff/test.cpp @@ -12,12 +12,8 @@ #include #include -/* TEST(TESTSuite, takeoff) //{ */ - TEST(TESTSuite, takeoff) { - int result = 0; - // | ------------------ initialize test node ------------------ | ros::NodeHandle nh = ros::NodeHandle("~"); @@ -26,6 +22,9 @@ TEST(TESTSuite, takeoff) { ros::Time::waitForValid(); + ros::AsyncSpinner spinner(2); + spinner.start(); + std::string uav_name = "uav1"; // | ----------------------- subscribers ---------------------- | @@ -63,8 +62,7 @@ TEST(TESTSuite, takeoff) { break; } - ros::spinOnce(); - ros::Duration(0.1).sleep(); + ros::Duration(0.01).sleep(); } // | ---------------------- arm the drone --------------------- | @@ -74,7 +72,14 @@ TEST(TESTSuite, takeoff) { std_srvs::SetBool arming; arming.request.data = true; - sch_arming.call(arming); + { + 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()); + GTEST_FAIL(); + } + } // | -------------------------- wait -------------------------- | @@ -86,7 +91,14 @@ TEST(TESTSuite, takeoff) { std_srvs::Trigger offboard; - sch_offboard.call(offboard); + { + bool service_call = sch_offboard.call(offboard); + + if (!service_call || !offboard.response.success) { + ROS_ERROR("[%s]: offboard service call failed", ros::this_node::getName().c_str()); + GTEST_FAIL(); + } + } // | -------------- wait for the takeoff finished ------------- | @@ -95,21 +107,19 @@ TEST(TESTSuite, takeoff) { 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) { - result = 1; - break; + + ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); + + GTEST_SUCCEED(); + return; } - ros::spinOnce(); - ros::Duration(0.1).sleep(); + ros::Duration(0.01).sleep(); } - ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); - - EXPECT_TRUE(result); + GTEST_FAIL(); } -//} - int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { testing::InitGoogleTest(&argc, argv);