ardupilot/libraries/AC_Avoidance/AP_OAPathPlanner.h

127 lines
4.7 KiB
C
Raw Normal View History

AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_Param/AP_Param.h>
2022-02-28 21:19:06 -04:00
#include <AP_HAL/Semaphores.h>
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
#include "AP_OABendyRuler.h"
#include "AP_OADijkstra.h"
#include "AP_OADatabase.h"
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
/*
* This class provides path planning around fence, stay-out zones and moving obstacles
*/
class AP_OAPathPlanner {
public:
AP_OAPathPlanner();
/* Do not allow copies */
2022-09-30 06:50:43 -03:00
CLASS_NO_COPY(AP_OAPathPlanner);
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
// get singleton instance
static AP_OAPathPlanner *get_singleton() {
return _singleton;
}
// perform any required initialisation
void init();
/// returns true if all pre-takeoff checks have completed successfully
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
// object avoidance processing return status enum
enum OA_RetState : uint8_t {
OA_NOT_REQUIRED = 0, // object avoidance is not required
OA_PROCESSING, // still calculating alternative path
OA_ERROR, // error during calculation
OA_SUCCESS // success
};
// path planner responsible for a particular result
enum OAPathPlannerUsed : uint8_t {
None = 0,
BendyRulerHorizontal,
BendyRulerVertical,
Dijkstras
};
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
// 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 &current_loc,
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
const Location &origin,
const Location &destination,
Location &result_origin,
Location &result_destination,
OAPathPlannerUsed &path_planner_used) WARN_IF_UNUSED;
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
// enumerations for _TYPE parameter
enum OAPathPlanTypes {
OA_PATHPLAN_DISABLED = 0,
OA_PATHPLAN_BENDYRULER = 1,
OA_PATHPLAN_DIJKSTRA = 2,
OA_PATHPLAN_DJIKSTRA_BENDYRULER = 3,
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
};
// enumeration for _OPTION parameter
enum OARecoveryOptions {
OA_OPTION_DISABLED = 0,
OA_OPTION_WP_RESET = (1 << 0),
OA_OPTION_LOG_DIJKSTRA_POINTS = (1 << 1),
};
uint16_t get_options() const { return _options;}
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
static const struct AP_Param::GroupInfo var_info[];
private:
// avoidance thread that continually updates the avoidance_result structure based on avoidance_request
void avoidance_thread();
bool start_thread();
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
// helper function to map OABendyType to OAPathPlannerUsed
OAPathPlannerUsed map_bendytype_to_pathplannerused(AP_OABendyRuler::OABendyType bendy_type);
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
// an avoidance request from the navigation code
struct avoidance_info {
Location current_loc;
Location origin;
Location destination;
Vector2f ground_speed_vec;
uint32_t request_time_ms;
} avoidance_request, avoidance_request2;
// an avoidance result from the avoidance thread
struct {
Location destination; // destination vehicle is trying to get to (also used to verify the result matches a recent request)
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
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
} avoidance_result;
// parameters
AP_Int8 _type; // avoidance algorithm to be used
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
AP_Float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle
AP_Int16 _options; // Bitmask for options while recovering from Object Avoidance
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
// internal variables used by front end
HAL_Semaphore _rsem; // semaphore for multi-thread use of avoidance_request and avoidance_result
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
bool _thread_created; // true once background thread has been created
AP_OABendyRuler *_oabendyruler; // Bendy Ruler algorithm
AP_OADijkstra *_oadijkstra; // Dijkstra's algorithm
AP_OADatabase _oadatabase; // Database of dynamic objects to avoid
uint32_t avoidance_latest_ms; // last time Dijkstra's or BendyRuler algorithms ran
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
bool proximity_only = true;
AP_OAPathPlanner: path planning around obstacles implements both bendy ruler and dijkstra's algorithms includes many changes after peer review including: removed unused airspeed and turn-rate arguments renamed constants to start with OA_BENDYRULER_ removed fence_ptr check from update function that could cause bendy ruler to fail when fences were disabled AC_Fence's fence-margin is not used and instead we always use a hard-coded 2m limit vehicle's heading is used instead of ground-course when speed is below 0.2m/s static assert added to ensure OA_BENDYRULER_BERAING_INC constant is defined correctly replace BAD_MARGIN/GOOD_MARGIN with FLT_MAX/-FLT_MAX use calc_margin_from_xxx function's return values calc-margin-from-proximity-sensors fails if copy-locations fails remove debug remove unused singletons and other changes including: integrate fence rename to get_boundary_points adapt to fence return point and duplicate final point from not being returned create bendyrule and dijkstra objects on init add pre-arm-check to ensure they've been created successfully Dijkstra's detects polygon fence changes minor fix to Dijkstra's return path allow update_visgraph to accept 0,0 as extra point and these fixes from peer reviews: remove unused methods add note about dangers of operator[] reduce memory consuption of ShortPathNode remove unused methods including print_all_nodes and print_shortest_path replace OA_DIJKSTRA_POLYGON_VISGRAPH_INFINITY_CM with FLT_MAX update method gets check that ekf origin has been set fix indexing bug in inner fence creation fix whitespace error find-node-from-id uses switch statement instead of 3 IF statements update_visgraph comments added to clarify usage of extra_position argument find-closest-node-idx always uses node_index instead of int16_t static assert that OA_DIJKSTRA_POLYGON_FENCE_PTS < 255 some looping style changes AP_OABendyRuler: fix to ground course calculation AP_OAPathPlanner: pre_arm_check writes to caller's buffer remove unnecessary hal and stdio.h includes rename fence_ptr local variable to fence add sanity check to OADijkstra's set_fence_margin add sanity check to OABendyRuler's set_config BendyRuler's test_bearing local constants changed to be float constants BendyRuler's ahrs_home made local constant BendyRuler's proximity margin calcs lose margin_found local variable (use FLT_MAX instead) BendyRuler's calc-margin-from-circular-fence performance improvement by removing one sqrt AP_OABendyRuler: fix probe directions to be 5deg offsets from dir to destination AP_OADijkstra: fixes after peer review remove unnecesary initialisations replace safe_sqrt(sq()) with length() remove unnecessary block comment fixes AP_OAPathPlanner: add logging of final an intermediate location
2019-05-10 02:59:31 -03:00
static AP_OAPathPlanner *_singleton;
};
namespace AP {
AP_OAPathPlanner *ap_oapathplanner();
};