ardupilot/libraries/AC_Avoidance/AP_OAPathPlanner.h

137 lines
5.8 KiB
C++

#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_Param/AP_Param.h>
#include <AP_HAL/Semaphores.h>
#include "AP_OABendyRuler.h"
#include "AP_OADijkstra.h"
#include "AP_OADatabase.h"
/*
* This class provides path planning around fence, stay-out zones and moving obstacles
*/
class AP_OAPathPlanner {
public:
AP_OAPathPlanner();
/* Do not allow copies */
CLASS_NO_COPY(AP_OAPathPlanner);
// 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
};
// provides an alternative target location if path planning around obstacles is required
// returns true and updates result_origin, result_destination and result_next_destination with an intermediate path
// result_dest_to_next_dest_clear is set to true if the path from result_destination to result_next_destination is clear (only supported by Dijkstras)
// path_planner_used updated with which path planner produced the result
OA_RetState mission_avoidance(const Location &current_loc,
const Location &origin,
const Location &destination,
const Location &next_destination,
Location &result_origin,
Location &result_destination,
Location &result_next_destination,
bool &result_dest_to_next_dest_clear,
OAPathPlannerUsed &path_planner_used) WARN_IF_UNUSED;
// enumerations for _TYPE parameter
enum OAPathPlanTypes {
OA_PATHPLAN_DISABLED = 0,
OA_PATHPLAN_BENDYRULER = 1,
OA_PATHPLAN_DIJKSTRA = 2,
OA_PATHPLAN_DJIKSTRA_BENDYRULER = 3,
};
// 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;}
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();
// 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;
Location origin;
Location destination;
Location next_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 next_destination; // next 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
Location next_destination_new; // intermediate next destination vehicle should move towards
bool dest_to_next_dest_clear; // true if the path from destination_new to next_destination_new is clear and does not require path planning (only supported by Dijkstras)
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;
// parameters
AP_Int8 _type; // avoidance algorithm to be used
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
// internal variables used by front end
HAL_Semaphore _rsem; // semaphore for multi-thread use of avoidance_request and avoidance_result
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 (in the avoidance thread)
uint32_t _last_update_ms; // system time that mission_avoidance was called in main thread
uint32_t _activated_ms; // system time that object avoidance was most recently activated (used to avoid timeout error on first run)
bool proximity_only = true;
static AP_OAPathPlanner *_singleton;
};
namespace AP {
AP_OAPathPlanner *ap_oapathplanner();
};