From 40755e62b155762c51dc9ade9b1bb71bf038574f Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Tue, 16 Jan 2024 08:33:42 +0100 Subject: [PATCH] fixed escalating failsafe test against safety area --- .../config/custom_config.yaml | 4 +-- .../config/world_config.yaml | 34 +++++++++++++++++++ .../escalating_failsafe_rc.test | 1 + .../config/custom_config.yaml | 4 +-- .../config/world_config.yaml | 34 +++++++++++++++++++ .../escalating_failsafe_service.test | 1 + test/include/escalating_failsafe_test.h | 4 +-- 7 files changed, 76 insertions(+), 6 deletions(-) create mode 100644 test/control_manager/escalating_failsafe_rc/config/world_config.yaml create mode 100644 test/control_manager/escalating_failsafe_service/config/world_config.yaml diff --git a/test/control_manager/escalating_failsafe_rc/config/custom_config.yaml b/test/control_manager/escalating_failsafe_rc/config/custom_config.yaml index 65070a63..3d6eb110 100644 --- 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 new file mode 100644 index 00000000..00e01eef --- /dev/null +++ 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 index 41389f07..54865e18 100644 --- 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 @@ + diff --git a/test/control_manager/escalating_failsafe_service/config/custom_config.yaml b/test/control_manager/escalating_failsafe_service/config/custom_config.yaml index 65070a63..3d6eb110 100644 --- 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 new file mode 100644 index 00000000..00e01eef --- /dev/null +++ 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 index c17dfb02..36898b00 100644 --- 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 @@ + diff --git a/test/include/escalating_failsafe_test.h b/test/include/escalating_failsafe_test.h index 76be291d..86b6230b 100644 --- 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; }