From aa90745e337c2621c5fcd9c513ff4625afac3a10 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Mon, 29 Jan 2024 11:22:26 +0100
Subject: [PATCH] updated tests

---
 .../mrs_uav_gazebo_simulation/CMakeLists.txt  |  1 +
 .../mrs_uav_gazebo_simulation/package.xml     |  3 +-
 .../test/takeoff/CMakeLists.txt               | 17 +----
 .../takeoff/takeoff_gps_baro/CMakeLists.txt   | 16 +++++
 .../config/custom_config.yaml                 | 16 +++++
 .../takeoff_gps_baro/takeoff_gps_baro.test    | 36 +++++++++++
 .../takeoff/{ => takeoff_gps_baro}/test.cpp   |  0
 .../takeoff/takeoff_gps_garmin/CMakeLists.txt | 16 +++++
 .../config/custom_config.yaml                 | 16 +++++
 .../takeoff_gps_garmin.test}                  |  1 +
 .../test/takeoff/takeoff_gps_garmin/test.cpp  | 62 +++++++++++++++++++
 11 files changed, 167 insertions(+), 17 deletions(-)
 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
 rename ros_packages/mrs_uav_gazebo_simulation/test/takeoff/{ => takeoff_gps_baro}/test.cpp (100%)
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/CMakeLists.txt
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/config/custom_config.yaml
 rename ros_packages/mrs_uav_gazebo_simulation/test/takeoff/{takeoff.test => takeoff_gps_garmin/takeoff_gps_garmin.test} (94%)
 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 2d70e0e..6e04e3c 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt
+++ b/ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt
@@ -8,6 +8,7 @@ set(CATKIN_DEPENDENCIES
   mrs_msgs
   rospy
   std_msgs
+  mrs_uav_testing
   )
 
 find_package(catkin REQUIRED COMPONENTS
diff --git a/ros_packages/mrs_uav_gazebo_simulation/package.xml b/ros_packages/mrs_uav_gazebo_simulation/package.xml
index a038867..85a0a6f 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/package.xml
+++ b/ros_packages/mrs_uav_gazebo_simulation/package.xml
@@ -26,11 +26,10 @@
   <depend>rospy</depend>
   <depend>std_msgs</depend>
   <depend>ignition-math6</depend>
+  <depend>mrs_uav_testing</depend>
   
   <depend>python3-jinja2</depend>
 
-  <test_depend>mrs_uav_testing</test_depend>
-
   <export>
     <!-- gazebo_ros_paths_plugin automatically adds these to
       GAZEBO_PLUGIN_PATH and GAZEBO_MODEL_PATH when you do this export inside
diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt
index bf8f985..328847d 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt
@@ -1,16 +1,3 @@
-get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME)
+add_subdirectory(takeoff_gps_garmin)
 
-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)
+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/test.cpp b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp
similarity index 100%
rename from ros_packages/mrs_uav_gazebo_simulation/test/takeoff/test.cpp
rename to ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp
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/takeoff/takeoff_gps_garmin/config/custom_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/config/custom_config.yaml
new file mode 100644
index 0000000..4b0275a
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/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_garmin",
+    ]
+
+    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.test b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test
similarity index 94%
rename from ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff.test
rename to ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test
index cc740a5..ba16e3a 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff.test
+++ b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test
@@ -23,6 +23,7 @@
   <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>
 
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();
+}