added max height check test

src/uav_manager.cpp

@@ -880,7 +880,7 @@ void UavManager::timerMaxHeight(const ros::TimerEvent& event) {
 
   auto control_manager_diag = sh_control_manager_diag_.getMsg();
 
-  if (!fixing_min_height_ && !control_manager_diag->flying_normally) {
+  if (!fixing_max_height_ && !control_manager_diag->flying_normally) {
     return;
   }
 

test/uav_manager/CMakeLists.txt

@@ -5,3 +5,5 @@ add_subdirectory(landing)
 add_subdirectory(land_home)
 
 add_subdirectory(min_height_check)
+
+add_subdirectory(max_height_check)

test/uav_manager/max_height_check/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)

test/uav_manager/max_height_check/config/custom_config.yaml

@@ -0,0 +1,24 @@
+# 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)
+
+  uav_manager:
+
+    max_height_checking:
+
+      enabled: true
+      rate: 10.0 # [Hz]
+      safety_height_offset: 0.25 # how much lower to descend below the max height

test/uav_manager/max_height_check/config/world_config.yaml

@@ -0,0 +1,34 @@
+world_origin:
+
+  units: "LATLON" # {"UTM, "LATLON"}
+
+  origin_x: 47.397743
+  origin_y: 8.545594
+
+safety_area:
+
+  enabled: false
+
+  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

test/uav_manager/max_height_check/max_height_check.test

@@ -0,0 +1,37 @@
+<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/mrs_simulator.launch">
+    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
+  </include>
+
+  <include file="$(find mrs_multirotor_simulator)/launch/hw_api.launch">
+    <arg name="UAV_NAME" default="$(arg UAV_NAME)" />
+  </include>
+
+  <include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
+    <arg name="automatic_start" default="false" />
+    <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
+    <arg name="custom_config" default="$(dirname)/config/custom_config.yaml" />
+    <arg name="world_config" default="$(dirname)/config/world_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="60.0">
+    <param name="test" value="$(arg test_name)" />
+    <param name="uav_name" value="$(arg UAV_NAME)" />
+  </test>
+
+</launch>

test/uav_manager/max_height_check/test.cpp

@@ -0,0 +1,83 @@
+#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] = activateMidAir();
+
+    if (!success) {
+      ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  // | --------------- goto to violate min height --------------- |
+
+  {
+    auto [success, message] = this->gotoAbs(0, 0, 100, 0);
+
+    if (success) {
+      ROS_ERROR("[%s]: goto should fail", ros::this_node::getName().c_str());
+      return false;
+    }
+  }
+
+  // | --------- wait till we are flying normally again --------- |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return false;
+    }
+
+    if (this->isFlyingNormally()) {
+      break;
+    }
+  }
+
+  // | ------------------- check the altitude ------------------- |
+
+  auto height = this->getHeightAgl();
+
+  // TODO substitude with subscribed max agl height
+  double max_height_agl = 40;
+
+  if (height) {
+    if (height.value() < max_height_agl) {
+      return true;
+    }
+  }
+
+  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();
+}