From a61ca910c72150898be44cd94457c0818dda4bbd Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Sun, 3 Dec 2023 20:30:36 +0100 Subject: [PATCH] initial commit --- .github/workflows/ros_build_test.yml | 25 + .github/workflows/ros_package_build.yml | 22 + .gitignore | 11 + CMakeLists.txt | 45 ++ README.md | 1 + include/mrs_uav_testing/test_generic.h | 489 ++++++++++++++++++ package.xml | 23 + test/CMakeLists.txt | 9 + test/all_tests.sh | 12 + test/goto/CMakeLists.txt | 14 + test/goto/config/custom_config.yaml | 16 + test/goto/config/network_config.yaml | 15 + test/goto/config/simulator.yaml | 20 + test/goto/config/world_config.yaml | 34 ++ test/goto/goto.test | 7 + test/goto/launch/mrs_uav_system.launch | 30 ++ test/goto/test.cpp | 62 +++ test/goto_relative/CMakeLists.txt | 14 + test/goto_relative/config/custom_config.yaml | 16 + test/goto_relative/config/network_config.yaml | 15 + test/goto_relative/config/simulator.yaml | 20 + test/goto_relative/config/world_config.yaml | 34 ++ test/goto_relative/goto_relative.test | 7 + .../launch/mrs_uav_system.launch | 30 ++ test/goto_relative/test.cpp | 62 +++ test/midair_activation/CMakeLists.txt | 14 + .../config/custom_config.yaml | 16 + .../config/network_config.yaml | 15 + test/midair_activation/config/simulator.yaml | 20 + .../config/world_config.yaml | 34 ++ .../launch/mrs_uav_system.launch | 30 ++ test/midair_activation/midair_activation.test | 7 + test/midair_activation/test.cpp | 51 ++ test/takeoff/CMakeLists.txt | 14 + test/takeoff/config/custom_config.yaml | 16 + test/takeoff/config/network_config.yaml | 15 + test/takeoff/config/simulator.yaml | 20 + test/takeoff/config/world_config.yaml | 34 ++ test/takeoff/launch/mrs_uav_system.launch | 35 ++ test/takeoff/takeoff.test | 7 + test/takeoff/test.cpp | 51 ++ 41 files changed, 1412 insertions(+) create mode 100644 .github/workflows/ros_build_test.yml create mode 100644 .github/workflows/ros_package_build.yml create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 include/mrs_uav_testing/test_generic.h create mode 100644 package.xml create mode 100644 test/CMakeLists.txt create mode 100755 test/all_tests.sh create mode 100644 test/goto/CMakeLists.txt create mode 100644 test/goto/config/custom_config.yaml create mode 100644 test/goto/config/network_config.yaml create mode 100644 test/goto/config/simulator.yaml create mode 100644 test/goto/config/world_config.yaml create mode 100644 test/goto/goto.test create mode 100644 test/goto/launch/mrs_uav_system.launch create mode 100644 test/goto/test.cpp create mode 100644 test/goto_relative/CMakeLists.txt create mode 100644 test/goto_relative/config/custom_config.yaml create mode 100644 test/goto_relative/config/network_config.yaml create mode 100644 test/goto_relative/config/simulator.yaml create mode 100644 test/goto_relative/config/world_config.yaml create mode 100644 test/goto_relative/goto_relative.test create mode 100644 test/goto_relative/launch/mrs_uav_system.launch create mode 100644 test/goto_relative/test.cpp create mode 100644 test/midair_activation/CMakeLists.txt create mode 100644 test/midair_activation/config/custom_config.yaml create mode 100644 test/midair_activation/config/network_config.yaml create mode 100644 test/midair_activation/config/simulator.yaml create mode 100644 test/midair_activation/config/world_config.yaml create mode 100644 test/midair_activation/launch/mrs_uav_system.launch create mode 100644 test/midair_activation/midair_activation.test create mode 100644 test/midair_activation/test.cpp create mode 100644 test/takeoff/CMakeLists.txt create mode 100644 test/takeoff/config/custom_config.yaml create mode 100644 test/takeoff/config/network_config.yaml create mode 100644 test/takeoff/config/simulator.yaml create mode 100644 test/takeoff/config/world_config.yaml create mode 100644 test/takeoff/launch/mrs_uav_system.launch create mode 100644 test/takeoff/takeoff.test create mode 100644 test/takeoff/test.cpp diff --git a/.github/workflows/ros_build_test.yml b/.github/workflows/ros_build_test.yml new file mode 100644 index 0000000..28768ce --- /dev/null +++ b/.github/workflows/ros_build_test.yml @@ -0,0 +1,25 @@ +name: ros_build_test + +on: + + push: + branches: [ devel ] + + paths-ignore: + - '**/README.md' + + pull_request: + branches: [ master ] + + workflow_dispatch: + +concurrency: + group: ${{ github.ref }} + cancel-in-progress: true + +jobs: + + build: + uses: ctu-mrs/ci_scripts/.github/workflows/ros_build_test.yml@master + secrets: + PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }} diff --git a/.github/workflows/ros_package_build.yml b/.github/workflows/ros_package_build.yml new file mode 100644 index 0000000..d17da2a --- /dev/null +++ b/.github/workflows/ros_package_build.yml @@ -0,0 +1,22 @@ +name: ros_package_build + +on: + + push: + branches: [ master ] + + paths-ignore: + - '**/README.md' + + workflow_dispatch: + +concurrency: + group: ${{ github.ref }} + cancel-in-progress: true + +jobs: + + build: + uses: ctu-mrs/ci_scripts/.github/workflows/ros_package_build.yml@master + secrets: + PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }} diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c136f4d --- /dev/null +++ b/.gitignore @@ -0,0 +1,11 @@ + +# ignore temp files +*~ + +# ignore vim temp files +*.swp +*.swo +*.swn + +# Runtime-python +*.pyc diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..40c39a2 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(mrs_uav_testing) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") + +set(CATKIN_DEPENDENCIES + cmake_modules + roscpp + mrs_lib + mrs_msgs + ) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_DEPENDENCIES} + ) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} + ) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ) + +if(CATKIN_ENABLE_TESTING) + + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --coverage") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage") + + add_subdirectory(test) + +endif() + +## -------------------------------------------------------------- +## | Install | +## -------------------------------------------------------------- + +install(DIRECTORY include/mrs_uav_testing/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + ) diff --git a/README.md b/README.md index cdbdd18..e5df9b2 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,3 @@ # mrs_uav_testing + Common templates and functions for rostesting the MRS UAV System. diff --git a/include/mrs_uav_testing/test_generic.h b/include/mrs_uav_testing/test_generic.h new file mode 100644 index 0000000..cb7d4b9 --- /dev/null +++ b/include/mrs_uav_testing/test_generic.h @@ -0,0 +1,489 @@ +#ifndef TEST_GENERIC_H +#define TEST_GENERIC_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace mrs_uav_testing +{ + +using radians = mrs_lib::geometry::radians; +using sradians = mrs_lib::geometry::sradians; + +using namespace std; + +/* TestGeneric //{ */ + +class TestGeneric { + +public: + TestGeneric(); + TestGeneric(const string &test_name); + TestGeneric(const string &test_name, const string &uav_name); + + void initialize(void); + + virtual bool test() = 0; + + tuple takeoff(void); + tuple activateMidAir(void); + + tuple gotoAbs(const double &x, const double &y, const double &z, const double &hdg); + tuple gotoRel(const double &x, const double &y, const double &z, const double &hdg); + + void sleep(const double &duration); + + bool mrsSystemReady(void); + + bool flyingNormally(void); + + bool canTakeOff(void); + +protected: + ros::NodeHandle nh_; + + string name_; + + mrs_lib::SubscribeHandler sh_control_manager_diag_; + mrs_lib::SubscribeHandler sh_uav_manager_diag_; + mrs_lib::SubscribeHandler sh_estim_manager_diag_; + mrs_lib::SubscribeHandler sh_gain_manager_diag_; + mrs_lib::SubscribeHandler sh_constraint_manager_diag_; + + mrs_lib::SubscribeHandler sh_hw_api_status_; + + mrs_lib::ServiceClientHandler sch_arming_; + mrs_lib::ServiceClientHandler sch_offboard_; + mrs_lib::ServiceClientHandler sch_midair_activation_; + + mrs_lib::ServiceClientHandler sch_goto_; + mrs_lib::ServiceClientHandler sch_goto_relative_; + +private: + shared_ptr spinner_; + + string _uav_name_; + string _test_name_; +}; + +//} + +/* constructors //{ */ + +TestGeneric::TestGeneric() { + + _test_name_ = "TestGeneric"; + _uav_name_ = "uav1"; + + initialize(); +} + +TestGeneric::TestGeneric(const string &test_name) { + + this->_test_name_ = test_name; + + initialize(); +} + +TestGeneric::TestGeneric(const string &test_name, const string &uav_name) { + + this->_test_name_ = test_name; + this->_uav_name_ = uav_name; + + initialize(); +} + +//} + +/* initialize() //{ */ + +void TestGeneric::initialize(void) { + + name_ = _uav_name_ + "/" + _test_name_; + + nh_ = ros::NodeHandle("~"); + + ROS_INFO("[%s]: ROS node initialized", name_.c_str()); + + ros::Time::waitForValid(); + + spinner_ = make_shared(4); + spinner_->start(); + + // | ----------------------- subscribers ---------------------- | + + mrs_lib::SubscribeHandlerOptions shopts; + shopts.nh = nh_; + shopts.node_name = name_; + 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_uav_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + _uav_name_ + "/uav_manager/diagnostics"); + sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + _uav_name_ + "/estimation_manager/diagnostics"); + sh_constraint_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + _uav_name_ + "/constraint_manager/diagnostics"); + sh_gain_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + _uav_name_ + "/gain_manager/diagnostics"); + + sh_hw_api_status_ = mrs_lib::SubscribeHandler(shopts, "/" + _uav_name_ + "/hw_api/status"); + + // | --------------------- service clients -------------------- | + + sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/hw_api/arming"); + sch_offboard_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/hw_api/offboard"); + sch_midair_activation_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/uav_manager/midair_activation"); + + sch_goto_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/control_manager/goto"); + sch_goto_relative_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/control_manager/goto_relative"); + + // | --------------------- finish the init -------------------- | + + ROS_INFO("[%s]: initialized", name_.c_str()); +} + +//} + +/* sleep() //{ */ + +void TestGeneric::sleep(const double &duration) { + + ros::Duration(duration).sleep(); +} + +//} + +/* takeoff() //{ */ + +tuple TestGeneric::takeoff(void) { + + // | ---------------- wait for ready to takeoff --------------- | + + while (true) { + + if (!ros::ok()) { + return {false, "shut down from outside"}; + } + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", name_.c_str()); + + if (mrsSystemReady()) { + ROS_INFO("[%s]: MRS UAV System is ready", name_.c_str()); + break; + } + + sleep(0.1); + } + + // | ---------------------- arm the drone --------------------- | + + ROS_INFO("[%s]: arming the drone", name_.c_str()); + + { + std_srvs::SetBool srv; + srv.request.data = true; + + { + bool service_call = sch_arming_.call(srv); + + if (!service_call || !srv.response.success) { + return {false, "arming service call failed"}; + } + } + } + + // | ---------------------- wait a second --------------------- | + + sleep(1.0); + + // | --------------------- check if armed --------------------- | + + if (!sh_hw_api_status_.getMsg()->armed) { + return {false, "not armed"}; + } + + // | ------------------- switch to offboard ------------------- | + + { + std_srvs::Trigger srv; + + { + bool service_call = sch_offboard_.call(srv); + + if (!service_call || !srv.response.success) { + return {false, "offboard service call failed"}; + } + } + } + + // | -------------------------- wait -------------------------- | + + sleep(0.1); + + // | ------------------ check if in offboard ------------------ | + + if (!sh_hw_api_status_.getMsg()->offboard) { + return {false, "not in offboard"}; + } + + // | --------------- wait for takeoff to finish --------------- | + + while (true) { + + if (!ros::ok()) { + return {false, "shut down from outside"}; + } + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the takeoff to finish", name_.c_str()); + + if (sh_control_manager_diag_.getMsg()->flying_normally) { + + return {true, "takeoff finished"}; + } + + sleep(0.1); + } + + return {false, "reached end of the method without assertion"}; +} + +//} + +/* activateMidAir() //{ */ + +tuple TestGeneric::activateMidAir(void) { + + // | ---------------- wait for ready to takeoff --------------- | + + while (true) { + + if (!ros::ok()) { + return {false, "shut down from outside"}; + } + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", name_.c_str()); + + if (mrsSystemReady()) { + ROS_INFO("[%s]: MRS UAV System is ready", name_.c_str()); + break; + } + + sleep(0.1); + } + + // | ---------------------- arm the drone --------------------- | + + ROS_INFO("[%s]: arming the drone", name_.c_str()); + + { + std_srvs::SetBool srv; + srv.request.data = true; + + { + bool service_call = sch_arming_.call(srv); + + if (!service_call || !srv.response.success) { + return {false, "arming service call failed"}; + } + } + } + + // | -------------------------- wait -------------------------- | + + sleep(0.1); + + // | --------------------- check if armed --------------------- | + + if (!sh_hw_api_status_.getMsg()->armed) { + return {false, "not armed"}; + } + + // | ----------------- call midair activation ----------------- | + + { + std_srvs::Trigger srv; + + { + bool service_call = sch_midair_activation_.call(srv); + + if (!service_call || !srv.response.success) { + return {false, "midair activation service call failed"}; + } + } + } + + // | --------------- wait for takeoff to finish --------------- | + + while (true) { + + if (!ros::ok()) { + return {false, "shut down from outside"}; + } + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the midair activation to finish", name_.c_str()); + + if (sh_control_manager_diag_.getMsg()->flying_normally) { + + return {true, "midair activation finished"}; + } + + sleep(0.1); + } + + return {false, "reached end of the method without assertion"}; +} + +//} + +/* mrsSystemReady() //{ */ + +bool TestGeneric::mrsSystemReady(void) { + + bool got_control_manager_diag = sh_control_manager_diag_.hasMsg(); + bool got_estimation_manager_diag = sh_estim_manager_diag_.hasMsg(); + bool got_uav_manager_diag = sh_uav_manager_diag_.hasMsg(); + bool got_gain_manager_diag = sh_gain_manager_diag_.hasMsg(); + bool got_constraint_manager_diag = sh_constraint_manager_diag_.hasMsg(); + + return got_control_manager_diag && got_estimation_manager_diag && got_uav_manager_diag && got_gain_manager_diag && got_constraint_manager_diag; +} + +//} + +/* flyingNormally() //{ */ + +bool TestGeneric::flyingNormally(void) { + + if (sh_control_manager_diag_.hasMsg()) { + return sh_control_manager_diag_.getMsg()->flying_normally; + } else { + return false; + } +} + +//} + +/* gotoAbs() //{ */ + +tuple TestGeneric::gotoAbs(const double &x, const double &y, const double &z, const double &hdg) { + + mrs_msgs::Vec4 srv; + + srv.request.goal[0] = x; + srv.request.goal[1] = y; + srv.request.goal[2] = z; + srv.request.goal[3] = hdg; + + { + bool service_call = sch_goto_.call(srv); + + if (!service_call || !srv.response.success) { + return {false, "goto service call failed"}; + } + } + + // | -------------------- check for result -------------------- | + + while (true) { + + if (!ros::ok()) { + return {false, "shut down from outside"}; + } + + if (!flyingNormally()) { + return {false, "not flying normally"}; + } + + auto diag = sh_estim_manager_diag_.getMsg(); + + auto current_hdg = mrs_lib::AttitudeConverter(diag->pose.orientation).getHeading(); + + if (abs(x - diag->pose.position.x) < 0.1 && abs(y - diag->pose.position.y) < 0.1 && abs(z - diag->pose.position.z) < 0.1 && abs(hdg - current_hdg) < 0.1) { + + return {true, "goal reached"}; + } + + ros::Duration(0.1).sleep(); + } + + return {false, "reached end of the method without assertion"}; +} + +//} + +/* gotoRel() //{ */ + +tuple TestGeneric::gotoRel(const double &x, const double &y, const double &z, const double &hdg) { + + auto start_pose = sh_estim_manager_diag_.getMsg()->pose.position; + auto start_hdg = mrs_lib::AttitudeConverter(sh_estim_manager_diag_.getMsg()->pose.orientation).getHeading(); + + { + mrs_msgs::Vec4 srv; + + srv.request.goal[0] = x; + srv.request.goal[1] = y; + srv.request.goal[2] = z; + srv.request.goal[3] = hdg; + + { + bool service_call = sch_goto_relative_.call(srv); + + if (!service_call || !srv.response.success) { + return {false, "goto relative service call failed"}; + } + } + } + + // | -------------------- check for result -------------------- | + + while (true) { + + if (!ros::ok()) { + return {false, "shut down from outside"}; + } + + if (!flyingNormally()) { + return {false, "not flying normally"}; + } + + auto diag = sh_estim_manager_diag_.getMsg(); + + auto current_hdg = mrs_lib::AttitudeConverter(diag->pose.orientation).getHeading(); + + if (abs(start_pose.x + x - diag->pose.position.x) < 0.1 && abs(start_pose.y + y - diag->pose.position.y) < 0.1 && + abs(start_pose.z + z - diag->pose.position.z) < 0.1 && abs(radians::diff(start_hdg + hdg, current_hdg)) < 0.1) { + + return {true, "goal reached"}; + } + + ros::Duration(0.1).sleep(); + } + + return {false, "reached end of the method without assertion"}; +} + +//} + +} // namespace mrs_uav_testing + +#endif // TEST_GENERIC_H diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..de3f39a --- /dev/null +++ b/package.xml @@ -0,0 +1,23 @@ + + + + mrs_uav_testing + 1.0.0 + The mrs_uav_testing package + + Tomas Baca + Tomas Baca + + BSD 3-Clause + + catkin + + cmake_modules + mrs_lib + mrs_msgs + roscpp + + + + + diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..29deb0c --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,9 @@ +find_package(rostest REQUIRED) + +add_subdirectory(takeoff) + +add_subdirectory(midair_activation) + +add_subdirectory(goto) + +add_subdirectory(goto_relative) diff --git a/test/all_tests.sh b/test/all_tests.sh new file mode 100755 index 0000000..cf370b1 --- /dev/null +++ b/test/all_tests.sh @@ -0,0 +1,12 @@ +#!/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 + +catkin test --this -i -p 1 diff --git a/test/goto/CMakeLists.txt b/test/goto/CMakeLists.txt new file mode 100644 index 0000000..d1bfff7 --- /dev/null +++ b/test/goto/CMakeLists.txt @@ -0,0 +1,14 @@ +catkin_add_executable_with_gtest(test_goto + test.cpp + ) + +target_link_libraries(test_goto + ${catkin_LIBRARIES} + ) + +add_dependencies(test_goto + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +add_rostest(goto.test) diff --git a/test/goto/config/custom_config.yaml b/test/goto/config/custom_config.yaml new file mode 100644 index 0000000..9f6bbb8 --- /dev/null +++ b/test/goto/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/goto/config/network_config.yaml b/test/goto/config/network_config.yaml new file mode 100644 index 0000000..08f370d --- /dev/null +++ b/test/goto/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/goto/config/simulator.yaml b/test/goto/config/simulator.yaml new file mode 100644 index 0000000..a964981 --- /dev/null +++ b/test/goto/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: 1.57 diff --git a/test/goto/config/world_config.yaml b/test/goto/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/test/goto/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/goto/goto.test b/test/goto/goto.test new file mode 100644 index 0000000..74b6537 --- /dev/null +++ b/test/goto/goto.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/test/goto/launch/mrs_uav_system.launch b/test/goto/launch/mrs_uav_system.launch new file mode 100644 index 0000000..e498d2e --- /dev/null +++ b/test/goto/launch/mrs_uav_system.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/goto/test.cpp b/test/goto/test.cpp new file mode 100644 index 0000000..4dc4e4e --- /dev/null +++ b/test/goto/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->flyingNormally()) { + return true; + } else { + ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); + return false; + } +} + + +TEST(TESTSuite, goto) { + + 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/goto_relative/CMakeLists.txt b/test/goto_relative/CMakeLists.txt new file mode 100644 index 0000000..8c43691 --- /dev/null +++ b/test/goto_relative/CMakeLists.txt @@ -0,0 +1,14 @@ +catkin_add_executable_with_gtest(test_goto_relative + test.cpp + ) + +target_link_libraries(test_goto_relative + ${catkin_LIBRARIES} + ) + +add_dependencies(test_goto_relative + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +add_rostest(goto_relative.test) diff --git a/test/goto_relative/config/custom_config.yaml b/test/goto_relative/config/custom_config.yaml new file mode 100644 index 0000000..9f6bbb8 --- /dev/null +++ b/test/goto_relative/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/goto_relative/config/network_config.yaml b/test/goto_relative/config/network_config.yaml new file mode 100644 index 0000000..08f370d --- /dev/null +++ b/test/goto_relative/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/goto_relative/config/simulator.yaml b/test/goto_relative/config/simulator.yaml new file mode 100644 index 0000000..a964981 --- /dev/null +++ b/test/goto_relative/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: 1.57 diff --git a/test/goto_relative/config/world_config.yaml b/test/goto_relative/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/test/goto_relative/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/goto_relative/goto_relative.test b/test/goto_relative/goto_relative.test new file mode 100644 index 0000000..74b6537 --- /dev/null +++ b/test/goto_relative/goto_relative.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/test/goto_relative/launch/mrs_uav_system.launch b/test/goto_relative/launch/mrs_uav_system.launch new file mode 100644 index 0000000..e498d2e --- /dev/null +++ b/test/goto_relative/launch/mrs_uav_system.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/goto_relative/test.cpp b/test/goto_relative/test.cpp new file mode 100644 index 0000000..10f6fc7 --- /dev/null +++ b/test/goto_relative/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->gotoRel(1, 2, 3, 1); + + if (!success) { + ROS_ERROR("[%s]: goto relative failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + this->sleep(5.0); + + if (this->flyingNormally()) { + return true; + } else { + ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); + return false; + } +} + + +TEST(TESTSuite, goto_relative) { + + 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/midair_activation/CMakeLists.txt b/test/midair_activation/CMakeLists.txt new file mode 100644 index 0000000..d2a2c54 --- /dev/null +++ b/test/midair_activation/CMakeLists.txt @@ -0,0 +1,14 @@ +catkin_add_executable_with_gtest(test_midair + test.cpp + ) + +target_link_libraries(test_midair + ${catkin_LIBRARIES} + ) + +add_dependencies(test_midair + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +add_rostest(midair_activation.test) diff --git a/test/midair_activation/config/custom_config.yaml b/test/midair_activation/config/custom_config.yaml new file mode 100644 index 0000000..9f6bbb8 --- /dev/null +++ b/test/midair_activation/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/midair_activation/config/network_config.yaml b/test/midair_activation/config/network_config.yaml new file mode 100644 index 0000000..08f370d --- /dev/null +++ b/test/midair_activation/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/midair_activation/config/simulator.yaml b/test/midair_activation/config/simulator.yaml new file mode 100644 index 0000000..196c2e8 --- /dev/null +++ b/test/midair_activation/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/midair_activation/config/world_config.yaml b/test/midair_activation/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/test/midair_activation/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/midair_activation/launch/mrs_uav_system.launch b/test/midair_activation/launch/mrs_uav_system.launch new file mode 100644 index 0000000..98b3f88 --- /dev/null +++ b/test/midair_activation/launch/mrs_uav_system.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/midair_activation/midair_activation.test b/test/midair_activation/midair_activation.test new file mode 100644 index 0000000..b9633b8 --- /dev/null +++ b/test/midair_activation/midair_activation.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/test/midair_activation/test.cpp b/test/midair_activation/test.cpp new file mode 100644 index 0000000..96bbcc7 --- /dev/null +++ b/test/midair_activation/test.cpp @@ -0,0 +1,51 @@ +#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; + } + + this->sleep(5.0); + + if (this->flyingNormally()) { + return true; + } else { + ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); + return false; + } +} + + +TEST(TESTSuite, midair_activation) { + + 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/takeoff/CMakeLists.txt b/test/takeoff/CMakeLists.txt new file mode 100644 index 0000000..d5b0c90 --- /dev/null +++ b/test/takeoff/CMakeLists.txt @@ -0,0 +1,14 @@ +catkin_add_executable_with_gtest(test_takeoff + test.cpp + ) + +target_link_libraries(test_takeoff + ${catkin_LIBRARIES} + ) + +add_dependencies(test_takeoff + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +add_rostest(takeoff.test) diff --git a/test/takeoff/config/custom_config.yaml b/test/takeoff/config/custom_config.yaml new file mode 100644 index 0000000..9f6bbb8 --- /dev/null +++ b/test/takeoff/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/takeoff/config/network_config.yaml b/test/takeoff/config/network_config.yaml new file mode 100644 index 0000000..08f370d --- /dev/null +++ b/test/takeoff/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/takeoff/config/simulator.yaml b/test/takeoff/config/simulator.yaml new file mode 100644 index 0000000..c8f1060 --- /dev/null +++ b/test/takeoff/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/takeoff/config/world_config.yaml b/test/takeoff/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/test/takeoff/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/takeoff/launch/mrs_uav_system.launch b/test/takeoff/launch/mrs_uav_system.launch new file mode 100644 index 0000000..e937ed1 --- /dev/null +++ b/test/takeoff/launch/mrs_uav_system.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/takeoff/takeoff.test b/test/takeoff/takeoff.test new file mode 100644 index 0000000..bee656b --- /dev/null +++ b/test/takeoff/takeoff.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/test/takeoff/test.cpp b/test/takeoff/test.cpp new file mode 100644 index 0000000..e778481 --- /dev/null +++ b/test/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->flyingNormally()) { + return true; + } else { + ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); + return false; + } +} + + +TEST(TESTSuite, takeoff) { + + 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(); +}