From 4952ee1f1b126082263265c5071f5f6c48b7573c Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 16 Feb 2023 13:28:49 +0300 Subject: [PATCH] Revert "added unit tests" This reverts commit 925ad97ff3aec3b22aa1bdd5f834957ac0fc264c. --- .../MissionFeasibility/CMakeLists.txt | 4 +- .../MissionFeasibility/FeasibilityChecker.cpp | 31 +- .../MissionFeasibility/FeasibilityChecker.hpp | 1 - .../FeasibilityCheckerTest.cpp | 410 +----------------- 4 files changed, 16 insertions(+), 430 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/CMakeLists.txt b/src/modules/navigator/MissionFeasibility/CMakeLists.txt index 72ee524a3d..bebf2fa5df 100644 --- a/src/modules/navigator/MissionFeasibility/CMakeLists.txt +++ b/src/modules/navigator/MissionFeasibility/CMakeLists.txt @@ -2,6 +2,4 @@ px4_add_library(mission_feasibility_checker FeasibilityChecker.cpp ) -target_link_libraries(mission_feasibility_checker PUBLIC modules__navigator modules__dataman) - -px4_add_functional_gtest(SRC FeasibilityCheckerTest.cpp LINKLIBS mission_feasibility_checker) +px4_add_functional_gtest(SRC FeasibilityCheckerTest.cpp LINKLIBS mission_feasibility_checker test_stubs) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index cef803fff2..86e5a56533 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -46,13 +46,6 @@ 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; @@ -82,8 +75,7 @@ void FeasibilityChecker::updateData() { home_position_s home = {}; - if (_home_pos_sub.updated()) { - _home_pos_sub.update(&home); + if (_home_pos_sub.copy(&home)) { if (home.valid_hpos) { _home_lat_lon = matrix::Vector2d(home.lat, home.lon); } @@ -95,16 +87,14 @@ void FeasibilityChecker::updateData() vehicle_status_s status = {}; - if (_status_sub.updated()) { - _status_sub.copy(&status); + if (_status_sub.copy(&status)) { _vehicle_type = status.vehicle_type; _is_vtol = status.is_vtol; } vehicle_land_detected_s land_detected = {}; - if (_land_detector_sub.updated()) { - _land_detector_sub.copy(&land_detected); + if (_land_detector_sub.copy(&land_detected)) { _is_landed = land_detected.landed; } @@ -129,7 +119,7 @@ void FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int doMulticopterChecks(mission_item, current_index); } - if (current_index == total_count - 1) { + if (current_index == total_count) { _takeoff_land_available_failed = !checkTakeoffLandAvailable(); } @@ -172,7 +162,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); } @@ -441,6 +431,12 @@ 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; } } @@ -449,6 +445,7 @@ 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) { @@ -463,8 +460,6 @@ 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) { @@ -568,7 +563,7 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item) { - if (_param_mis_dist_1wp.get() <= 0.0f || !_home_lat_lon.isAllFinite()) { + if (_param_mis_dist_1wp.get() <= 0.0f) { /* 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 aae5ea301c..5ae2bf0ce4 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -60,7 +60,6 @@ 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 115887add9..727581ca2e 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -33,12 +33,9 @@ #include #include "FeasibilityChecker.hpp" -#include -#include -#include - // to run: make tests TESTFILTER=FeasibilityChecker + class FeasibilityCheckerTest : public ::testing::Test { public: @@ -54,410 +51,7 @@ 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, 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); -} +TEST_F(FeasibilityCheckerTest, instantiation) { FeasibilityChecker cp(); }