fixed escalating failsafe test against safety area

ctu-mrs · Jan 16, 2024 · 40755e6 · 40755e6
1 parent a18c860
commit 40755e6
Show file tree

Hide file tree

Showing 7 changed files with 76 additions and 6 deletions.
diff --git a/test/control_manager/escalating_failsafe_rc/config/custom_config.yaml b/test/control_manager/escalating_failsafe_rc/config/custom_config.yaml
@@ -9,10 +9,10 @@ mrs_uav_managers:
 
     # loaded state estimator plugins
     state_estimators: [
-      "gps_garmin",
+      "gps_baro",
     ]
 
-    initial_state_estimator: "gps_garmin" # will be used as the first state estimator
+    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)
 
   control_manager:

diff --git a/test/control_manager/escalating_failsafe_rc/config/world_config.yaml b/test/control_manager/escalating_failsafe_rc/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
diff --git a/test/control_manager/escalating_failsafe_rc/escalating_failsafe_rc.test b/test/control_manager/escalating_failsafe_rc/escalating_failsafe_rc.test
@@ -25,6 +25,7 @@
     <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>
 

diff --git a/test/control_manager/escalating_failsafe_service/config/custom_config.yaml b/test/control_manager/escalating_failsafe_service/config/custom_config.yaml
@@ -9,10 +9,10 @@ mrs_uav_managers:
 
     # loaded state estimator plugins
     state_estimators: [
-      "gps_garmin",
+      "gps_baro",
     ]
 
-    initial_state_estimator: "gps_garmin" # will be used as the first state estimator
+    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)
 
   control_manager:

diff --git a/test/control_manager/escalating_failsafe_service/config/world_config.yaml b/test/control_manager/escalating_failsafe_service/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
diff --git a/test/control_manager/escalating_failsafe_service/escalating_failsafe_service.test b/test/control_manager/escalating_failsafe_service/escalating_failsafe_service.test
@@ -25,6 +25,7 @@
     <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>
 

diff --git a/test/include/escalating_failsafe_test.h b/test/include/escalating_failsafe_test.h
@@ -24,7 +24,7 @@ bool EscalatingFailsafeTest::test() {
   // | ----------------------- goto higher ---------------------- |
 
   {
-    auto [success, message] = this->gotoAbs(0, 0, 10.0, 0);
+    auto [success, message] = this->gotoAbs(0, 0, 15.0, 0);
 
     if (!success) {
       ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
@@ -85,7 +85,7 @@ bool EscalatingFailsafeTest::test() {
 
   // check if we have not moved
 
-  if (!isAtPosition(0, 0, 10.0, 0, 0.5)) {
+  if (!isAtPosition(0, 0, 15.0, 0, 0.5)) {
     ROS_ERROR("[%s]: we moved after ehover", ros::this_node::getName().c_str());
     return false;
   }