From b27bc2e1fd7cdabd62b87e63986cea3e849e553a Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Thu, 1 Feb 2024 10:01:58 +0100
Subject: [PATCH] restored the testing pipeline from history

---
 .../mrs_uav_gazebo_simulation/CMakeLists.txt  |  10 +-
 .../mrs_uav_gazebo_simulation/package.xml     |   1 +
 .../test/CMakeLists.txt                       |  24 +-
 .../test/all_tests.sh                         |  43 ++--
 .../test/basic/basic.test                     |   7 -
 .../test/basic/config/network_config.yaml     |  15 --
 .../test/basic/config/world_config.yaml       |  34 ---
 .../test/basic/launch/mrs_uav_system.launch   |  36 ---
 .../test/basic/test.cpp                       | 242 ------------------
 .../test/compile_tests.sh                     |  10 +
 .../test/coverage.sh                          |  25 --
 .../test/single_test.sh                       |  27 --
 .../test/takeoff/CMakeLists.txt               |   3 +
 .../takeoff/takeoff_gps_baro/CMakeLists.txt   |  16 ++
 .../config/custom_config.yaml                 |  16 ++
 .../takeoff_gps_baro/takeoff_gps_baro.test    |  36 +++
 .../test/takeoff/takeoff_gps_baro/test.cpp    |  62 +++++
 .../takeoff/takeoff_gps_garmin/CMakeLists.txt |  16 ++
 .../config/custom_config.yaml                 |   4 +-
 .../takeoff_gps_garmin.test                   |  36 +++
 .../test/takeoff/takeoff_gps_garmin/test.cpp  |  62 +++++
 21 files changed, 289 insertions(+), 436 deletions(-)
 delete mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/basic/basic.test
 delete mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/basic/config/network_config.yaml
 delete mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/basic/config/world_config.yaml
 delete mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/basic/launch/mrs_uav_system.launch
 delete mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/basic/test.cpp
 create mode 100755 ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh
 delete mode 100755 ros_packages/mrs_uav_gazebo_simulation/test/coverage.sh
 delete mode 100755 ros_packages/mrs_uav_gazebo_simulation/test/single_test.sh
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/CMakeLists.txt
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/config/custom_config.yaml
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/takeoff_gps_baro.test
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/CMakeLists.txt
 rename ros_packages/mrs_uav_gazebo_simulation/test/{basic => takeoff/takeoff_gps_garmin}/config/custom_config.yaml (82%)
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/test.cpp

diff --git a/ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt
index a745ad2..3730dca 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt
+++ b/ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt
@@ -13,6 +13,7 @@ set(CATKIN_DEPENDENCIES
   px4
   rospy
   std_msgs
+  mrs_uav_testing
   )
 
 find_package(catkin REQUIRED COMPONENTS
@@ -32,15 +33,18 @@ include_directories(
   ${mavros_msgs_INCLUDE_DIRS}
   )
 
+if(COVERAGE)
+  message(WARNING "building with --coverage, the performance might be limited")
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --coverage")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage")
+endif()
+
 ## --------------------------------------------------------------
 ## |                           Testing                          |
 ## --------------------------------------------------------------
 
 if(CATKIN_ENABLE_TESTING)
 
-  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --coverage")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage")
-
   add_subdirectory(test)
 
 endif()
diff --git a/ros_packages/mrs_uav_gazebo_simulation/package.xml b/ros_packages/mrs_uav_gazebo_simulation/package.xml
index 9d1f34d..b7e1615 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/package.xml
+++ b/ros_packages/mrs_uav_gazebo_simulation/package.xml
@@ -25,6 +25,7 @@
   <depend>px4</depend>
   <depend>rospy</depend>
   <depend>std_msgs</depend>
+  <depend>mrs_uav_testing</depend>
 
   <depend>python3-jinja2</depend>
 
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt
index 49d9d9c..5084ddf 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt
@@ -1,25 +1,3 @@
 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
-###     )
-
-# 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(takeoff)
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/all_tests.sh b/ros_packages/mrs_uav_gazebo_simulation/test/all_tests.sh
index 676421e..6614ec3 100755
--- a/ros_packages/mrs_uav_gazebo_simulation/test/all_tests.sh
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/all_tests.sh
@@ -5,31 +5,30 @@ 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_gazebo_simulation"
-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")
+ORIGINAL_PATH=`pwd`
+
+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
 
-for TEST_FILE in `echo $TEST_FILES`; do
+cd build
 
-  echo "$0: running test '$TEST_FILE'"
+OLD_FILES=$(find . -name "*.gcda")
 
-  # folder for test results
-  TEST_RESULT_PATH=$(realpath /tmp/$RANDOM)
-  mkdir -p $TEST_RESULT_PATH
+for FILE in $OLD_FILES; do
+  echo "$0: removing old coverage file '$FILE'"
+  rm $FILE
+done
 
-  # run the test
-  rostest $PACKAGE $TEST_FILE $TEXT_OUTPUT --results-filename=$PACKAGE.test --results-base-dir="$TEST_RESULT_PATH"
+cd $ORIGINAL_PATH
 
-  # evaluate the test results
-  echo test result path is $TEST_RESULT_PATH
-  catkin_test_results "$TEST_RESULT_PATH"
+# build the package
+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/ros_packages/mrs_uav_gazebo_simulation/test/basic/basic.test b/ros_packages/mrs_uav_gazebo_simulation/test/basic/basic.test
deleted file mode 100644
index b697bb9..0000000
--- a/ros_packages/mrs_uav_gazebo_simulation/test/basic/basic.test
+++ /dev/null
@@ -1,7 +0,0 @@
-<launch>
-
-  <include file="$(find mrs_uav_gazebo_simulation)/test/basic/launch/mrs_uav_system.launch" />
-
-  <test pkg="mrs_uav_gazebo_simulation" type="test_basic" test-name="basic" time-limit="120.0" />
-
-</launch>
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/network_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/network_config.yaml
deleted file mode 100644
index 08f370d..0000000
--- a/ros_packages/mrs_uav_gazebo_simulation/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/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/world_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/world_config.yaml
deleted file mode 100644
index 9b55067..0000000
--- a/ros_packages/mrs_uav_gazebo_simulation/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/ros_packages/mrs_uav_gazebo_simulation/test/basic/launch/mrs_uav_system.launch b/ros_packages/mrs_uav_gazebo_simulation/test/basic/launch/mrs_uav_system.launch
deleted file mode 100644
index 0efb4f1..0000000
--- a/ros_packages/mrs_uav_gazebo_simulation/test/basic/launch/mrs_uav_system.launch
+++ /dev/null
@@ -1,36 +0,0 @@
-<launch>
-
-  <arg name="this_session_path" default="$(find mrs_uav_gazebo_simulation)/test/basic" />
-
-  <arg name="UAV_NAME" default="uav1" />
-  <arg name="RUN_TYPE" default="simulation" />
-  <arg name="UAV_TYPE" default="x500" />
-
-  <include file="$(find mrs_uav_gazebo_simulation)/launch/simulation.launch">
-    <arg name="world_name" default="grass_plane" />
-    <arg name="gui" default="false" />
-  </include>
-
-  <include file="$(find mrs_uav_px4_api)/launch/api.launch">
-    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
-    <arg name="RUN_TYPE" default="$(arg RUN_TYPE)" />
-  </include>
-
-  <include file="$(find mrs_uav_autostart)/launch/automatic_start.launch">
-    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
-    <arg name="RUN_TYPE" default="$(arg RUN_TYPE)" />
-  </include>
-
-  <include file="$(find mrs_uav_core)/launch/core.launch">
-
-    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
-    <arg name="RUN_TYPE" default="$(arg RUN_TYPE)" />
-
-    <arg name="world_config" default="$(arg this_session_path)/config/world_config.yaml" />
-    <arg name="network_config" default="$(arg this_session_path)/config/network_config.yaml" />
-    <arg name="custom_config" default="$(arg this_session_path)/config/custom_config.yaml" />
-    <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
-
-  </include>
-
-</launch>
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/basic/test.cpp b/ros_packages/mrs_uav_gazebo_simulation/test/basic/test.cpp
deleted file mode 100644
index e605ac5..0000000
--- a/ros_packages/mrs_uav_gazebo_simulation/test/basic/test.cpp
+++ /dev/null
@@ -1,242 +0,0 @@
-#include <gtest/gtest.h>
-#include <ros/console.h>
-#include <log4cxx/logger.h>
-
-#include <mrs_lib/subscribe_handler.h>
-#include <mrs_lib/service_client_handler.h>
-
-#include <std_msgs/Bool.h>
-#include <mrs_msgs/ControlManagerDiagnostics.h>
-#include <mrs_msgs/EstimationDiagnostics.h>
-#include <mrs_msgs/String.h>
-#include <mrs_msgs/GazeboSpawnerDiagnostics.h>
-
-#include <std_srvs/SetBool.h>
-#include <std_srvs/Trigger.h>
-
-/* class Tester //{ */
-
-class Tester {
-
-public:
-  Tester();
-
-  bool test();
-
-private:
-  ros::NodeHandle                    nh_;
-  std::shared_ptr<ros::AsyncSpinner> spinner_;
-
-  mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
-  mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>     sh_estim_manager_diag_;
-  mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>  sh_spawner_diagnostics_;
-  mrs_lib::SubscribeHandler<std_msgs::Bool>                      sh_can_takeoff;
-
-  mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
-  mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_offboard_;
-
-  mrs_lib::ServiceClientHandler<mrs_msgs::String> sch_spawn_;
-};
-
-//}
-
-/* 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<ros::AsyncSpinner>(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<mrs_msgs::ControlManagerDiagnostics>(shopts, "/" + uav_name + "/control_manager/diagnostics");
-  sh_estim_manager_diag_   = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "/" + uav_name + "/estimation_manager/diagnostics");
-  sh_can_takeoff           = mrs_lib::SubscribeHandler<std_msgs::Bool>(shopts, "/" + uav_name + "/automatic_start/can_takeoff");
-  sh_spawner_diagnostics_  = mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>(shopts, "/mrs_drone_spawner/diagnostics");
-
-  ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str());
-
-  // | --------------------- service clients -------------------- |
-
-  sch_arming_   = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "/" + uav_name + "/hw_api/arming");
-  sch_offboard_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + uav_name + "/hw_api/offboard");
-  sch_spawn_    = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "/mrs_drone_spawner/spawn");
-
-  ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str());
-}
-
-//}
-
-/* test() //{ */
-
-bool Tester::test() {
-
-  // | ------------ wait for the spawner diagnostics ------------ |
-
-  while (ros::ok()) {
-
-    ROS_INFO_THROTTLE(1.0, "[%s]: waiting for spawner diagnostics", ros::this_node::getName().c_str());
-
-    if (sh_spawner_diagnostics_.hasMsg()) {
-      break;
-    }
-
-    ros::Duration(0.01).sleep();
-  }
-
-  // | -------------------------- spawn ------------------------- |
-
-  ROS_INFO("[%s]: spawning the drone", ros::this_node::getName().c_str());
-
-  {
-    mrs_msgs::String spawn;
-    spawn.request.value = "1 --x500 --enable-rangefinder";
-
-    {
-      bool service_call = sch_spawn_.call(spawn);
-
-      if (!service_call || !spawn.response.success) {
-        ROS_ERROR("[%s]: spawning failed", ros::this_node::getName().c_str());
-        return false;
-      }
-    }
-  }
-
-  // | ------------------- wait for the spawn ------------------- |
-
-  while (ros::ok()) {
-
-    ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the spawn", ros::this_node::getName().c_str());
-
-    if (!sh_spawner_diagnostics_.getMsg()->processing) {
-      break;
-    }
-
-    ros::Duration(0.01).sleep();
-  }
-
-  ROS_INFO("[%s]: The UAV is ready", ros::this_node::getName().c_str());
-
-  ros::Duration(1.0).sleep();
-
-  // | ---------------- 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();
-
-  // | -------------------- trigger offboard -------------------- |
-
-  ROS_INFO("[%s]: triggering offboard", ros::this_node::getName().c_str());
-
-  {
-    std_srvs::Trigger trigger;
-
-    {
-      bool service_call = sch_offboard_.call(trigger);
-
-      if (!service_call || !trigger.response.success) {
-        ROS_ERROR("[%s]: offboard service call failed", ros::this_node::getName().c_str());
-        return false;
-      }
-    }
-  }
-
-  // | -------------- wait for the takeoff finished ------------- |
-
-  while (ros::ok()) {
-
-    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) {
-
-      ROS_INFO("[%s]: finished", ros::this_node::getName().c_str());
-
-      return true;
-    }
-
-    ros::Duration(0.01).sleep();
-  }
-
-  return false;
-}
-
-//}
-
-std::shared_ptr<Tester> tester_;
-
-TEST(TESTSuite, takeoff) {
-
-  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, "takeoff_test");
-
-  tester_ = std::make_shared<Tester>();
-
-  testing::InitGoogleTest(&argc, argv);
-
-  Tester tester;
-
-  return RUN_ALL_TESTS();
-}
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh b/ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh
new file mode 100755
index 0000000..999699e
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh
@@ -0,0 +1,10 @@
+#!/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
+
+# build the package
+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
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/coverage.sh b/ros_packages/mrs_uav_gazebo_simulation/test/coverage.sh
deleted file mode 100755
index db8ab5d..0000000
--- a/ros_packages/mrs_uav_gazebo_simulation/test/coverage.sh
+++ /dev/null
@@ -1,25 +0,0 @@
-#!/bin/bash
-
-PACKAGE_NAME=mrs_uav_gazebo_simulation
-
-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/ros_packages/mrs_uav_gazebo_simulation/test/single_test.sh b/ros_packages/mrs_uav_gazebo_simulation/test/single_test.sh
deleted file mode 100755
index e8abc69..0000000
--- a/ros_packages/mrs_uav_gazebo_simulation/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_gazebo_simulation"
-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/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt
new file mode 100644
index 0000000..328847d
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt
@@ -0,0 +1,3 @@
+add_subdirectory(takeoff_gps_garmin)
+
+add_subdirectory(takeoff_gps_baro)
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/CMakeLists.txt
new file mode 100644
index 0000000..bf8f985
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/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/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/config/custom_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/config/custom_config.yaml
new file mode 100644
index 0000000..cd90283
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/config/custom_config.yaml
@@ -0,0 +1,16 @@
+# 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: "" # only slightly filtered height for checking min height (not used in control feedback)
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/takeoff_gps_baro.test b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/takeoff_gps_baro.test
new file mode 100644
index 0000000..536da84
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/takeoff_gps_baro.test
@@ -0,0 +1,36 @@
+<launch>
+
+  <arg name="this_path" default="$(dirname)" />
+
+  <arg name="UAV_NAME" default="uav1" />
+  <arg name="UAV_TYPE" default="x500" />
+
+    <!-- automatically deduce the test name -->
+  <arg name="test_name" default="$(eval arg('this_path').split('/')[-1])" />
+
+    <!-- automatically deduce the package name -->
+  <arg name="import_eval" default="eval('_' + '_import_' + '_')"/>
+  <arg name="package_eval" default="eval(arg('import_eval') + '(\'rospkg\')').get_package_name(arg('this_path'))" />
+  <arg name="package" default="$(eval eval(arg('package_eval')))" />
+
+  <include file="$(find mrs_uav_testing)/launch/gazebo_simulator.launch" />
+
+  <include file="$(find mrs_uav_px4_api)/launch/api.launch">
+    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
+    <arg name="RUN_TYPE" default="simulation" />
+  </include>
+
+  <include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
+    <arg name="automatic_start" default="true" />
+    <arg name="platform_config" default="$(find mrs_uav_gazebo_simulation)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
+    <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
+    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
+  </include>
+
+  <test pkg="$(arg package)" type="test_$(arg test_name)" test-name="$(arg test_name)" time-limit="120.0">
+    <param name="test_name" value="$(arg test_name)" />
+    <param name="uav_name" value="$(arg UAV_NAME)" />
+    <param name="gazebo_spawner_params" value="1 --$(arg UAV_TYPE)" />
+  </test>
+
+</launch>
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
new file mode 100644
index 0000000..6eb2ab8
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp
@@ -0,0 +1,62 @@
+#include <gtest/gtest.h>
+
+#include <mrs_uav_testing/test_generic.h>
+
+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/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/CMakeLists.txt
new file mode 100644
index 0000000..bf8f985
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/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/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/custom_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/config/custom_config.yaml
similarity index 82%
rename from ros_packages/mrs_uav_gazebo_simulation/test/basic/config/custom_config.yaml
rename to ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/config/custom_config.yaml
index 9f6bbb8..4b0275a 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/custom_config.yaml
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/config/custom_config.yaml
@@ -9,8 +9,8 @@ mrs_uav_managers:
 
     # loaded state estimator plugins
     state_estimators: [
-      "gps_baro",
+      "gps_garmin",
     ]
 
