Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: move OA path planning handling into a separate class

This commit is contained in:
Randy Mackay 2022-01-05 13:35:34 +09:00
parent 2a1013e896
commit 6515e71fc1
4 changed files with 297 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -145,65 +145,13 @@ void AR_WPNav::update(float dt)
}
_last_update_ms = AP_HAL::millis();
// run path planning around obstacles
bool stop_vehicle = false;
// true if OA has been recently active;
bool _oa_was_active = _oa_active;
AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton();
if (oa != nullptr) {
AP_OAPathPlanner::OAPathPlannerUsed path_planner_used;
const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin, _destination, _oa_origin, _oa_destination, path_planner_used);
switch (oa_retstate) {
case AP_OAPathPlanner::OA_NOT_REQUIRED:
_oa_active = false;
break;
case AP_OAPathPlanner::OA_PROCESSING:
case AP_OAPathPlanner::OA_ERROR:
// during processing or in case of error, slow vehicle to a stop
stop_vehicle = true;
_oa_active = false;
break;
case AP_OAPathPlanner::OA_SUCCESS:
_oa_active = true;
break;
}
}
if (!_oa_active) {
_oa_origin = _origin;
_oa_destination = _destination;
}
update_distance_and_bearing_to_destination();
// if object avoidance is active check if vehicle should pivot towards destination
if (_oa_active) {
_pivot.check_activation(_oa_wp_bearing_cd * 0.01);
}
// check if vehicle is in recovering state after switching off OA
if (!_oa_active && _oa_was_active) {
if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET) {
//reset wp origin to vehicles current location
_origin = current_loc;
}
}
// advance target along path unless vehicle is pivoting
if (!_pivot.active()) {
advance_wp_target_along_track(current_loc, dt);
}
// handle stopping vehicle if avoidance has failed
if (stop_vehicle) {
// decelerate to speed to zero and set turn rate to zero
_desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
_desired_lat_accel = 0.0f;
_desired_turn_rate_rads = 0.0f;
return;
}
// update_steering_and_speed
update_steering_and_speed(current_loc, dt);
}
@ -221,8 +169,8 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
_scurve_prev_leg = _scurve_this_leg;
// initialise some variables
_oa_origin = _origin = _destination;
_oa_destination = _destination = destination;
_origin = _destination;
_destination = destination;
_orig_and_dest_valid = true;
_reached_destination = false;
@ -232,7 +180,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
// or journey to previous waypoint was interrupted or navigation has just started
if (!_fast_waypoint) {
_pivot.deactivate();
_pivot.check_activation(_oa_wp_bearing_cd * 0.01);
_pivot.check_activation(oa_wp_bearing_cd() * 0.01);
}
// convert origin and destination to offset from EKF origin
@ -434,23 +382,10 @@ void AR_WPNav::update_distance_and_bearing_to_destination()
if (!_orig_and_dest_valid || !AP::ahrs().get_location(current_loc)) {
_distance_to_destination = 0.0f;
_wp_bearing_cd = 0.0f;
// update OA adjusted values
_oa_distance_to_destination = 0.0f;
_oa_wp_bearing_cd = 0.0f;
return;
}
_distance_to_destination = current_loc.get_distance(_destination);
_wp_bearing_cd = current_loc.get_bearing_to(_destination);
// update OA adjusted values
if (_oa_active) {
_oa_distance_to_destination = current_loc.get_distance(_oa_destination);
_oa_wp_bearing_cd = current_loc.get_bearing_to(_oa_destination);
} else {
_oa_distance_to_destination = _distance_to_destination;
_oa_wp_bearing_cd = _wp_bearing_cd;
}
}
// calculate steering and speed to drive along line from origin to destination waypoint
@ -459,7 +394,7 @@ void AR_WPNav::update_steering_and_speed(const Location &current_loc, float dt)
// handle pivot turns
if (_pivot.active()) {
_cross_track_error = calc_crosstrack_error(current_loc);
_desired_heading_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;;
_desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd();
_desired_turn_rate_rads = _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt);
_desired_lat_accel = 0.0f;
@ -495,26 +430,30 @@ void AR_WPNav::apply_speed_min(float &desired_speed) const
desired_speed = MAX(desired_speed, _speed_min);
}
// calculate the crosstrack error (does not rely on L1 controller)
// calculate the crosstrack error
float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const
{
if (!_orig_and_dest_valid) {
return 0.0f;
}
// calculate the NE position of destination relative to origin
Vector2f dest_from_origin = _oa_origin.get_distance_NE(_oa_destination);
// get object avoidance adjusted origin and destination
const Location &orig = get_oa_origin();
const Location &dest = get_oa_destination();
// return distance to origin if length of track is very small
// calculate the NE position of destination relative to origin
Vector2f dest_from_origin = orig.get_distance_NE(dest);
// return distance to destination if length of track is very small
if (dest_from_origin.length() < 1.0e-6f) {
return current_loc.get_distance_NE(_oa_destination).length();
return current_loc.get_distance_NE(dest).length();
}
// convert to a vector indicating direction only
dest_from_origin.normalize();
// calculate the NE position of the vehicle relative to origin
const Vector2f veh_from_origin = _oa_origin.get_distance_NE(current_loc);
const Vector2f veh_from_origin = orig.get_distance_NE(current_loc);
// calculate distance to target track, for reporting
return fabsf(veh_from_origin % dest_from_origin);

30
libraries/AR_WPNav/AR_WPNav.h
View File

@ -7,8 +7,6 @@
#include <AC_Avoidance/AP_OAPathPlanner.h>
#include "AR_PivotTurn.h"
const float AR_WPNAV_HEADING_UNKNOWN = 99999.0f; // used to indicate to set_desired_location method that next leg's heading is unknown
class AR_WPNav {
public:
@ -23,7 +21,7 @@ public:
void init(float speed_max, float accel_max, float lat_accel_max, float jerk_max);
// update navigation
void update(float dt);
virtual void update(float dt);
// return desired speed
float get_desired_speed() const { return _desired_speed; }
@ -47,7 +45,7 @@ public:
// set desired location and (optionally) next_destination
// next_destination should be provided if known to allow smooth cornering
bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED;
virtual bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED;
// set desired location to a reasonable stopping point, return true on success
bool set_desired_location_to_stopping_location() WARN_IF_UNUSED;
@ -57,7 +55,7 @@ public:
bool set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination) WARN_IF_UNUSED;
// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
bool reached_destination() const { return _reached_destination; }
virtual bool reached_destination() const { return _reached_destination; }
// return distance (in meters) to destination
float get_distance_to_destination() const { return _distance_to_destination; }
@ -66,18 +64,21 @@ public:
bool is_destination_valid() const { return _orig_and_dest_valid; }
// get current destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
const Location &get_destination() { return _destination; }
// get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
const Location &get_oa_destination() { return _oa_destination; }
const Location &get_destination() const { return _destination; }
// return heading (in centi-degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing_cd() const { return _wp_bearing_cd; }
float nav_bearing_cd() const { return _desired_heading_cd; }
float crosstrack_error() const { return _cross_track_error; }
// get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
virtual const Location &get_oa_origin() const { return _origin; }
// get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
virtual const Location &get_oa_destination() const { return get_destination(); }
// return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats)
float oa_wp_bearing_cd() const { return _oa_wp_bearing_cd; }
virtual float oa_wp_bearing_cd() const { return wp_bearing_cd(); }
// settor to allow vehicle code to provide turn related param values to this library (should be updated regularly)
void set_turn_params(float turn_radius, bool pivot_possible);
@ -94,7 +95,7 @@ public:
// parameter var table
static const struct AP_Param::GroupInfo var_info[];
private:
protected:
// true if update has been called recently
bool is_active() const;
@ -155,11 +156,4 @@ private:
// variables for reporting
float _distance_to_destination; // distance from vehicle to final destination in meters
bool _reached_destination; // true once the vehicle has reached the destination
// object avoidance variables
bool _oa_active; // true if we should use alternative destination to avoid obstacles
Location _oa_origin; // intermediate origin during avoidance
Location _oa_destination; // intermediate destination during avoidance
float _oa_distance_to_destination; // OA (object avoidance) distance from vehicle to _oa_destination in meters
float _oa_wp_bearing_cd; // OA adjusted heading to _oa_destination in centi-degrees
};

