From 46b897e9b4be7113bc2f75c8b3925659e8fa61ca Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Sun, 11 Feb 2024 10:46:30 +0100 Subject: [PATCH] updated tests --- test/CMakeLists.txt | 3 -- test/compile_tests.sh | 2 +- test/gazebo_takeoff/CMakeLists.txt | 16 ------- test/gazebo_takeoff/gazebo_takeoff.test | 35 -------------- test/gazebo_takeoff/test.cpp | 62 ------------------------- test/moving_drone/test.cpp | 27 ++++++++--- test/outside_safety_area/test.cpp | 27 ++++++++--- test/takeoff/test.cpp | 25 ++++++++-- test/topic_check/test.cpp | 27 ++++++++--- test/without_takeoff/test.cpp | 25 +++++++--- 10 files changed, 103 insertions(+), 146 deletions(-) delete mode 100644 test/gazebo_takeoff/CMakeLists.txt delete mode 100644 test/gazebo_takeoff/gazebo_takeoff.test delete mode 100644 test/gazebo_takeoff/test.cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f6a419a..52743ad 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -3,9 +3,6 @@ find_package(rostest REQUIRED) # will test takeoff in mrs simulator add_subdirectory(takeoff) -# will test takeoff in gazebo -add_subdirectory(gazebo_takeoff) - # will test if takeoff is not allowed outside of safety area add_subdirectory(outside_safety_area) diff --git a/test/compile_tests.sh b/test/compile_tests.sh index efbc058..a93d8bf 100755 --- a/test/compile_tests.sh +++ b/test/compile_tests.sh @@ -6,5 +6,5 @@ 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 -DMRS_ENABLE_TESTING=1 --no-deps # it has to be fully built normally before building with --catkin-make-args tests catkin build --this -DMRS_ENABLE_TESTING=1 --no-deps --catkin-make-args tests diff --git a/test/gazebo_takeoff/CMakeLists.txt b/test/gazebo_takeoff/CMakeLists.txt deleted file mode 100644 index bf8f985..0000000 --- a/test/gazebo_takeoff/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -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/gazebo_takeoff/gazebo_takeoff.test b/test/gazebo_takeoff/gazebo_takeoff.test deleted file mode 100644 index cc740a5..0000000 --- a/test/gazebo_takeoff/gazebo_takeoff.test +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/gazebo_takeoff/test.cpp b/test/gazebo_takeoff/test.cpp deleted file mode 100644 index 6eb2ab8..0000000 --- a/test/gazebo_takeoff/test.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#include - -#include - -class Tester : public mrs_uav_testing::TestGeneric { - -public: - bool test(); -}; - -bool Tester::test() { - - { - auto [success, message] = spawnGazeboUav(); - - if (!success) { - ROS_ERROR("[%s]: gazebo UAV spawning failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); - return false; - } - } - - { - 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->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/moving_drone/test.cpp b/test/moving_drone/test.cpp index 7b9eeb3..6f5ae79 100644 --- a/test/moving_drone/test.cpp +++ b/test/moving_drone/test.cpp @@ -36,13 +36,28 @@ void Tester::timerMain([[maybe_unused]] const ros::TimerEvent& event) { bool Tester::test() { - auto [success, message] = takeoff(); + std::shared_ptr uh; - if (!success) { - return true; - } else { - ROS_ERROR("[%s]: takeoff initiated, this should not be possible", ros::this_node::getName().c_str()); - return false; + { + auto [uhopt, message] = getUAVHandler(_uav_name_); + + if (!uhopt) { + ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); + return false; + } + + uh = uhopt.value(); + } + + { + auto [success, message] = uh->takeoff(); + + if (!success) { + return true; + } else { + ROS_ERROR("[%s]: takeoff initiated, this should not be possible", ros::this_node::getName().c_str()); + return false; + } } } diff --git a/test/outside_safety_area/test.cpp b/test/outside_safety_area/test.cpp index 10dc8e8..88d992b 100644 --- a/test/outside_safety_area/test.cpp +++ b/test/outside_safety_area/test.cpp @@ -10,13 +10,28 @@ class Tester : public mrs_uav_testing::TestGeneric { bool Tester::test() { - auto [success, message] = takeoff(); + std::shared_ptr uh; - if (!success) { - return true; - } else { - ROS_ERROR("[%s]: takeoff initiated, this should not be possible", ros::this_node::getName().c_str()); - return false; + { + auto [uhopt, message] = getUAVHandler(_uav_name_); + + if (!uhopt) { + ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); + return false; + } + + uh = uhopt.value(); + } + + { + auto [success, message] = uh->takeoff(); + + if (!success) { + return true; + } else { + ROS_ERROR("[%s]: takeoff initiated, this should not be possible", ros::this_node::getName().c_str()); + return false; + } } } diff --git a/test/takeoff/test.cpp b/test/takeoff/test.cpp index 13b8599..0fd0c14 100644 --- a/test/takeoff/test.cpp +++ b/test/takeoff/test.cpp @@ -10,16 +10,31 @@ class Tester : public mrs_uav_testing::TestGeneric { bool Tester::test() { - auto [success, message] = takeoff(); + std::shared_ptr uh; - if (!success) { - ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); - return false; + { + auto [uhopt, message] = getUAVHandler(_uav_name_); + + if (!uhopt) { + ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); + return false; + } + + uh = uhopt.value(); + } + + { + auto [success, message] = uh->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->isFlyingNormally()) { + if (uh->isFlyingNormally()) { return true; } else { ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); diff --git a/test/topic_check/test.cpp b/test/topic_check/test.cpp index f0cda40..07bfd56 100644 --- a/test/topic_check/test.cpp +++ b/test/topic_check/test.cpp @@ -10,13 +10,28 @@ class Tester : public mrs_uav_testing::TestGeneric { bool Tester::test() { - auto [success, message] = takeoff(); + std::shared_ptr uh; - if (!success) { - return true; - } else { - ROS_ERROR("[%s]: takeoff initiated, this should not be possible: '%s'", ros::this_node::getName().c_str(), message.c_str()); - return false; + { + auto [uhopt, message] = getUAVHandler(_uav_name_); + + if (!uhopt) { + ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); + return false; + } + + uh = uhopt.value(); + } + + { + auto [success, message] = uh->takeoff(); + + if (!success) { + return true; + } else { + ROS_ERROR("[%s]: takeoff initiated, this should not be possible: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } } } diff --git a/test/without_takeoff/test.cpp b/test/without_takeoff/test.cpp index 9b33f3c..14939a3 100644 --- a/test/without_takeoff/test.cpp +++ b/test/without_takeoff/test.cpp @@ -20,6 +20,19 @@ Tester::Tester() : mrs_uav_testing::TestGeneric() { bool Tester::test() { + std::shared_ptr uh; + + { + auto [uhopt, message] = getUAVHandler(_uav_name_); + + if (!uhopt) { + ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); + return false; + } + + uh = uhopt.value(); + } + // | ---------------- wait for ready to takeoff --------------- | while (true) { @@ -30,7 +43,7 @@ bool Tester::test() { ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", name_.c_str()); - if (mrsSystemReady()) { + if (uh->mrsSystemReady()) { ROS_INFO("[%s]: MRS UAV System is ready", name_.c_str()); break; } @@ -47,7 +60,7 @@ bool Tester::test() { srv.request.data = true; { - bool service_call = sch_arming_.call(srv); + bool service_call = uh->sch_arming_.call(srv); if (!service_call || !srv.response.success) { return false; @@ -61,7 +74,7 @@ bool Tester::test() { // | --------------------- check if armed --------------------- | - if (!sh_hw_api_status_.getMsg()->armed) { + if (!uh->sh_hw_api_status_.getMsg()->armed) { return false; } @@ -71,7 +84,7 @@ bool Tester::test() { std_srvs::Trigger srv; { - bool service_call = sch_offboard_.call(srv); + bool service_call = uh->sch_offboard_.call(srv); if (!service_call || !srv.response.success) { return false; @@ -85,7 +98,7 @@ bool Tester::test() { // | ------------------ check if in offboard ------------------ | - if (!sh_hw_api_status_.getMsg()->offboard) { + if (!uh->sh_hw_api_status_.getMsg()->offboard) { return false; } @@ -115,7 +128,7 @@ bool Tester::test() { ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the takeoff to finish", name_.c_str()); - if (sh_control_manager_diag_.getMsg()->flying_normally) { + if (uh->sh_control_manager_diag_.getMsg()->flying_normally) { return true; }