Skip to content

Commit

Permalink
updated tests
Browse files Browse the repository at this point in the history
  • Loading branch information
klaxalk committed Nov 21, 2023
1 parent e55150c commit 949495e
Show file tree
Hide file tree
Showing 6 changed files with 54 additions and 61 deletions.
1 change: 1 addition & 0 deletions .github/workflows/ros_build_test.yml
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ on:

paths-ignore:
- '**/README.md'
- 'test/**'

pull_request:
branches: [ master ]
Expand Down
1 change: 1 addition & 0 deletions .github/workflows/ros_package_build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ on:

paths-ignore:
- '**/README.md'
- 'test/**'

workflow_dispatch:

Expand Down
19 changes: 9 additions & 10 deletions test/control_manager/goto/test.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
#include <ros/ros.h>
#include <ros/package.h>

#include <mrs_lib/subscribe_handler.h>
#include <mrs_lib/service_client_handler.h>
Expand Down Expand Up @@ -50,7 +49,7 @@ TEST(TESTSuite, goto) {
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics> sh_estim_manager_diag =
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "/" + uav_name + "/estimation_manager/diagnostics");

std::cout << "[Test]: subscribers initialized" << std::endl;
ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str());

// | --------------------- service clients -------------------- |

Expand All @@ -59,13 +58,13 @@ TEST(TESTSuite, goto) {
mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh, "/" + uav_name + "/uav_manager/midair_activation");
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4> sch_goto = mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>(nh, "/" + uav_name + "/control_manager/goto");

std::cout << "[Test]: service clients initialized" << std::endl;
ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str());

// | ---------------- wait for ready to takeoff --------------- |

while (ros::ok()) {

std::cout << "[Test]: waiting for 'ready to midair activation'" << std::endl;
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;
Expand All @@ -75,13 +74,13 @@ TEST(TESTSuite, goto) {
ros::Duration(0.1).sleep();
}

std::cout << "[Test]: we are ready 'for midair activation'" << std::endl;
ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str());

ros::Duration(1.0).sleep();

// | ---------------------- arm the drone --------------------- |

std::cout << "[Test]: arming the drone" << std::endl;
ROS_INFO("[%s]: arming th edrone", ros::this_node::getName().c_str());

std_srvs::SetBool arming;
arming.request.data = true;
Expand All @@ -94,7 +93,7 @@ TEST(TESTSuite, goto) {

// | -------------------- midair activation ------------------- |

std::cout << "[Test]: activating the drone 'in mid air'" << std::endl;
ROS_INFO("[%s]: activating the drone 'in mid air'", ros::this_node::getName().c_str());

std_srvs::Trigger midair;

Expand All @@ -115,15 +114,15 @@ TEST(TESTSuite, goto) {
goto_cmd.request.goal[2] = 5.5;
goto_cmd.request.goal[3] = 2.2;

std::cout << "[Test]: calling goto" << std::endl;
ROS_INFO("[%s]: calling goto", ros::this_node::getName().c_str());

sch_goto.call(goto_cmd);

// | ------------------ check for the result ------------------ |

while (ros::ok()) {

std::cout << "[Test]: waiting for the goto" << std::endl;
ROS_INFO_THROTTLE(1.0, "[%s]: waiting for goto", ros::this_node::getName().c_str());

auto diag = sh_estim_manager_diag.getMsg();

Expand All @@ -138,7 +137,7 @@ TEST(TESTSuite, goto) {
}

ros::spinOnce();
ros::Duration(1.0).sleep();
ros::Duration(0.01).sleep();
}

ROS_INFO("[%s]: finished", ros::this_node::getName().c_str());
Expand Down
35 changes: 0 additions & 35 deletions test/run_test.sh

This file was deleted.

33 changes: 33 additions & 0 deletions test/run_tests.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
#!/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_managers"
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 --catkin-make-args tests

TEST_FILES=$(find . -name "*.test" -type f -printf "%f\n")

for TEST_FILE in `echo $TEST_FILES`; do

# 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"

done
26 changes: 10 additions & 16 deletions test/uav_manager/takeoff/test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,6 @@
#include <std_srvs/SetBool.h>
#include <std_srvs/Trigger.h>

#include <ros/package.h>

#include <boost/process.hpp>

/* TEST(TESTSuite, takeoff) //{ */

TEST(TESTSuite, takeoff) {
Expand All @@ -24,10 +20,10 @@ TEST(TESTSuite, takeoff) {

// | ------------------ initialize test node ------------------ |

std::cout << "[Test]: initializing the test node" << std::endl;

ros::NodeHandle nh = ros::NodeHandle("~");

ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str());

ros::Time::waitForValid();

std::string uav_name = "uav1";
Expand All @@ -48,20 +44,20 @@ TEST(TESTSuite, takeoff) {
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag =
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "/" + uav_name + "/control_manager/diagnostics");

std::cout << "[Test]: subscribers initialized" << std::endl;
ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str());

// | --------------------- service clients -------------------- |

mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh, "/" + uav_name + "/hw_api/arming");
mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_offboard = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh, "/" + uav_name + "/hw_api/offboard");

std::cout << "[Test]: service clients initialized" << std::endl;
ROS_INFO("[%s]: service clients initialized", ros::this_node::getName().c_str());

// | ---------------- wait for ready to takeoff --------------- |

while (ros::ok()) {

std::cout << "[Test]: waiting for 'ready to takeoff'" << std::endl;
ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str());

if (sh_can_takeoff.hasMsg() && sh_can_takeoff.getMsg()->data) {
break;
Expand All @@ -71,11 +67,9 @@ TEST(TESTSuite, takeoff) {
ros::Duration(0.1).sleep();
}

std::cout << "[Test]: ready for takeoff" << std::endl;

// | ---------------------- arm the drone --------------------- |

std::cout << "[Test]: arming the drone" << std::endl;
ROS_INFO("[%s]: arming the drone", ros::this_node::getName().c_str());

std_srvs::SetBool arming;
arming.request.data = true;
Expand All @@ -88,7 +82,7 @@ TEST(TESTSuite, takeoff) {

// | -------------------- trigger offboard -------------------- |

std::cout << "[Test]: triggering offboard" << std::endl;
ROS_INFO("[%s]: triggering offboard", ros::this_node::getName().c_str());

std_srvs::Trigger offboard;

Expand All @@ -98,7 +92,7 @@ TEST(TESTSuite, takeoff) {

while (ros::ok()) {

std::cout << "[Test]: waiting for the takeoff to finish" << std::endl;
ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the takeoff to finish", ros::this_node::getName().c_str());

if (sh_control_manager_diag.getMsg()->flying_normally) {
result = 1;
Expand All @@ -109,7 +103,7 @@ TEST(TESTSuite, takeoff) {
ros::Duration(0.1).sleep();
}

std::cout << "[Test]: finished" << std::endl;
ROS_INFO("[%s]: finished", ros::this_node::getName().c_str());

EXPECT_TRUE(result);
}
Expand All @@ -120,7 +114,7 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) {

testing::InitGoogleTest(&argc, argv);

ros::init(argc, argv, "Test");
ros::init(argc, argv, "takeoff_test");

return RUN_ALL_TESTS();
}

0 comments on commit 949495e

Please sign in to comment.