diff --git a/test/compile_tests.sh b/test/compile_tests.sh index efbc058..b127806 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 --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/landoff_tracker/takeoff_landing/test.cpp b/test/landoff_tracker/takeoff_landing/test.cpp index 105a63a..95d9353 100644 --- a/test/landoff_tracker/takeoff_landing/test.cpp +++ b/test/landoff_tracker/takeoff_landing/test.cpp @@ -10,10 +10,23 @@ 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(); + } + // | ------------------------- takeoff ------------------------ | { - 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()); @@ -23,7 +36,7 @@ bool Tester::test() { this->sleep(1.0); - if (!this->isFlyingNormally()) { + if (!uh->isFlyingNormally()) { ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); return false; } @@ -31,7 +44,7 @@ bool Tester::test() { // | --------------------- goto somewhere --------------------- | { - auto [success, message] = gotoRel(10, 1, 2, 1.5); + auto [success, message] = uh->gotoRel(10, 1, 2, 1.5); if (!success) { ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -41,7 +54,7 @@ bool Tester::test() { this->sleep(1.0); - if (!this->isFlyingNormally()) { + if (!uh->isFlyingNormally()) { ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); return false; } @@ -49,7 +62,7 @@ bool Tester::test() { // | -------------------------- land -------------------------- | { - auto [success, message] = land(); + auto [success, message] = uh->land(); if (!success) { ROS_ERROR("[%s]: landing failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); diff --git a/test/line_tracker/line_tracker_goto/test.cpp b/test/line_tracker/line_tracker_goto/test.cpp index 3b87498..b776bb7 100644 --- a/test/line_tracker/line_tracker_goto/test.cpp +++ b/test/line_tracker/line_tracker_goto/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] = activateMidAir(); + auto [success, message] = uh->activateMidAir(); if (!success) { ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -19,13 +32,13 @@ bool Tester::test() { } } - if (this->getActiveTracker() != "LineTracker") { + if (uh->getActiveTracker() != "LineTracker") { ROS_ERROR("[%s]: LineTracker is not active", ros::this_node::getName().c_str()); return false; } { - auto [success, message] = this->gotoRel(10, 1, 2, 1.5); + auto [success, message] = uh->gotoRel(10, 1, 2, 1.5); if (!success) { ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -35,7 +48,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/test/mpc_tracker/mpc_tracker_goto/test.cpp b/test/mpc_tracker/mpc_tracker_goto/test.cpp index a51cee0..d71eb95 100644 --- a/test/mpc_tracker/mpc_tracker_goto/test.cpp +++ b/test/mpc_tracker/mpc_tracker_goto/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] = activateMidAir(); + auto [success, message] = uh->activateMidAir(); if (!success) { ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -19,13 +32,13 @@ bool Tester::test() { } } - if (this->getActiveTracker() != "MpcTracker") { + if (uh->getActiveTracker() != "MpcTracker") { ROS_ERROR("[%s]: MpcTracker is not active", ros::this_node::getName().c_str()); return false; } { - auto [success, message] = this->gotoRel(10, 1, 2, 1.5); + auto [success, message] = uh->gotoRel(10, 1, 2, 1.5); if (!success) { ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -35,7 +48,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());