From 4d50cdba773fed053e11652a5d753d9383788879 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Fri, 15 Dec 2023 10:49:04 +0100 Subject: [PATCH] updated tests to the new format --- CMakeLists.txt | 4 +- package.xml | 1 + src/mrs_trajectory_generation.cpp | 30 ++- test/CMakeLists.txt | 24 +-- test/all_tests.sh | 29 +-- test/basic/basic.test | 7 - test/basic/config/custom_config.yaml | 24 --- test/basic/config/network_config.yaml | 15 -- test/basic/config/simulator.yaml | 20 -- test/basic/config/world_config.yaml | 34 ---- test/basic/launch/mrs_uav_system.launch | 30 --- test/basic/test.cpp | 228 ---------------------- test/coverage.sh | 25 --- test/include/trajectory_generation_test.h | 34 ++++ test/service_fly_now/CMakeLists.txt | 16 ++ test/service_fly_now/service_fly_now.test | 35 ++++ test/service_fly_now/test.cpp | 105 ++++++++++ test/single_test.sh | 27 --- test/topic_fly_now/CMakeLists.txt | 16 ++ test/topic_fly_now/test.cpp | 105 ++++++++++ test/topic_fly_now/topic_fly_now.test | 35 ++++ 21 files changed, 366 insertions(+), 478 deletions(-) delete mode 100644 test/basic/basic.test delete mode 100644 test/basic/config/custom_config.yaml delete mode 100644 test/basic/config/network_config.yaml delete mode 100644 test/basic/config/simulator.yaml delete mode 100644 test/basic/config/world_config.yaml delete mode 100644 test/basic/launch/mrs_uav_system.launch delete mode 100644 test/basic/test.cpp delete mode 100755 test/coverage.sh create mode 100644 test/include/trajectory_generation_test.h create mode 100644 test/service_fly_now/CMakeLists.txt create mode 100644 test/service_fly_now/service_fly_now.test create mode 100644 test/service_fly_now/test.cpp delete mode 100755 test/single_test.sh create mode 100644 test/topic_fly_now/CMakeLists.txt create mode 100644 test/topic_fly_now/test.cpp create mode 100644 test/topic_fly_now/topic_fly_now.test diff --git a/CMakeLists.txt b/CMakeLists.txt index a3bafb6..e2cd409 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ set(CATKIN_DEPENDENCIES mrs_msgs mrs_lib nlopt + mrs_uav_testing ) find_package(catkin REQUIRED COMPONENTS @@ -47,7 +48,7 @@ set(LIBRARIES ) catkin_package( - INCLUDE_DIRS include + INCLUDE_DIRS include test/include LIBRARIES ${LIBRARIES} CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} DEPENDS Eigen @@ -70,6 +71,7 @@ add_library(MrsUavTrajectoryGeneration_MrsTrajectoryGeneration include_directories( include + test/include ${catkin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake diff --git a/package.xml b/package.xml index 569a434..c405777 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ std_msgs mrs_msgs mrs_lib + mrs_uav_testing nlopt diff --git a/src/mrs_trajectory_generation.cpp b/src/mrs_trajectory_generation.cpp index 8690e4b..203c5e7 100644 --- a/src/mrs_trajectory_generation.cpp +++ b/src/mrs_trajectory_generation.cpp @@ -616,7 +616,7 @@ std::tuple MrsTrajectoryGenera bool safe = false; int traj_idx; std::vector segment_safeness; - double max_deviation; + double max_deviation = 0; eth_mav_msgs::EigenTrajectoryPoint::Vector trajectory; @@ -903,6 +903,15 @@ std::optional MrsTrajectoryGeneratio double vertical_speed_lim = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed); double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration); + v_max_horizontal = constraints.horizontal_speed; + a_max_horizontal = constraints.horizontal_acceleration; + + v_max_vertical = vertical_speed_lim; + a_max_vertical = vertical_acceleration_lim; + + j_max_horizontal = constraints.horizontal_jerk; + j_max_vertical = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk); + if (override_constraints_) { bool can_change = (hypot(initial_state.velocity.x, initial_state.velocity.y) < override_max_velocity_horizontal_) && @@ -927,20 +936,8 @@ std::optional MrsTrajectoryGeneratio ROS_WARN("[MrsTrajectoryGeneration]: overrifing constraints refused due to possible infeasibility"); } - - } else { - - v_max_horizontal = constraints.horizontal_speed; - a_max_horizontal = constraints.horizontal_acceleration; - - v_max_vertical = vertical_speed_lim; - a_max_vertical = vertical_acceleration_lim; - - j_max_horizontal = constraints.horizontal_jerk; - j_max_vertical = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk); } - double v_max_heading, a_max_heading, j_max_heading; if (relax_heading) { @@ -1424,19 +1421,14 @@ std::vector MrsTrajectoryGeneration::getTrajectorySegmentCenterIdxs(const e // next sample const vec3_t next_sample = vec3_t(trajectory[i + 1].position_W[0], trajectory[i + 1].position_W[1], trajectory[i + 1].position_W[2]); - // segment start - const vec3_t segment_start = - vec3_t(waypoints.at(last_segment_start).coords[0], waypoints.at(last_segment_start).coords[1], waypoints.at(last_segment_start).coords[2]); - // segment end const vec3_t segment_end = vec3_t(waypoints.at(last_segment_start + 1).coords[0], waypoints.at(last_segment_start + 1).coords[1], waypoints.at(last_segment_start + 1).coords[2]); - const double distance_from_segment = distFromSegment(sample, segment_start, segment_end); - const double segment_end_dist = distFromSegment(segment_end, sample, next_sample); if (segment_end_dist < 0.05 && last_segment_start < (int(waypoints.size()) - 2)) { + last_segment_start++; segment_centers.push_back(int(floor(double(i - last_segment_start_sample) / 2.0))); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 49d9d9c..65a4219 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,25 +1,5 @@ find_package(rostest REQUIRED) -### WARNING -### NEVER name your test build unit the same as the folder that it is placed in! -### e.g., this will cause problems: -### add_rostest_gtest(my_test -### my_test/basic.test -### my_test/test.cpp -### ) +add_subdirectory(service_fly_now) -# basic test - -add_rostest_gtest(test_basic - basic/basic.test - basic/test.cpp - ) - -target_link_libraries(test_basic - ${catkin_LIBRARIES} - ) - -add_dependencies(test_basic - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) +add_subdirectory(topic_fly_now) diff --git a/test/all_tests.sh b/test/all_tests.sh index 57bb481..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_trajectory_generation" -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/basic/basic.test b/test/basic/basic.test deleted file mode 100644 index 982852d..0000000 --- a/test/basic/basic.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/test/basic/config/custom_config.yaml b/test/basic/config/custom_config.yaml deleted file mode 100644 index 71de8b8..0000000 --- a/test/basic/config/custom_config.yaml +++ /dev/null @@ -1,24 +0,0 @@ -# GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: -## -------------------------------------------------------------- -## | rosrun mrs_uav_core get_public_params.py # -## -------------------------------------------------------------- - -mrs_uav_managers: - - estimation_manager: - - # loaded state estimator plugins - state_estimators: [ - "gps_baro", - ] - - initial_state_estimator: "gps_baro" # will be used as the first state estimator - agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) - - uav_manager: - - midair_activation: - - after_activation: - controller: "MpcController" - tracker: "MpcTracker" diff --git a/test/basic/config/network_config.yaml b/test/basic/config/network_config.yaml deleted file mode 100644 index 08f370d..0000000 --- a/test/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/basic/config/simulator.yaml b/test/basic/config/simulator.yaml deleted file mode 100644 index 196c2e8..0000000 --- a/test/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/basic/config/world_config.yaml b/test/basic/config/world_config.yaml deleted file mode 100644 index 9b55067..0000000 --- a/test/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/basic/launch/mrs_uav_system.launch b/test/basic/launch/mrs_uav_system.launch deleted file mode 100644 index b97bda5..0000000 --- a/test/basic/launch/mrs_uav_system.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/basic/test.cpp b/test/basic/test.cpp deleted file mode 100644 index 4cbaea2..0000000 --- a/test/basic/test.cpp +++ /dev/null @@ -1,228 +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_path_; -}; - -//} - -/* 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_path_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/trajectory_generation/path"); - - 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::PathSrv path_cmd; - - path_cmd.request.path.fly_now = true; - path_cmd.request.path.use_heading = true; - - mrs_msgs::Reference point; - - point.position.x = 0; - point.position.y = 0; - point.position.z = 5.5; - point.heading = 2.2; - - path_cmd.request.path.points.push_back(point); - - { - - ROS_INFO("[%s]: calling path", ros::this_node::getName().c_str()); - - { - bool service_call = sch_path_.call(path_cmd); - - if (!service_call || !path_cmd.response.success) { - ROS_ERROR("[%s]: path 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, 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, "basic"); - - tester_ = std::make_shared(); - - testing::InitGoogleTest(&argc, argv); - - Tester tester; - - return RUN_ALL_TESTS(); -} diff --git a/test/coverage.sh b/test/coverage.sh deleted file mode 100755 index 54769b4..0000000 --- a/test/coverage.sh +++ /dev/null @@ -1,25 +0,0 @@ -#!/bin/bash - -PACKAGE_NAME=mrs_uav_trajectory_generation - -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 "*/src/*${PACKAGE_NAME}/*" --output-file coverage.info.cleaned -genhtml --title "${PACKAGE_NAME} - Test coverage report" --demangle-cpp --legend --frames --show-details -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/include/trajectory_generation_test.h b/test/include/trajectory_generation_test.h new file mode 100644 index 0000000..ddf408f --- /dev/null +++ b/test/include/trajectory_generation_test.h @@ -0,0 +1,34 @@ +#ifndef TRAJECTORY_GENERATION_TEST_H +#define TRAJECTORY_GENERATION_TEST_H + +#include + +class TrajectoryGenerationTest : public mrs_uav_testing::TestGeneric { + +public: + std::tuple checkPathFlythrough(const std::vector &waypoints); +}; + +std::tuple TrajectoryGenerationTest::checkPathFlythrough(const std::vector &waypoints) { + + unsigned long current_idx = 0; + + while (true) { + + if (!ros::ok()) { + return {false, "terminated form outside"}; + } + + if (this->isAtPosition(waypoints[current_idx][0], waypoints[current_idx][1], waypoints[current_idx][2], waypoints[current_idx][3], 0.5)) { + current_idx++; + } + + if (current_idx == waypoints.size()) { + return {true, "waypoints passed"}; + } + + sleep(0.01); + } +} + +#endif // TRAJECTORY_GENERATION_TEST_H diff --git a/test/service_fly_now/CMakeLists.txt b/test/service_fly_now/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/service_fly_now/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/service_fly_now/service_fly_now.test b/test/service_fly_now/service_fly_now.test new file mode 100644 index 0000000..4d187b1 --- /dev/null +++ b/test/service_fly_now/service_fly_now.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/service_fly_now/test.cpp b/test/service_fly_now/test.cpp new file mode 100644 index 0000000..b7fa9aa --- /dev/null +++ b/test/service_fly_now/test.cpp @@ -0,0 +1,105 @@ +#include + +// include the generic test customized for this package +#include + +class Tester : public TrajectoryGenerationTest { + +public: + bool test(); +}; + +bool Tester::test() { + + std::vector points; + + points.push_back(Eigen::Vector4d(-5, -5, 5, 1)); + points.push_back(Eigen::Vector4d(-5, 5, 5, 2)); + points.push_back(Eigen::Vector4d(5, -5, 5, 3)); + points.push_back(Eigen::Vector4d(5, 5, 5, 4)); + + // | -------------------- activate the UAV -------------------- | + + { + 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; + } + } + + // | ---------------- prepare the path message ---------------- | + + mrs_msgs::Path path; + + path.fly_now = true; + path.use_heading = true; + + for (Eigen::Vector4d point : points) { + + mrs_msgs::Reference reference; + reference.position.x = point[0]; + reference.position.y = point[1]; + reference.position.z = point[2]; + reference.heading = point[3]; + + path.points.push_back(reference); + } + + // | -------------------- call the service -------------------- | + + { + auto [success, message] = setPathSrv(path); + + if (!success) { + ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | --------------- check if we pass each point -------------- | + + { + auto [success, message] = checkPathFlythrough(points); + + if (!success) { + ROS_ERROR("[%s]: path flythrough failed: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | ---------- wait and check if we are still flying --------- | + + 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 d181a54..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_trajectory_generation" -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 ./basic/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" diff --git a/test/topic_fly_now/CMakeLists.txt b/test/topic_fly_now/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/topic_fly_now/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/topic_fly_now/test.cpp b/test/topic_fly_now/test.cpp new file mode 100644 index 0000000..7689614 --- /dev/null +++ b/test/topic_fly_now/test.cpp @@ -0,0 +1,105 @@ +#include + +// include the generic test customized for this package +#include + +class Tester : public TrajectoryGenerationTest { + +public: + bool test(); +}; + +bool Tester::test() { + + std::vector points; + + points.push_back(Eigen::Vector4d(-5, -5, 5, 1)); + points.push_back(Eigen::Vector4d(-5, 5, 5, 2)); + points.push_back(Eigen::Vector4d(5, -5, 5, 3)); + points.push_back(Eigen::Vector4d(5, 5, 5, 4)); + + // | -------------------- activate the UAV -------------------- | + + { + 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; + } + } + + // | ---------------- prepare the path message ---------------- | + + mrs_msgs::Path path; + + path.fly_now = true; + path.use_heading = true; + + for (Eigen::Vector4d point : points) { + + mrs_msgs::Reference reference; + reference.position.x = point[0]; + reference.position.y = point[1]; + reference.position.z = point[2]; + reference.heading = point[3]; + + path.points.push_back(reference); + } + + // | -------------------- call the service -------------------- | + + { + auto [success, message] = setPathTopic(path); + + if (!success) { + ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | --------------- check if we pass each point -------------- | + + { + auto [success, message] = checkPathFlythrough(points); + + if (!success) { + ROS_ERROR("[%s]: path flythrough failed: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | ---------- wait and check if we are still flying --------- | + + 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/topic_fly_now/topic_fly_now.test b/test/topic_fly_now/topic_fly_now.test new file mode 100644 index 0000000..4d187b1 --- /dev/null +++ b/test/topic_fly_now/topic_fly_now.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +