From c4388b90f181179879531a25bf41bc81d5089d28 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Sun, 11 Feb 2024 11:16:22 +0100 Subject: [PATCH] updated tests --- .../test/compile_tests.sh | 2 +- .../test/takeoff/takeoff_gps_baro/test.cpp | 19 ++++++++++++++++--- .../test/takeoff/takeoff_gps_garmin/test.cpp | 19 ++++++++++++++++--- .../test/uav_models/test.cpp | 19 ++++++++++++++++--- 4 files changed, 49 insertions(+), 10 deletions(-) diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh b/ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh index efbc058..a93d8bf 100755 --- a/ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh +++ b/ros_packages/mrs_uav_gazebo_simulation/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/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp index 6eb2ab8..8f3b4f3 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp +++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp @@ -10,8 +10,21 @@ class Tester : public 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(); + } + { - auto [success, message] = spawnGazeboUav(); + auto [success, message] = uh->spawnGazeboUAV(_gazebo_spawner_params_); if (!success) { ROS_ERROR("[%s]: gazebo UAV spawning failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -20,7 +33,7 @@ bool Tester::test() { } { - auto [success, message] = takeoff(); + auto [success, message] = uh->takeoff(); if (!success) { ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -30,7 +43,7 @@ bool Tester::test() { 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/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/test.cpp b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/test.cpp index 6eb2ab8..8f3b4f3 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/test.cpp +++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/test.cpp @@ -10,8 +10,21 @@ class Tester : public 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(); + } + { - auto [success, message] = spawnGazeboUav(); + auto [success, message] = uh->spawnGazeboUAV(_gazebo_spawner_params_); if (!success) { ROS_ERROR("[%s]: gazebo UAV spawning failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -20,7 +33,7 @@ bool Tester::test() { } { - auto [success, message] = takeoff(); + auto [success, message] = uh->takeoff(); if (!success) { ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -30,7 +43,7 @@ bool Tester::test() { 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/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/test.cpp b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/test.cpp index 6eb2ab8..8f3b4f3 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/test.cpp +++ b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/test.cpp @@ -10,8 +10,21 @@ class Tester : public 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(); + } + { - auto [success, message] = spawnGazeboUav(); + auto [success, message] = uh->spawnGazeboUAV(_gazebo_spawner_params_); if (!success) { ROS_ERROR("[%s]: gazebo UAV spawning failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -20,7 +33,7 @@ bool Tester::test() { } { - auto [success, message] = takeoff(); + auto [success, message] = uh->takeoff(); if (!success) { ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -30,7 +43,7 @@ bool Tester::test() { 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());