-    initial_state_estimator: "gps_baro" # will be used as the first state estimator
+    initial_state_estimator: "gps_garmin" # 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)
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test
new file mode 100644
index 0000000..ba16e3a
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test
@@ -0,0 +1,36 @@
+<launch>
+
+  <arg name="this_path" default="$(dirname)" />
+
+  <arg name="UAV_NAME" default="uav1" />
+  <arg name="UAV_TYPE" default="x500" />
+
+    <!-- automatically deduce the test name -->
+  <arg name="test_name" default="$(eval arg('this_path').split('/')[-1])" />
+
+    <!-- automatically deduce the package name -->
+  <arg name="import_eval" default="eval('_' + '_import_' + '_')"/>
+  <arg name="package_eval" default="eval(arg('import_eval') + '(\'rospkg\')').get_package_name(arg('this_path'))" />
+  <arg name="package" default="$(eval eval(arg('package_eval')))" />
+
+  <include file="$(find mrs_uav_testing)/launch/gazebo_simulator.launch" />
+
+  <include file="$(find mrs_uav_px4_api)/launch/api.launch">
+    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
+    <arg name="RUN_TYPE" default="simulation" />
+  </include>
+
+  <include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
+    <arg name="automatic_start" default="true" />
+    <arg name="platform_config" default="$(find mrs_uav_gazebo_simulation)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
+    <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
+    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
+  </include>
+
+  <test pkg="$(arg package)" type="test_$(arg test_name)" test-name="$(arg test_name)" time-limit="120.0">
+    <param name="test_name" value="$(arg test_name)" />
+    <param name="uav_name" value="$(arg UAV_NAME)" />
+    <param name="gazebo_spawner_params" value="1 --$(arg UAV_TYPE) --enable-rangefinder" />
+  </test>
+
+</launch>
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
new file mode 100644
index 0000000..6eb2ab8
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/test.cpp
@@ -0,0 +1,62 @@
+#include <gtest/gtest.h>
+
+#include <mrs_uav_testing/test_generic.h>
+
+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();
+}