From 925ad97ff3aec3b22aa1bdd5f834957ac0fc264c Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 10 Jan 2023 17:04:39 +0300 Subject: [PATCH] added unit tests Signed-off-by: RomanBapst --- .../MissionFeasibility/CMakeLists.txt | 4 +- .../MissionFeasibility/FeasibilityChecker.cpp | 31 +- .../MissionFeasibility/FeasibilityChecker.hpp | 1 + .../FeasibilityCheckerTest.cpp | 410 +++++++++++++++++- 4 files changed, 430 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/CMakeLists.txt b/src/modules/navigator/MissionFeasibility/CMakeLists.txt index bebf2fa5df..72ee524a3d 100644 --- a/src/modules/navigator/MissionFeasibility/CMakeLists.txt +++ b/src/modules/navigator/MissionFeasibility/CMakeLists.txt @@ -2,4 +2,6 @@ px4_add_library(mission_feasibility_checker FeasibilityChecker.cpp ) -px4_add_functional_gtest(SRC FeasibilityCheckerTest.cpp LINKLIBS mission_feasibility_checker test_stubs) +target_link_libraries(mission_feasibility_checker PUBLIC modules__navigator modules__dataman) + +px4_add_functional_gtest(SRC FeasibilityCheckerTest.cpp LINKLIBS mission_feasibility_checker) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index 86e5a56533..cef803fff2 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -46,6 +46,13 @@ FeasibilityChecker::FeasibilityChecker() : void FeasibilityChecker::reset() { + + _is_landed = false; + _home_alt_msl = NAN; + _home_lat_lon = matrix::Vector2d(NAN, NAN); + _vehicle_type = 0; + _is_vtol = false; + _mission_validity_failed = false; _takeoff_failed = false; _fixed_wing_landing_failed = false; @@ -75,7 +82,8 @@ void FeasibilityChecker::updateData() { home_position_s home = {}; - if (_home_pos_sub.copy(&home)) { + if (_home_pos_sub.updated()) { + _home_pos_sub.update(&home); if (home.valid_hpos) { _home_lat_lon = matrix::Vector2d(home.lat, home.lon); } @@ -87,14 +95,16 @@ void FeasibilityChecker::updateData() vehicle_status_s status = {}; - if (_status_sub.copy(&status)) { + if (_status_sub.updated()) { + _status_sub.copy(&status); _vehicle_type = status.vehicle_type; _is_vtol = status.is_vtol; } vehicle_land_detected_s land_detected = {}; - if (_land_detector_sub.copy(&land_detected)) { + if (_land_detector_sub.updated()) { + _land_detector_sub.copy(&land_detected); _is_landed = land_detected.landed; } @@ -119,7 +129,7 @@ void FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int doMulticopterChecks(mission_item, current_index); } - if (current_index == total_count) { + if (current_index == total_count - 1) { _takeoff_land_available_failed = !checkTakeoffLandAvailable(); } @@ -162,7 +172,7 @@ void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const i _fixed_wing_landing_failed = !checkFixedWingLanding(mission_item, current_index); } - if (_fixed_wing_land_approach_failed) { + if (!_fixed_wing_land_approach_failed) { _fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index); } @@ -431,12 +441,6 @@ bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item _landing_valid = true; - } else { - // mission item before land doesn't have a position - mavlink_log_critical(_mavlink_log_pub, "Mission rejected: need landing approach.\t"); - events::send(events::ID("navigator_mis_req_landing_approach"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: landing approach is required"); - return false; } } @@ -445,7 +449,6 @@ bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item bool FeasibilityChecker::checkFixedWingLanding(mission_item_s &mission_item, const int current_index) { - const bool land_start_found = _do_land_start_index > 0; // if DO_LAND_START found then require valid landing AFTER if (mission_item.nav_cmd == NAV_CMD_DO_LAND_START) { @@ -460,6 +463,8 @@ bool FeasibilityChecker::checkFixedWingLanding(mission_item_s &mission_item, con _do_land_start_index = current_index; } + const bool land_start_found = _do_land_start_index >= 0; + if (mission_item.nav_cmd == NAV_CMD_LAND || mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { if (current_index > 0) { @@ -563,7 +568,7 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item) { - if (_param_mis_dist_1wp.get() <= 0.0f) { + if (_param_mis_dist_1wp.get() <= 0.0f || !_home_lat_lon.isAllFinite()) { /* param not set, check is ok */ return true; } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index 5ae2bf0ce4..aae5ea301c 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -60,6 +60,7 @@ public: _distance_first_waypoint_failed || _distance_between_waypoints_failed || _fixed_wing_landing_failed || + _fixed_wing_land_approach_failed || _below_home_alt_failed || _mission_validity_failed || _takeoff_land_available_failed; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index 727581ca2e..115887add9 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -33,9 +33,12 @@ #include #include "FeasibilityChecker.hpp" +#include +#include +#include + // to run: make tests TESTFILTER=FeasibilityChecker - class FeasibilityCheckerTest : public ::testing::Test { public: @@ -51,7 +54,410 @@ class TestFeasibilityChecker : public FeasibilityChecker public: TestFeasibilityChecker() : FeasibilityChecker() {} void paramsChanged() {FeasibilityChecker::updateParamsImpl();} + + void publishHomePosition(double lat, double lon, float alt) + { + home_position_s home = {}; + home.alt = alt; + home.valid_alt = true; + home.lat = lat; + home.lon = lon; + home.valid_hpos = true; + orb_advert_t home_pub = orb_advertise(ORB_ID(home_position), &home); + orb_publish(ORB_ID(home_position), home_pub, &home); + } + + void publishLanded(bool landed) + { + vehicle_land_detected_s land_detected = {}; + land_detected.landed = true; + orb_advert_t landed_pub = orb_advertise(ORB_ID(vehicle_land_detected), &land_detected); + orb_publish(ORB_ID(vehicle_land_detected), landed_pub, &land_detected); + } + + void publishVehicleType(int vehicle_type) + { + vehicle_status_s status = {}; + status.vehicle_type = vehicle_type; + orb_advert_t status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + } + }; +TEST_F(FeasibilityCheckerTest, instantiation) +{ + FeasibilityChecker checker(); +} -TEST_F(FeasibilityCheckerTest, instantiation) { FeasibilityChecker cp(); } +TEST_F(FeasibilityCheckerTest, mission_item_validity) +{ + TestFeasibilityChecker checker; + + + checker.publishLanded(true); + + // supported mission item should pass + mission_item_s mission_item = {}; + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), false); + + // when landed the first item cannot be a land item + checker.reset(); + checker.publishLanded(true); + mission_item.nav_cmd = NAV_CMD_LAND; + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), true); +} + +TEST_F(FeasibilityCheckerTest, max_dist_between_waypoints) +{ + TestFeasibilityChecker checker; + checker.publishLanded(true); + + param_t param = param_handle(px4::params::MIS_DIST_WPS); + + float max_dist = 1000.0f; + param_set(param, &max_dist); + checker.paramsChanged(); + + mission_item_s mission_item = {}; + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + checker.processNextItem(mission_item, 0, 3); + + // waypoints are within limits, check should pass + double lat_new, lon_new; + waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 999, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + checker.processNextItem(mission_item, 1, 3); + + ASSERT_EQ(checker.someCheckFailed(), false); + + // waypoints are above limits, check should fail + waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1001, &lat_new, &lon_new); + + mission_item.lat = lat_new; + mission_item.lon = lon_new; + checker.processNextItem(mission_item, 2, 3); + + ASSERT_EQ(checker.someCheckFailed(), true); +} + + +TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint) +{ + TestFeasibilityChecker checker; + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 0); + + param_t param = param_handle(px4::params::MIS_DIST_1WP); + + float max_dist = 500.0f; + param_set(param, &max_dist); + checker.paramsChanged(); + + + mission_item_s mission_item = {}; + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + double lat_new, lon_new; + waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 501, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + checker.processNextItem(mission_item, 0, 1); + + ASSERT_EQ(checker.someCheckFailed(), true); + + checker.reset(); + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 0); + waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + checker.processNextItem(mission_item, 0, 1); + + ASSERT_EQ(checker.someCheckFailed(), false); +} + +TEST_F(FeasibilityCheckerTest, check_below_home) +{ + TestFeasibilityChecker checker; + checker.publishLanded(true); + //checker.publishHomePosition(0,0,100); + + + mission_item_s mission_item = {}; + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + mission_item.altitude = 50; + mission_item.altitude_is_relative = true; + + checker.processNextItem(mission_item, 0, 1); + + // this is done to invalidate the home position + checker.reset(); + checker.publishLanded(true); + checker.processNextItem(mission_item, 0, 1); + + // cannot have relative altitude without valid home position + ASSERT_EQ(checker.someCheckFailed(), true); +} + +TEST_F(FeasibilityCheckerTest, check_takeoff) +{ + TestFeasibilityChecker checker; + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 100); + + + // takeoff altitude is smaller than acceptance radius, should fail + mission_item_s mission_item = {}; + mission_item.nav_cmd = NAV_CMD_TAKEOFF; + mission_item.altitude = 5.0f; + mission_item.altitude_is_relative = true; + mission_item.acceptance_radius = 10.0f; + + checker.processNextItem(mission_item, 0, 1); + + ASSERT_EQ(checker.someCheckFailed(), true); + + checker.reset(); + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 100); + + + // takeoff altitude is larger than acceptance radius, should pass + mission_item.acceptance_radius = 3.0f; + checker.processNextItem(mission_item, 0, 1); + + ASSERT_EQ(checker.someCheckFailed(), false); + + + // takeoff item needs to be first item + checker.reset(); + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 100); + + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + checker.processNextItem(mission_item, 0, 2); + + mission_item.nav_cmd = NAV_CMD_TAKEOFF; + + checker.processNextItem(mission_item, 1, 2); + + ASSERT_EQ(checker.someCheckFailed(), true); +} + +TEST_F(FeasibilityCheckerTest, fixed_wing_land_approach) +{ + TestFeasibilityChecker checker; + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 100); + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + + mission_item_s mission_item = {}; + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + mission_item.altitude = 50; + mission_item.altitude_is_relative = true; + + checker.processNextItem(mission_item, 0, 2); + + mission_item.nav_cmd = NAV_CMD_LAND; + mission_item.altitude = 60; + checker.processNextItem(mission_item, 1, 2); + + ASSERT_EQ(checker.someCheckFailed(), true); + + + // landing point should not be within loiter radius of previous waypoint + checker.reset(); + checker.publishLanded(true); + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + checker.publishHomePosition(0, 0, 100); + mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + mission_item.altitude = 50; + mission_item.loiter_radius = 100; + + checker.processNextItem(mission_item, 0, 2); + + double lat_new, lon_new; + waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 99, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + mission_item.nav_cmd = NAV_CMD_LAND; + mission_item.altitude = 40; + + checker.processNextItem(mission_item, 1, 2); + + ASSERT_EQ(checker.someCheckFailed(), true); + + // only loiter to alt or plain waypoint type are allowed before landing + checker.reset(); + checker.publishLanded(true); + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + checker.publishHomePosition(0, 0, 100); + mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + mission_item.altitude = 50; + mission_item.loiter_radius = 50; + + checker.processNextItem(mission_item, 0, 2); + + waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 99, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + mission_item.nav_cmd = NAV_CMD_LAND; + mission_item.altitude = 40; + + checker.processNextItem(mission_item, 1, 2); + + ASSERT_EQ(checker.someCheckFailed(), true); + + // fail the glide slope + checker.reset(); + checker.publishLanded(true); + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + checker.publishHomePosition(0, 0, 100); + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + mission_item.altitude = 50; + mission_item.loiter_radius = 50; + + checker.processNextItem(mission_item, 0, 2); + + waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + mission_item.nav_cmd = NAV_CMD_LAND; + mission_item.altitude = 40; + + checker.processNextItem(mission_item, 1, 2); + + ASSERT_EQ(checker.someCheckFailed(), true); + + // fixed wing land approach checks should not be executed for rotary wing + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + checker.reset(); + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 100); + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + mission_item.altitude = 50; + mission_item.loiter_radius = 50; + + checker.processNextItem(mission_item, 0, 2); + + waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + mission_item.nav_cmd = NAV_CMD_LAND; + mission_item.altitude = 40; + + checker.processNextItem(mission_item, 1, 2); + + ASSERT_EQ(checker.someCheckFailed(), false); +} + +TEST_F(FeasibilityCheckerTest, fixed_wing_landing) +{ + // multiple do land start are not allowed + TestFeasibilityChecker checker; + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 100); + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + + mission_item_s mission_item = {}; + mission_item.nav_cmd = NAV_CMD_DO_LAND_START; + + checker.processNextItem(mission_item, 0, 2); + checker.processNextItem(mission_item, 1, 2); + + ASSERT_EQ(checker.someCheckFailed(), true); + + + // cannot start with land waypoint + checker.reset(); + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + mission_item.nav_cmd = NAV_CMD_LAND; + checker.processNextItem(mission_item, 0, 2); + ASSERT_EQ(checker.someCheckFailed(), true); + + // cannot have land start before RTL + checker.reset(); + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + mission_item.nav_cmd = NAV_CMD_DO_LAND_START; + checker.processNextItem(mission_item, 0, 2); + + mission_item.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; + checker.processNextItem(mission_item, 1, 2); + ASSERT_EQ(checker.someCheckFailed(), true); + + // cannot have land start after landing waypoint + checker.reset(); + checker.publishHomePosition(0, 0, 100); + checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + mission_item.altitude = 10; + mission_item.altitude_is_relative = true; + checker.processNextItem(mission_item, 0, 3); + + mission_item.nav_cmd = NAV_CMD_LAND; + mission_item.altitude = 0; + double lat_new, lon_new; + waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 200, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + checker.processNextItem(mission_item, 1, 3); + + mission_item.nav_cmd = NAV_CMD_DO_LAND_START; + checker.processNextItem(mission_item, 2, 3); + ASSERT_EQ(checker.someCheckFailed(), true); +} + +TEST_F(FeasibilityCheckerTest, check_takeoff_land_requirements) +{ + + TestFeasibilityChecker checker; + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 0); + + param_t param = param_handle(px4::params::MIS_TKO_LAND_REQ); + + // not takeoff land requiremntes, check should pass + int param_val = 0; + param_set(param, ¶m_val); + checker.paramsChanged(); + + mission_item_s mission_item = {}; + mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), false); + + // require takeoff, check should fail + param_val = 1; + param_set(param, ¶m_val); + checker.paramsChanged(); + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), true); + + // require landing, check should fail + param_val = 2; + param_set(param, ¶m_val); + checker.paramsChanged(); + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), true); + + // require both takeoff and landing, check should fail + param_val = 3; + param_set(param, ¶m_val); + checker.paramsChanged(); + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), true); + + // require takeoff and land or none, this should pass as we don't have either + param_val = 4; + param_set(param, ¶m_val); + checker.paramsChanged(); + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), false); +}