228
libraries/AR_WPNav/AR_WPNav_OA.cpp Normal file
View File

@ -0,0 +1,228 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include "AR_WPNav_OA.h"
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
// update navigation
void AR_WPNav_OA::update(float dt)
{
// exit immediately if no current location, origin or destination
Location current_loc;
float speed;
if (!hal.util->get_soft_armed() || !is_destination_valid() || !AP::ahrs().get_location(current_loc) || !_atc.get_forward_speed(speed)) {
_desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
_desired_lat_accel = 0.0f;
_desired_turn_rate_rads = 0.0f;
_oa_active = false;
return;
}
// run path planning around obstacles
bool stop_vehicle = false;
// backup _origin and _destination when not doing oa
if (!_oa_active) {
_origin_oabak = _origin;
_destination_oabak = _destination;
}
AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton();
if (oa != nullptr) {
Location oa_origin_new, oa_destination_new;
AP_OAPathPlanner::OAPathPlannerUsed path_planner_used;
const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin_oabak, _destination_oabak, oa_origin_new, oa_destination_new, path_planner_used);
switch (oa_retstate) {
case AP_OAPathPlanner::OA_NOT_REQUIRED:
if (_oa_active) {
// object avoidance has become inactive so reset target to original destination
if (!AR_WPNav::set_desired_location(_destination_oabak)) {
// this should never happen because we should have an EKF origin and the destination must be valid
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
stop_vehicle = true;
}
_oa_active = false;
// ToDo: handle "if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET)"
}
break;
case AP_OAPathPlanner::OA_PROCESSING:
case AP_OAPathPlanner::OA_ERROR:
// during processing or in case of error, slow vehicle to a stop
stop_vehicle = true;
_oa_active = false;
break;
case AP_OAPathPlanner::OA_SUCCESS:
// handling of returned destination depends upon path planner used
switch (path_planner_used) {
case AP_OAPathPlanner::OAPathPlannerUsed::None:
case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerVertical:
// this should never happen. this means the path planner has returned success but has returned an invalid planner
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
_oa_active = false;
stop_vehicle = true;
return;
case AP_OAPathPlanner::OAPathPlannerUsed::Dijkstras:
// Dijkstra's. Action is only needed if path planner has just became active or the target destination's lat or lon has changed
if (!_oa_active || !oa_destination_new.same_latlon_as(_oa_destination)) {
if (AR_WPNav::set_desired_location(oa_destination_new)) {
// if new target set successfully, update oa state and destination
_oa_active = true;
_oa_origin = oa_origin_new;
_oa_destination = oa_destination_new;
} else {
// this should never happen
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
stop_vehicle = true;
}
}
break;
case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerHorizontal: {
// calculate final destination as an offset from EKF origin in NE
Vector2f dest_NE_ftype;
if (oa_destination_new.get_vector_xy_from_origin_NE(dest_NE_ftype)) {
// initialise position controller if required
if (!_oa_active) {
_pos_control.init();
_oa_active = true;
}
_oa_origin = oa_origin_new;
_oa_destination = oa_destination_new;
// check for pivot turn
update_oa_distance_and_bearing_to_destination();
_pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01);
if (!_pivot.active()) {
// pass the desired position directly to the position controller
dest_NE_ftype *= 0.01; // convert to meters
Vector2p dest_NE(dest_NE_ftype.x, dest_NE_ftype.y);
_pos_control.input_pos_target(dest_NE, dt);
// run position controllers
_pos_control.update(dt);
}
update_distance_and_bearing_to_destination();
// update_steering_and_speed
update_steering_and_speed(current_loc, dt);
// return without calling parent AC_WPNav
return;
} else {
// this should never happen because we can only get here if we have an EKF origin
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
stop_vehicle = true;
}
}
break;
} // switch (path_planner_used) {
} // switch (oa_retstate) {
} // if (oa != nullptr) {
update_oa_distance_and_bearing_to_destination();
// handle stopping vehicle if avoidance has failed
if (stop_vehicle) {
// decelerate to speed to zero and set turn rate to zero
_desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
_desired_lat_accel = 0.0f;
_desired_turn_rate_rads = 0.0f;
return;
}
// call parent update
AR_WPNav::update(dt);
}
// set desired location and (optionally) next_destination
// next_destination should be provided if known to allow smooth cornering
bool AR_WPNav_OA::set_desired_location(const struct Location& destination, Location next_destination)
{
const bool ret = AR_WPNav::set_desired_location(destination, next_destination);
if (ret) {
// disable object avoidance, it will be re-enabled (if necessary) on next update
_oa_active = false;
}
return ret;
}
// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
bool AR_WPNav_OA::reached_destination() const
{
// object avoidance should always be deactivated before reaching final destination
if (_oa_active) {
return false;
}
return AR_WPNav::reached_destination();
}
// get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
const Location &AR_WPNav_OA::get_oa_origin() const
{
if (_oa_active) {
return _oa_origin;
}
return _origin;
}
// get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
const Location &AR_WPNav_OA::get_oa_destination() const
{
if (_oa_active) {
return _oa_destination;
}
return AR_WPNav::get_oa_destination();
}
// return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats)
float AR_WPNav_OA::oa_wp_bearing_cd() const
{
if (_oa_active) {
return _oa_wp_bearing_cd;
}
return AR_WPNav::oa_wp_bearing_cd();
}
// update distance from vehicle's current position to destination
void AR_WPNav_OA::update_oa_distance_and_bearing_to_destination()
{
// update OA adjusted values
Location current_loc;
if (_oa_active && AP::ahrs().get_location(current_loc)) {
_oa_distance_to_destination = current_loc.get_distance(_oa_destination);
_oa_wp_bearing_cd = current_loc.get_bearing_to(_oa_destination);
} else {
_oa_distance_to_destination = AR_WPNav::get_distance_to_destination();
_oa_wp_bearing_cd = AR_WPNav::wp_bearing_cd();
}
}

