diff --git a/CMakeLists.txt b/CMakeLists.txt index 63b21d4f..ffec1457 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -157,6 +157,16 @@ target_link_libraries(MrsUavManagers_TransformManager ${Eigen_LIBRARIES} ) +## -------------------------------------------------------------- +## | Testing | +## -------------------------------------------------------------- + +if(CATKIN_ENABLE_TESTING) + + add_subdirectory(test) + +endif() + ## -------------------------------------------------------------- ## | Install | ## -------------------------------------------------------------- diff --git a/launch/estimation_manager.launch b/launch/estimation_manager.launch index 5bc507c8..8686a3eb 100644 --- a/launch/estimation_manager.launch +++ b/launch/estimation_manager.launch @@ -1,7 +1,7 @@ - + diff --git a/launch/transform_manager.launch b/launch/transform_manager.launch index f39f747c..bd7bb5f9 100644 --- a/launch/transform_manager.launch +++ b/launch/transform_manager.launch @@ -1,7 +1,7 @@ - + diff --git a/package.xml b/package.xml index 1e284793..d045fa7f 100644 --- a/package.xml +++ b/package.xml @@ -25,6 +25,7 @@ tf2 tf2_ros tf2_geometry_msgs + rostest diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 00000000..a03072d4 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,37 @@ +find_package(rostest REQUIRED) + +# takeoff test + +add_rostest_gtest(MrsUavManagers_UavManagerTakeoffTest + uav_manager/takeoff/uav_manager_takeoff.test + uav_manager/takeoff/test.cpp + ) + +target_link_libraries( + MrsUavManagers_UavManagerTakeoffTest + ${catkin_LIBRARIES} + ) + +add_dependencies( + MrsUavManagers_UavManagerTakeoffTest + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +# goto test + +add_rostest_gtest(MrsUavManagers_ControlManagerGotoTest + control_manager/goto/control_manager_goto.test + control_manager/goto/test.cpp + ) + +target_link_libraries( + MrsUavManagers_ControlManagerGotoTest + ${catkin_LIBRARIES} + ) + +add_dependencies( + MrsUavManagers_ControlManagerGotoTest + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) diff --git a/test/control_manager/goto/control_manager_goto.test b/test/control_manager/goto/control_manager_goto.test new file mode 100644 index 00000000..8bb909e9 --- /dev/null +++ b/test/control_manager/goto/control_manager_goto.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/test/control_manager/goto/test.cpp b/test/control_manager/goto/test.cpp new file mode 100644 index 00000000..08a06e33 --- /dev/null +++ b/test/control_manager/goto/test.cpp @@ -0,0 +1,158 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +/* TEST(TESTSuite, goto) //{ */ + +TEST(TESTSuite, goto) { + + int result = 0; + + // | ------------------ initialize test node ------------------ | + + ros::NodeHandle nh = ros::NodeHandle("~"); + + ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); + + ros::Time::waitForValid(); + + 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"); + + std::cout << "[Test]: subscribers initialized" << std::endl; + + // | --------------------- 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_goto = mrs_lib::ServiceClientHandler(nh, "/" + uav_name + "/control_manager/goto"); + + std::cout << "[Test]: service clients initialized" << std::endl; + + // | ---------------- wait for ready to takeoff --------------- | + + while (ros::ok()) { + + std::cout << "[Test]: waiting for 'ready to midair activation'" << std::endl; + + if (sh_control_manager_diag.hasMsg() && sh_estim_manager_diag.hasMsg()) { + break; + } + + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + std::cout << "[Test]: we are ready 'for midair activation'" << std::endl; + + ros::Duration(1.0).sleep(); + + // | ---------------------- arm the drone --------------------- | + + std::cout << "[Test]: arming the drone" << std::endl; + + std_srvs::SetBool arming; + arming.request.data = true; + + sch_arming.call(arming); + + // | -------------------------- wait -------------------------- | + + ros::Duration(0.2).sleep(); + + // | -------------------- midair activation ------------------- | + + std::cout << "[Test]: activating the drone 'in mid air'" << std::endl; + + std_srvs::Trigger midair; + + sch_midair.call(midair); + + // | ----------------- 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; + + std::cout << "[Test]: calling goto" << std::endl; + + sch_goto.call(goto_cmd); + + // | ------------------ check for the result ------------------ | + + while (ros::ok()) { + + std::cout << "[Test]: waiting for the goto" << std::endl; + + 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) { + result = 1; + break; + } + + ros::spinOnce(); + ros::Duration(1.0).sleep(); + } + + ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); + + EXPECT_TRUE(result); +} + +//} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { + + ros::init(argc, argv, "goto_test"); + + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/test/run_test.sh b/test/run_test.sh new file mode 100755 index 00000000..724ff863 --- /dev/null +++ b/test/run_test.sh @@ -0,0 +1,35 @@ +#!/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 control_manager_goto.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" +success=$? + +echo "" +if [ $success = "0" ]; then + echo -e "\033[0;32mSuccess! \033[1;" +else + echo -e "\033[0;31mFailed some tests! \033[1;" +fi diff --git a/test/sessions/x500_in_mid_air/config/custom_config.yaml b/test/sessions/x500_in_mid_air/config/custom_config.yaml new file mode 100644 index 00000000..9f6bbb87 --- /dev/null +++ b/test/sessions/x500_in_mid_air/config/custom_config.yaml @@ -0,0 +1,16 @@ +# 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 new file mode 100644 index 00000000..08f370d6 --- /dev/null +++ b/test/sessions/x500_in_mid_air/config/network_config.yaml @@ -0,0 +1,15 @@ +# 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 new file mode 100644 index 00000000..196c2e8d --- /dev/null +++ b/test/sessions/x500_in_mid_air/config/simulator.yaml @@ -0,0 +1,20 @@ +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 new file mode 100644 index 00000000..9b550677 --- /dev/null +++ b/test/sessions/x500_in_mid_air/config/world_config.yaml @@ -0,0 +1,34 @@ +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 new file mode 100644 index 00000000..83567506 --- /dev/null +++ b/test/sessions/x500_in_mid_air/launch/mrs_uav_system.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/sessions/x500_on_the_ground/config/custom_config.yaml b/test/sessions/x500_on_the_ground/config/custom_config.yaml new file mode 100644 index 00000000..9f6bbb87 --- /dev/null +++ b/test/sessions/x500_on_the_ground/config/custom_config.yaml @@ -0,0 +1,16 @@ +# 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_on_the_ground/config/network_config.yaml b/test/sessions/x500_on_the_ground/config/network_config.yaml new file mode 100644 index 00000000..08f370d6 --- /dev/null +++ b/test/sessions/x500_on_the_ground/config/network_config.yaml @@ -0,0 +1,15 @@ +# 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_on_the_ground/config/simulator.yaml b/test/sessions/x500_on_the_ground/config/simulator.yaml new file mode 100644 index 00000000..c8f10602 --- /dev/null +++ b/test/sessions/x500_on_the_ground/config/simulator.yaml @@ -0,0 +1,20 @@ +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/sessions/x500_on_the_ground/config/world_config.yaml b/test/sessions/x500_on_the_ground/config/world_config.yaml new file mode 100644 index 00000000..9b550677 --- /dev/null +++ b/test/sessions/x500_on_the_ground/config/world_config.yaml @@ -0,0 +1,34 @@ +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_on_the_ground/launch/mrs_uav_system.launch b/test/sessions/x500_on_the_ground/launch/mrs_uav_system.launch new file mode 100644 index 00000000..1d2143ef --- /dev/null +++ b/test/sessions/x500_on_the_ground/launch/mrs_uav_system.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/uav_manager/takeoff/test.cpp b/test/uav_manager/takeoff/test.cpp new file mode 100644 index 00000000..0cf953d1 --- /dev/null +++ b/test/uav_manager/takeoff/test.cpp @@ -0,0 +1,126 @@ +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +#include + +/* TEST(TESTSuite, takeoff) //{ */ + +TEST(TESTSuite, takeoff) { + + int result = 0; + + // | ------------------ initialize test node ------------------ | + + std::cout << "[Test]: initializing the test node" << std::endl; + + ros::NodeHandle nh = ros::NodeHandle("~"); + + ros::Time::waitForValid(); + + 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_can_takeoff = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/automatic_start/can_takeoff"); + + mrs_lib::SubscribeHandler sh_control_manager_diag = + mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); + + std::cout << "[Test]: subscribers initialized" << std::endl; + + // | --------------------- service clients -------------------- | + + mrs_lib::ServiceClientHandler sch_arming = mrs_lib::ServiceClientHandler(nh, "/" + uav_name + "/hw_api/arming"); + mrs_lib::ServiceClientHandler sch_offboard = mrs_lib::ServiceClientHandler(nh, "/" + uav_name + "/hw_api/offboard"); + + std::cout << "[Test]: service clients initialized" << std::endl; + + // | ---------------- wait for ready to takeoff --------------- | + + while (ros::ok()) { + + std::cout << "[Test]: waiting for 'ready to takeoff'" << std::endl; + + if (sh_can_takeoff.hasMsg() && sh_can_takeoff.getMsg()->data) { + break; + } + + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + std::cout << "[Test]: ready for takeoff" << std::endl; + + // | ---------------------- arm the drone --------------------- | + + std::cout << "[Test]: arming the drone" << std::endl; + + std_srvs::SetBool arming; + arming.request.data = true; + + sch_arming.call(arming); + + // | -------------------------- wait -------------------------- | + + ros::Duration(0.2).sleep(); + + // | -------------------- trigger offboard -------------------- | + + std::cout << "[Test]: triggering offboard" << std::endl; + + std_srvs::Trigger offboard; + + sch_offboard.call(offboard); + + // | -------------- wait for the takeoff finished ------------- | + + while (ros::ok()) { + + std::cout << "[Test]: waiting for the takeoff to finish" << std::endl; + + if (sh_control_manager_diag.getMsg()->flying_normally) { + result = 1; + break; + } + + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + std::cout << "[Test]: finished" << std::endl; + + EXPECT_TRUE(result); +} + +//} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { + + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "Test"); + + return RUN_ALL_TESTS(); +} diff --git a/test/uav_manager/takeoff/uav_manager_takeoff.test b/test/uav_manager/takeoff/uav_manager_takeoff.test new file mode 100644 index 00000000..fea6600b --- /dev/null +++ b/test/uav_manager/takeoff/uav_manager_takeoff.test @@ -0,0 +1,9 @@ + + + + + + + + +