From 8d641e1bb3379debecc09f4602984f7856562b11 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 24 Mar 2021 11:33:04 +0900 Subject: [PATCH] AC_Avoidance: OA_PathPlanner returns which planner was used this replaces get_bendy_type --- libraries/AC_Avoidance/AP_OAPathPlanner.cpp | 47 +++++++++++++++------ libraries/AC_Avoidance/AP_OAPathPlanner.h | 19 +++++++-- 2 files changed, 48 insertions(+), 18 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 70fe37e3c0..836471c447 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp @@ -111,15 +111,6 @@ void AP_OAPathPlanner::init() start_thread(); } -// return type of BendyRuler in use -AP_OABendyRuler::OABendyType AP_OAPathPlanner::get_bendy_type() const -{ - if (_oabendyruler == nullptr) { - return AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED; - } - return _oabendyruler->get_type(); -} - // pre-arm checks that algorithms have been initialised successfully bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { @@ -173,13 +164,30 @@ bool AP_OAPathPlanner::start_thread() return true; } +// helper function to map OABendyType to OAPathPlannerUsed +AP_OAPathPlanner::OAPathPlannerUsed AP_OAPathPlanner::map_bendytype_to_pathplannerused(AP_OABendyRuler::OABendyType bendy_type) +{ + switch (bendy_type) { + case AP_OABendyRuler::OABendyType::OA_BENDY_HORIZONTAL: + return OAPathPlannerUsed::BendyRulerHorizontal; + + case AP_OABendyRuler::OABendyType::OA_BENDY_VERTICAL: + return OAPathPlannerUsed::BendyRulerVertical; + + default: + case AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED: + return OAPathPlannerUsed::None; + } +} + // provides an alternative target location if path planning around obstacles is required // returns true and updates result_loc with an intermediate location AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location ¤t_loc, const Location &origin, const Location &destination, Location &result_origin, - Location &result_destination) + Location &result_destination, + OAPathPlannerUsed &path_planner_used) { // exit immediately if disabled or thread is not running from a failed init if (_type == OA_PATHPLAN_DISABLED || !_thread_created) { @@ -207,6 +215,7 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location // we have a result from the thread result_origin = avoidance_result.origin_new; result_destination = avoidance_result.destination_new; + path_planner_used = avoidance_result.path_planner_used; return avoidance_result.ret_state; } @@ -269,18 +278,23 @@ void AP_OAPathPlanner::avoidance_thread() // run background task looking for best alternative destination OA_RetState res = OA_NOT_REQUIRED; + OAPathPlannerUsed path_planner_used = OAPathPlannerUsed::None; switch (_type) { case OA_PATHPLAN_DISABLED: continue; - case OA_PATHPLAN_BENDYRULER: + case OA_PATHPLAN_BENDYRULER: { if (_oabendyruler == nullptr) { continue; } _oabendyruler->set_config(_margin_max); - if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, false)) { + + AP_OABendyRuler::OABendyType bendy_type; + if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, bendy_type, false)) { res = OA_SUCCESS; } + path_planner_used = map_bendytype_to_pathplannerused(bendy_type); break; + } case OA_PATHPLAN_DIJKSTRA: { if (_oadijkstra == nullptr) { @@ -299,6 +313,7 @@ void AP_OAPathPlanner::avoidance_thread() res = OA_SUCCESS; break; } + path_planner_used = OAPathPlannerUsed::Dijkstras; break; } @@ -307,10 +322,12 @@ void AP_OAPathPlanner::avoidance_thread() continue; } _oabendyruler->set_config(_margin_max); - if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, proximity_only)) { + AP_OABendyRuler::OABendyType bendy_type; + if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new, bendy_type, proximity_only)) { // detected a obstacle by vehicle's proximity sensor. Switch avoidance to BendyRuler till obstacle is out of the way proximity_only = false; res = OA_SUCCESS; + path_planner_used = map_bendytype_to_pathplannerused(bendy_type); break; } else { // cleared all obstacles, trigger Dijkstra's to calculate path based on current deviated position @@ -333,10 +350,11 @@ void AP_OAPathPlanner::avoidance_thread() res = OA_SUCCESS; break; } + path_planner_used = OAPathPlannerUsed::Dijkstras; break; } - } + } // switch { // give the main thread the avoidance result @@ -345,6 +363,7 @@ void AP_OAPathPlanner::avoidance_thread() avoidance_result.origin_new = (res == OA_SUCCESS) ? origin_new : avoidance_result.origin_new; avoidance_result.destination_new = (res == OA_SUCCESS) ? destination_new : avoidance_result.destination; avoidance_result.result_time_ms = AP_HAL::millis(); + avoidance_result.path_planner_used = path_planner_used; avoidance_result.ret_state = res; } } diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.h b/libraries/AC_Avoidance/AP_OAPathPlanner.h index 50af4587b5..e064712a89 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.h +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.h @@ -39,13 +39,23 @@ public: OA_SUCCESS // success }; + // path planner responsible for a particular result + enum OAPathPlannerUsed : uint8_t { + None = 0, + BendyRulerHorizontal, + BendyRulerVertical, + Dijkstras + }; + // provides an alternative target location if path planning around obstacles is required // returns true and updates result_origin and result_destination with an intermediate path + // path_planner_used updated with which path planner produced the result OA_RetState mission_avoidance(const Location ¤t_loc, const Location &origin, const Location &destination, Location &result_origin, - Location &result_destination) WARN_IF_UNUSED; + Location &result_destination, + OAPathPlannerUsed &path_planner_used) WARN_IF_UNUSED; // enumerations for _TYPE parameter enum OAPathPlanTypes { @@ -63,9 +73,6 @@ public: uint16_t get_options() const { return _options;} - // helper function to return type of BendyRuler in use. This is used by AC_WPNav_OA - AP_OABendyRuler::OABendyType get_bendy_type() const; - static const struct AP_Param::GroupInfo var_info[]; private: @@ -74,6 +81,9 @@ private: void avoidance_thread(); bool start_thread(); + // helper function to map OABendyType to OAPathPlannerUsed + OAPathPlannerUsed map_bendytype_to_pathplannerused(AP_OABendyRuler::OABendyType bendy_type); + // an avoidance request from the navigation code struct avoidance_info { Location current_loc; @@ -89,6 +99,7 @@ private: Location origin_new; // intermediate origin. The start of line segment that vehicle should follow Location destination_new; // intermediate destination vehicle should move towards uint32_t result_time_ms; // system time the result was calculated (used to verify the result is recent) + OAPathPlannerUsed path_planner_used; // path planner that produced the result OA_RetState ret_state; // OA_SUCCESS if the vehicle should move along the path from origin_new to destination_new } avoidance_result;