43
libraries/AR_WPNav/AR_WPNav_OA.h Normal file
View File

@ -0,0 +1,43 @@
#pragma once
#include "AR_WPNav.h"
class AR_WPNav_OA : public AR_WPNav {
public:
// re-use parent's constructor
using AR_WPNav::AR_WPNav;
// update navigation
void update(float dt) override;
// set desired location and (optionally) next_destination
// next_destination should be provided if known to allow smooth cornering
bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED;
// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
bool reached_destination() const override;
// get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
const Location &get_oa_origin() const override;
// get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
const Location &get_oa_destination() const override;
// return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats)
float oa_wp_bearing_cd() const override;
private:
// update distance and bearing from vehicle's current position to destination
void update_oa_distance_and_bearing_to_destination();
// object avoidance variables
bool _oa_active; // true if we should use alternative destination to avoid obstacles
Location _origin_oabak; // backup of _origin so it can be restored when oa completes
Location _destination_oabak; // backup of _desitnation so it can be restored when oa completes
Location _oa_origin; // intermediate origin during avoidance
Location _oa_destination; // intermediate destination during avoidance
float _oa_distance_to_destination; // OA (object avoidance) distance from vehicle to _oa_destination in meters
float _oa_wp_bearing_cd; // OA adjusted heading to _oa_destination in centi-degrees
};