mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
CollisionPrevention: rewrite for Acceleration based manual flight mode
This commit is contained in:
Claudio Chies
parent
b8c2805263
commit
0cd6a553b9
@ -40,8 +40,6 @@
|
||||
#include "CollisionPrevention.hpp"
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
namespace
|
||||
{
|
||||
@ -113,7 +111,7 @@ bool CollisionPrevention::is_active()
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude)
|
||||
CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude)
|
||||
{
|
||||
int msg_index = 0;
|
||||
float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi());
|
||||
@ -197,6 +195,36 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
CollisionPrevention::_checkSetpointDirectionFeasability()
|
||||
{
|
||||
bool setpoint_feasible = true;
|
||||
|
||||
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
|
||||
// check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data
|
||||
if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_nodata.get()
|
||||
|| (_param_cp_go_nodata.get() && _data_fov[i]))) {
|
||||
setpoint_feasible = false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return setpoint_feasible;
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_transformSetpoint(const Vector2f &setpoint)
|
||||
{
|
||||
const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi();
|
||||
_setpoint_dir = setpoint / setpoint.norm();;
|
||||
const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - vehicle_yaw_angle_rad;
|
||||
const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) -
|
||||
_obstacle_map_body_frame.angle_offset);
|
||||
_setpoint_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
|
||||
// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
|
||||
_adaptSetpointDirection(_setpoint_dir, _setpoint_index, vehicle_yaw_angle_rad);
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_updateObstacleMap()
|
||||
{
|
||||
@ -244,12 +272,49 @@ CollisionPrevention::_updateObstacleMap()
|
||||
_obstacle_distance_pub.publish(_obstacle_map_body_frame);
|
||||
}
|
||||
|
||||
void CollisionPrevention::_updateObstacleData()
|
||||
{
|
||||
_obstacle_data_present = false;
|
||||
_closest_dist = UINT16_MAX;
|
||||
_closest_dist_dir.setZero();
|
||||
const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi();
|
||||
|
||||
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
|
||||
|
||||
// if the data is stale, reset the bin
|
||||
if (getTime() - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) {
|
||||
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
|
||||
}
|
||||
|
||||
float angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG +
|
||||
_obstacle_map_body_frame.angle_offset));
|
||||
const Vector2f bin_direction = {cosf(angle), sinf(angle)};
|
||||
uint bin_distance = _obstacle_map_body_frame.distances[i];
|
||||
|
||||
// check if there is avaliable data and the data of the map is not stale
|
||||
if (bin_distance < UINT16_MAX
|
||||
&& (getTime() - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
|
||||
_obstacle_data_present = true;
|
||||
}
|
||||
|
||||
if (bin_distance * 0.01f < _closest_dist) {
|
||||
_closest_dist = bin_distance * 0.01f;
|
||||
_closest_dist_dir = bin_direction;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude)
|
||||
CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude)
|
||||
{
|
||||
// clamp at maximum sensor range
|
||||
float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance);
|
||||
|
||||
// negative values indicate out of range but valid measurements.
|
||||
if (fabsf(distance_sensor.current_distance - -1.f) < FLT_EPSILON && distance_sensor.signal_quality == 0) {
|
||||
distance_reading = distance_sensor.max_distance;
|
||||
}
|
||||
|
||||
// discard values below min range
|
||||
if ((distance_reading > distance_sensor.min_distance)) {
|
||||
|
||||
@ -268,7 +333,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
|
||||
if (upper_bound < 0) { upper_bound++; }
|
||||
|
||||
// rotate vehicle attitude into the sensor body frame
|
||||
matrix::Quatf attitude_sensor_frame = vehicle_attitude;
|
||||
Quatf attitude_sensor_frame = vehicle_attitude;
|
||||
attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
|
||||
float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
|
||||
|
||||
@ -294,7 +359,6 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
|
||||
void
|
||||
CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
|
||||
{
|
||||
const float col_prev_d = _param_cp_dist.get();
|
||||
const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG);
|
||||
const int sp_index_original = setpoint_index;
|
||||
float best_cost = 9999.f;
|
||||
@ -310,7 +374,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
|
||||
int bin = wrap_bin(j);
|
||||
|
||||
if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
|
||||
mean_dist += col_prev_d * 100.f;
|
||||
mean_dist += _param_cp_dist.get() * 100.f;
|
||||
|
||||
} else {
|
||||
mean_dist += _obstacle_map_body_frame.distances[bin];
|
||||
@ -319,7 +383,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
|
||||
|
||||
const int bin = wrap_bin(i);
|
||||
mean_dist = mean_dist / (2.f * filter_size + 1.f);
|
||||
const float deviation_cost = col_prev_d * 50.f * abs(i - sp_index_original);
|
||||
const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original);
|
||||
const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];
|
||||
|
||||
if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) {
|
||||
@ -376,7 +440,7 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist
|
||||
break;
|
||||
|
||||
case distance_sensor_s::ROTATION_CUSTOM:
|
||||
offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
|
||||
offset = Eulerf(Quatf(distance_sensor.q)).psi();
|
||||
break;
|
||||
}
|
||||
|
||||
@ -384,154 +448,164 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
|
||||
const Vector2f &curr_vel)
|
||||
CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
|
||||
{
|
||||
_updateObstacleMap();
|
||||
_updateObstacleData();
|
||||
|
||||
// read parameters
|
||||
const float col_prev_d = _param_cp_dist.get();
|
||||
const float col_prev_dly = _param_cp_delay.get();
|
||||
const bool move_no_data = _param_cp_go_nodata.get();
|
||||
const float xy_p = _param_mpc_xy_p.get();
|
||||
const float max_jerk = _param_mpc_jerk_max.get();
|
||||
const float max_accel = _param_mpc_acc_hor.get();
|
||||
const matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
|
||||
const Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
|
||||
const float vehicle_yaw_angle_rad = Eulerf(attitude).psi();
|
||||
|
||||
const float setpoint_length = setpoint.norm();
|
||||
const float setpoint_length = setpoint_accel.norm();
|
||||
_min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get());
|
||||
|
||||
const hrt_abstime constrain_time = getTime();
|
||||
int num_fov_bins = 0;
|
||||
const hrt_abstime now = getTime();
|
||||
|
||||
if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
|
||||
if (setpoint_length > 0.001f) {
|
||||
float vel_comp_accel = INFINITY;
|
||||
Vector2f vel_comp_accel_dir{};
|
||||
Vector2f constr_accel_setpoint{};
|
||||
|
||||
Vector2f setpoint_dir = setpoint / setpoint_length;
|
||||
float vel_max = setpoint_length;
|
||||
const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d);
|
||||
const bool is_stick_deflected = setpoint_length > 0.001f;
|
||||
|
||||
const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
|
||||
const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) -
|
||||
_obstacle_map_body_frame.angle_offset);
|
||||
int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
|
||||
if (_obstacle_data_present && is_stick_deflected) {
|
||||
|
||||
// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
|
||||
_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
|
||||
_transformSetpoint(setpoint_accel);
|
||||
|
||||
// limit speed for safe flight
|
||||
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message
|
||||
_getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now,
|
||||
vel_comp_accel, vel_comp_accel_dir);
|
||||
|
||||
// delete stale values
|
||||
const hrt_abstime data_age = constrain_time - _data_timestamps[i];
|
||||
|
||||
if (data_age > RANGE_STREAM_TIMEOUT_US) {
|
||||
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
|
||||
}
|
||||
|
||||
const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters
|
||||
const float max_range = _data_maxranges[i] * 0.01f; // convert to meters
|
||||
float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
|
||||
|
||||
// convert from body to local frame in the range [0, 2*pi]
|
||||
angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
|
||||
|
||||
// get direction of current bin
|
||||
const Vector2f bin_direction = {cosf(angle), sinf(angle)};
|
||||
|
||||
//count number of bins in the field of valid_new
|
||||
if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) {
|
||||
num_fov_bins ++;
|
||||
}
|
||||
|
||||
if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance
|
||||
&& _obstacle_map_body_frame.distances[i] < UINT16_MAX) {
|
||||
|
||||
if (setpoint_dir.dot(bin_direction) > 0) {
|
||||
// calculate max allowed velocity with a P-controller (same gain as in the position controller)
|
||||
const float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
|
||||
float delay_distance = curr_vel_parallel * col_prev_dly;
|
||||
|
||||
if (distance < max_range) {
|
||||
delay_distance += curr_vel_parallel * (data_age * 1e-6f);
|
||||
}
|
||||
|
||||
const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
|
||||
const float vel_max_posctrl = xy_p * stop_distance;
|
||||
|
||||
const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
|
||||
const float projection = bin_direction.dot(setpoint_dir);
|
||||
float vel_max_bin = vel_max;
|
||||
|
||||
if (projection > 0.01f) {
|
||||
vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection;
|
||||
}
|
||||
|
||||
// constrain the velocity
|
||||
if (vel_max_bin >= 0) {
|
||||
vel_max = math::min(vel_max, vel_max_bin);
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
|
||||
if (!move_no_data || (move_no_data && _data_fov[i])) {
|
||||
vel_max = 0.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//if the sensor field of view is zero, never allow to move (even if move_no_data=1)
|
||||
if (num_fov_bins == 0) {
|
||||
vel_max = 0.f;
|
||||
}
|
||||
|
||||
setpoint = setpoint_dir * vel_max;
|
||||
if (_checkSetpointDirectionFeasability()) {
|
||||
constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length);
|
||||
}
|
||||
|
||||
} else {
|
||||
//allow no movement
|
||||
float vel_max = 0.f;
|
||||
setpoint = setpoint * vel_max;
|
||||
setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir;
|
||||
|
||||
} else if (!_obstacle_data_present)
|
||||
|
||||
{
|
||||
// allow no movement
|
||||
PX4_WARN("No obstacle data, not moving...");
|
||||
setpoint_accel.setZero();
|
||||
|
||||
// if distance data is stale, switch to Loiter
|
||||
if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
|
||||
|
||||
if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US
|
||||
&& getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
|
||||
if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US &&
|
||||
getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
|
||||
_publishVehicleCmdDoLoiter();
|
||||
}
|
||||
|
||||
_last_timeout_warning = getTime();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float
|
||||
CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
|
||||
{
|
||||
const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi();
|
||||
Vector2f dir = direction / direction.norm();
|
||||
const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad;
|
||||
const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) -
|
||||
_obstacle_map_body_frame.angle_offset);
|
||||
int dir_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
|
||||
|
||||
return _obstacle_map_body_frame.distances[dir_index] * 0.01f;
|
||||
}
|
||||
|
||||
Vector2f
|
||||
CollisionPrevention::_constrainAccelerationSetpoint(const float &setpoint_length)
|
||||
{
|
||||
Vector2f new_setpoint{};
|
||||
const Vector2f normal_component = _closest_dist_dir * (_setpoint_dir.dot(_closest_dist_dir));
|
||||
const Vector2f tangential_component = _setpoint_dir - normal_component;
|
||||
|
||||
const float normal_scale = _getScale(_closest_dist);
|
||||
|
||||
|
||||
const float closest_dist_tangential = _getObstacleDistance(tangential_component);
|
||||
const float tangential_scale = _getScale(closest_dist_tangential);
|
||||
|
||||
|
||||
// only scale accelerations towards the obstacle
|
||||
if (_closest_dist_dir.dot(_setpoint_dir) > 0) {
|
||||
new_setpoint = (tangential_component * tangential_scale + normal_component * normal_scale) * setpoint_length;
|
||||
|
||||
} else {
|
||||
new_setpoint = _setpoint_dir * setpoint_length;
|
||||
}
|
||||
|
||||
return new_setpoint;
|
||||
}
|
||||
|
||||
float
|
||||
CollisionPrevention::_getScale(const float &reference_distance)
|
||||
{
|
||||
float scale = (reference_distance - _min_dist_to_keep);
|
||||
const float scale_distance = math::max(_min_dist_to_keep, _param_mpc_vel_manual.get() / _param_mpc_xy_p.get());
|
||||
|
||||
// if scale is positive, square it and scale it with the scale_distance
|
||||
scale = scale > 0 ? powf(scale / scale_distance, 2) : scale;
|
||||
scale = math::min(scale, 1.0f);
|
||||
return scale;
|
||||
}
|
||||
|
||||
void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad,
|
||||
const matrix::Vector2f &setpoint_vel,
|
||||
const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir)
|
||||
{
|
||||
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
|
||||
const float max_range = _data_maxranges[i] * 0.01f;
|
||||
|
||||
// get the vector pointing into the direction of current bin
|
||||
float bin_angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG +
|
||||
_obstacle_map_body_frame.angle_offset));
|
||||
|
||||
const Vector2f bin_direction = { cosf(bin_angle), sinf(bin_angle) };
|
||||
float bin_distance = _obstacle_map_body_frame.distances[i];
|
||||
|
||||
// only consider bins which are between min and max values
|
||||
if (bin_distance > _obstacle_map_body_frame.min_distance && bin_distance < UINT16_MAX) {
|
||||
const float distance = bin_distance * 0.01f;
|
||||
|
||||
// Assume current velocity is sufficiently close to the setpoint velocity, this breaks down if flying high
|
||||
// acceleration maneuvers
|
||||
const float curr_vel_parallel = math::max(0.f, setpoint_vel.dot(bin_direction));
|
||||
float delay_distance = curr_vel_parallel * _param_cp_delay.get();
|
||||
|
||||
const hrt_abstime data_age = now - _data_timestamps[i];
|
||||
|
||||
if (distance < max_range) {
|
||||
delay_distance += curr_vel_parallel * (data_age * 1e-6f);
|
||||
}
|
||||
|
||||
const float stop_distance = math::max(0.f, distance - _min_dist_to_keep - delay_distance);
|
||||
|
||||
const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(),
|
||||
_param_mpc_acc_hor.get(), stop_distance, 0.f);
|
||||
// we dont take the minimum of the last term because of stop_distance is zero but current velocity is not, we want the acceleration to become negative and slow us down.
|
||||
const float curr_acc_vel_constraint = _param_mpc_vel_p_acc.get() * (max_vel - curr_vel_parallel);
|
||||
|
||||
if (curr_acc_vel_constraint < vel_comp_accel) {
|
||||
vel_comp_accel = curr_acc_vel_constraint;
|
||||
vel_comp_accel_dir = bin_direction;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
|
||||
const Vector2f &curr_vel)
|
||||
CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
|
||||
{
|
||||
//calculate movement constraints based on range data
|
||||
Vector2f new_setpoint = original_setpoint;
|
||||
_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
|
||||
|
||||
//warn user if collision prevention starts to interfere
|
||||
bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
|
||||
|| new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
|
||||
|| new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
|
||||
|| new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);
|
||||
|
||||
_interfering = currently_interfering;
|
||||
Vector2f original_setpoint = setpoint_accel;
|
||||
_calculateConstrainedSetpoint(setpoint_accel, setpoint_vel);
|
||||
|
||||
// publish constraints
|
||||
collision_constraints_s constraints{};
|
||||
constraints.timestamp = getTime();
|
||||
original_setpoint.copyTo(constraints.original_setpoint);
|
||||
new_setpoint.copyTo(constraints.adapted_setpoint);
|
||||
setpoint_accel.copyTo(constraints.adapted_setpoint);
|
||||
constraints.timestamp = getTime();
|
||||
_constraints_pub.publish(constraints);
|
||||
|
||||
original_setpoint = new_setpoint;
|
||||
}
|
||||
|
||||
void CollisionPrevention::_publishVehicleCmdDoLoiter()
|
||||
|
||||
@ -34,6 +34,7 @@
|
||||
/**
|
||||
* @file CollisionPrevention.hpp
|
||||
* @author Tanja Baumann <tanja@auterion.com>
|
||||
* @author Claudio Chies <claudio@chies.com>
|
||||
*
|
||||
* CollisionPrevention controller.
|
||||
*
|
||||
@ -60,6 +61,12 @@
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
||||
using namespace time_literals;
|
||||
using matrix::Vector2f;
|
||||
using matrix::Vector3f;
|
||||
using matrix::Quatf;
|
||||
using matrix::Eulerf;
|
||||
using matrix::wrap;
|
||||
using matrix::wrap_2pi;
|
||||
|
||||
class CollisionPrevention : public ModuleParams
|
||||
{
|
||||
@ -74,13 +81,10 @@ public:
|
||||
|
||||
/**
|
||||
* Computes collision free setpoints
|
||||
* @param original_setpoint, setpoint before collision prevention intervention
|
||||
* @param max_speed, maximum xy speed
|
||||
* @param curr_pos, current vehicle position
|
||||
* @param curr_vel, current vehicle velocity
|
||||
* @param setpoint_accel setpoint purely based on sticks, to be modified
|
||||
* @param setpoint_vel current velocity setpoint as information to be able to stop in time, does not get changed
|
||||
*/
|
||||
void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
|
||||
const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
|
||||
void modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel);
|
||||
|
||||
protected:
|
||||
|
||||
@ -90,13 +94,13 @@ protected:
|
||||
uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof(
|
||||
_obstacle_map_body_frame.distances[0])]; /**< in cm */
|
||||
|
||||
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude);
|
||||
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude);
|
||||
|
||||
/**
|
||||
* Updates obstacle distance message with measurement from offboard
|
||||
* @param obstacle, obstacle_distance message to be updated
|
||||
*/
|
||||
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude);
|
||||
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude);
|
||||
|
||||
/**
|
||||
* Computes an adaption to the setpoint direction to guide towards free space
|
||||
@ -104,7 +108,40 @@ protected:
|
||||
* @param setpoint_index, index of the setpoint in the internal obstacle map
|
||||
* @param vehicle_yaw_angle_rad, vehicle orientation
|
||||
*/
|
||||
void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad);
|
||||
void _adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad);
|
||||
|
||||
/**
|
||||
* Calculate the constrained setpoint cosdering the current obstacle distances, the current acceleration setpoint and velocity setpoint
|
||||
*/
|
||||
void _calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel);
|
||||
|
||||
/**
|
||||
* Constrain the acceleration setpoint based on the distance to the obstacle
|
||||
* The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above
|
||||
* +1 ________ _ _
|
||||
* ┌─┐ │ //
|
||||
* │X│ │ //
|
||||
* │X│ │ //
|
||||
* │X│ │ ///
|
||||
* │X│ │ //
|
||||
* │X│ │/////
|
||||
* │X│──────┼─────────────┬─────────────
|
||||
* │X│ /│ scale_distance
|
||||
* │X│ / │
|
||||
* │X│ / │
|
||||
* │X│ / │
|
||||
* │X│ / │
|
||||
* └─┘/ │
|
||||
* -1
|
||||
*/
|
||||
Vector2f _constrainAccelerationSetpoint(const float &setpoint_length);
|
||||
|
||||
void _getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, const matrix::Vector2f &setpoint_vel,
|
||||
const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir);
|
||||
|
||||
float _getObstacleDistance(const Vector2f &direction);
|
||||
|
||||
float _getScale(const float &reference_distance);
|
||||
|
||||
/**
|
||||
* Determines whether a new sensor measurement is used
|
||||
@ -114,6 +151,10 @@ protected:
|
||||
*/
|
||||
bool _enterData(int map_index, float sensor_range, float sensor_reading);
|
||||
|
||||
bool _checkSetpointDirectionFeasability();
|
||||
|
||||
void _transformSetpoint(const Vector2f &setpoint);
|
||||
|
||||
|
||||
//Timing functions. Necessary to mock time in the tests
|
||||
virtual hrt_abstime getTime();
|
||||
@ -122,10 +163,20 @@ protected:
|
||||
|
||||
private:
|
||||
|
||||
bool _interfering{false}; /**< states if the collision prevention interferes with the user input */
|
||||
bool _data_stale{true}; /**< states if the data is stale */
|
||||
bool _was_active{false}; /**< states if the collision prevention interferes with the user input */
|
||||
bool _obstacle_data_present{false}; /**< states if obstacle data is present */
|
||||
|
||||
int _setpoint_index{}; /**< index of the setpoint*/
|
||||
Vector2f _setpoint_dir{}; /**< direction of the setpoint*/
|
||||
|
||||
float _closest_dist{}; /**< closest distance to an obstacle */
|
||||
Vector2f _closest_dist_dir{NAN, NAN}; /**< direction of the closest obstacle */
|
||||
|
||||
float _min_dist_to_keep{};
|
||||
|
||||
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
|
||||
Vector2f _DEBUG;
|
||||
|
||||
uORB::Publication<collision_constraints_s> _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */
|
||||
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */
|
||||
@ -142,13 +193,15 @@ private:
|
||||
hrt_abstime _time_activated{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
|
||||
(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
|
||||
(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
|
||||
(ParamBool<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
|
||||
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
|
||||
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
|
||||
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/
|
||||
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
|
||||
(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
|
||||
(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
|
||||
(ParamBool<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
|
||||
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
|
||||
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
|
||||
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/
|
||||
(ParamFloat<px4::params::MPC_XY_VEL_P_ACC>) _param_mpc_vel_p_acc, /**< p gain from velocity controller*/
|
||||
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/
|
||||
)
|
||||
|
||||
/**
|
||||
@ -164,15 +217,15 @@ private:
|
||||
* @param curr_pos, current vehicle position
|
||||
* @param curr_vel, current vehicle velocity
|
||||
*/
|
||||
void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos,
|
||||
const matrix::Vector2f &curr_vel);
|
||||
void _calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
|
||||
const Vector2f &curr_vel);
|
||||
|
||||
/**
|
||||
* Publishes collision_constraints message
|
||||
* @param original_setpoint, setpoint before collision prevention intervention
|
||||
* @param adapted_setpoint, collision prevention adaped setpoint
|
||||
*/
|
||||
void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
|
||||
void _publishConstrainedSetpoint(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint);
|
||||
|
||||
/**
|
||||
* Publishes obstacle_distance message with fused data from offboard and from distance sensors
|
||||
@ -185,6 +238,11 @@ private:
|
||||
*/
|
||||
void _updateObstacleMap();
|
||||
|
||||
/**
|
||||
* Updates the obstacle data based on stale data and calculates values from the map
|
||||
*/
|
||||
void _updateObstacleData();
|
||||
|
||||
/**
|
||||
* Publishes vehicle command.
|
||||
*/
|
||||
|
||||
@ -104,8 +104,6 @@ TEST_F(CollisionPreventionTest, noSensorData)
|
||||
// GIVEN: a simple setup condition
|
||||
TestCollisionPrevention cp;
|
||||
matrix::Vector2f original_setpoint(10, 0);
|
||||
float max_speed = 3.f;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
|
||||
// AND: a parameter handle
|
||||
@ -117,7 +115,7 @@ TEST_F(CollisionPreventionTest, noSensorData)
|
||||
cp.paramsChanged();
|
||||
|
||||
matrix::Vector2f modified_setpoint = original_setpoint;
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
|
||||
// THEN: collision prevention should be enabled and limit the speed to zero
|
||||
EXPECT_TRUE(cp.is_active());
|
||||
@ -130,8 +128,6 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage)
|
||||
TestCollisionPrevention cp;
|
||||
matrix::Vector2f original_setpoint1(10, 0);
|
||||
matrix::Vector2f original_setpoint2(-10, 0);
|
||||
float max_speed = 3;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
vehicle_attitude_s attitude;
|
||||
attitude.timestamp = hrt_absolute_time();
|
||||
@ -141,9 +137,12 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage)
|
||||
attitude.q[3] = 0.0f;
|
||||
|
||||
// AND: a parameter handle
|
||||
param_t param = param_handle(px4::params::CP_DIST);
|
||||
float value = 10; // try to keep 10m distance
|
||||
param_set(param, &value);
|
||||
param_t param1 = param_handle(px4::params::CP_DIST);
|
||||
float value1 = 10; // try to keep 10m distance
|
||||
param_set(param1, &value1);
|
||||
param_t param2 = param_handle(px4::params::CP_GUIDE_ANG);
|
||||
float value2 = 0; // dont guide sideways
|
||||
param_set(param2, &value2);
|
||||
cp.paramsChanged();
|
||||
|
||||
// AND: an obstacle message
|
||||
@ -174,19 +173,20 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage)
|
||||
orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
|
||||
matrix::Vector2f modified_setpoint1 = original_setpoint1;
|
||||
matrix::Vector2f modified_setpoint2 = original_setpoint2;
|
||||
cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint1, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint2, curr_vel);
|
||||
orb_unadvertise(obstacle_distance_pub);
|
||||
orb_unadvertise(vehicle_attitude_pub);
|
||||
|
||||
// THEN: the internal map should know the obstacle
|
||||
// case 1: the velocity setpoint should be cut down to zero
|
||||
// case 2: the velocity setpoint should stay the same as the input
|
||||
// case 1: the acceleration setpoint should be negative as its pushing you away from the obstacle, and sideways acceleration should be low
|
||||
// case 2: the acceleration setpoint should be lower
|
||||
EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100);
|
||||
EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
|
||||
|
||||
EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1);
|
||||
EXPECT_FLOAT_EQ(original_setpoint2.norm(), modified_setpoint2.norm());
|
||||
EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0);
|
||||
EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1);
|
||||
EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0);
|
||||
EXPECT_EQ(0.f, fabsf(modified_setpoint2(1))) << modified_setpoint2(1);
|
||||
}
|
||||
|
||||
TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
|
||||
@ -195,8 +195,6 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
|
||||
TestCollisionPrevention cp;
|
||||
matrix::Vector2f original_setpoint1(10, 0);
|
||||
matrix::Vector2f original_setpoint2(-10, 0);
|
||||
float max_speed = 3;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
vehicle_attitude_s attitude;
|
||||
attitude.timestamp = hrt_absolute_time();
|
||||
@ -234,19 +232,23 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
|
||||
//WHEN: We run the setpoint modification
|
||||
matrix::Vector2f modified_setpoint1 = original_setpoint1;
|
||||
matrix::Vector2f modified_setpoint2 = original_setpoint2;
|
||||
cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint1, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint2, curr_vel);
|
||||
orb_unadvertise(distance_sensor_pub);
|
||||
orb_unadvertise(vehicle_attitude_pub);
|
||||
|
||||
// THEN: the internal map should know the obstacle
|
||||
// case 1: the velocity setpoint should be cut down to zero because there is an obstacle
|
||||
// case 2: the velocity setpoint should be cut down to zero because there is no data
|
||||
// case 1: the acceleration setpoint should be negative as its pushing you away from the obstacle, and sideways acceleration should be low
|
||||
// case 2: the acceleration setpoint should be lower
|
||||
EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100);
|
||||
EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
|
||||
|
||||
EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1);
|
||||
EXPECT_FLOAT_EQ(0.f, modified_setpoint2.norm()) << modified_setpoint2(0) << "," << modified_setpoint2(1);
|
||||
EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100);
|
||||
EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
|
||||
EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0);
|
||||
EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1);
|
||||
EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0);
|
||||
EXPECT_EQ(0.f, fabsf(modified_setpoint2(1))) << modified_setpoint2(1);
|
||||
}
|
||||
|
||||
TEST_F(CollisionPreventionTest, testPurgeOldData)
|
||||
@ -256,8 +258,6 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
|
||||
hrt_abstime start_time = hrt_absolute_time();
|
||||
mocked_time = start_time;
|
||||
matrix::Vector2f original_setpoint(10, 0);
|
||||
float max_speed = 3;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
vehicle_attitude_s attitude;
|
||||
attitude.timestamp = start_time;
|
||||
@ -268,7 +268,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
|
||||
|
||||
// AND: a parameter handle
|
||||
param_t param = param_handle(px4::params::CP_DIST);
|
||||
float value = 10; // try to keep 10m distance
|
||||
float value = 1; // try to keep 10m distance
|
||||
param_set(param, &value);
|
||||
cp.paramsChanged();
|
||||
|
||||
@ -308,7 +308,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
|
||||
for (int i = 0; i < 10; i++) {
|
||||
|
||||
matrix::Vector2f modified_setpoint = original_setpoint;
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
|
||||
mocked_time = mocked_time + 100000; //advance time by 0.1 seconds
|
||||
message_lost_data.timestamp = mocked_time;
|
||||
@ -345,8 +345,6 @@ TEST_F(CollisionPreventionTest, testNoRangeData)
|
||||
hrt_abstime start_time = hrt_absolute_time();
|
||||
mocked_time = start_time;
|
||||
matrix::Vector2f original_setpoint(10, 0);
|
||||
float max_speed = 3;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
vehicle_attitude_s attitude;
|
||||
attitude.timestamp = start_time;
|
||||
@ -386,7 +384,7 @@ TEST_F(CollisionPreventionTest, testNoRangeData)
|
||||
for (int i = 0; i < 10; i++) {
|
||||
|
||||
matrix::Vector2f modified_setpoint = original_setpoint;
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
|
||||
//advance time by 0.1 seconds but no new message comes in
|
||||
mocked_time = mocked_time + 100000;
|
||||
@ -411,13 +409,11 @@ TEST_F(CollisionPreventionTest, noBias)
|
||||
// GIVEN: a simple setup condition
|
||||
TestCollisionPrevention cp;
|
||||
matrix::Vector2f original_setpoint(10, 0);
|
||||
float max_speed = 3;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
|
||||
// AND: a parameter handle
|
||||
param_t param = param_handle(px4::params::CP_DIST);
|
||||
float value = 5; // try to keep 5m distance
|
||||
float value = 2; // try to keep 2m distance
|
||||
param_set(param, &value);
|
||||
cp.paramsChanged();
|
||||
|
||||
@ -439,7 +435,7 @@ TEST_F(CollisionPreventionTest, noBias)
|
||||
orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
|
||||
matrix::Vector2f modified_setpoint = original_setpoint;
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
orb_unadvertise(obstacle_distance_pub);
|
||||
|
||||
// THEN: setpoint should go into the same direction as the stick input
|
||||
@ -451,8 +447,6 @@ TEST_F(CollisionPreventionTest, outsideFOV)
|
||||
{
|
||||
// GIVEN: a simple setup condition
|
||||
TestCollisionPrevention cp;
|
||||
float max_speed = 3;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
|
||||
// AND: a parameter handle
|
||||
@ -493,7 +487,7 @@ TEST_F(CollisionPreventionTest, outsideFOV)
|
||||
matrix::Vector2f modified_setpoint = original_setpoint;
|
||||
message.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
|
||||
//THEN: if the resulting setpoint demands velocities bigger zero, it must lie inside the FOV
|
||||
float setpoint_length = modified_setpoint.norm();
|
||||
@ -514,8 +508,6 @@ TEST_F(CollisionPreventionTest, goNoData)
|
||||
{
|
||||
// GIVEN: a simple setup condition with the initial state (no distance data)
|
||||
TestCollisionPrevention cp;
|
||||
float max_speed = 3;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
|
||||
// AND: an obstacle message
|
||||
@ -532,7 +524,7 @@ TEST_F(CollisionPreventionTest, goNoData)
|
||||
float angle = i * message.increment;
|
||||
|
||||
if (angle > 0.f && angle < 40.f) {
|
||||
message.distances[i] = 700;
|
||||
message.distances[i] = 1000;
|
||||
|
||||
} else {
|
||||
message.distances[i] = UINT16_MAX;
|
||||
@ -541,7 +533,7 @@ TEST_F(CollisionPreventionTest, goNoData)
|
||||
|
||||
// AND: a parameter handle
|
||||
param_t param = param_handle(px4::params::CP_DIST);
|
||||
float value = 5; // try to keep 5m distance
|
||||
float value = 2; // try to keep 5m distance
|
||||
param_set(param, &value);
|
||||
cp.paramsChanged();
|
||||
|
||||
@ -549,8 +541,8 @@ TEST_F(CollisionPreventionTest, goNoData)
|
||||
matrix::Vector2f original_setpoint = {-5, 0};
|
||||
matrix::Vector2f modified_setpoint = original_setpoint;
|
||||
|
||||
//THEN: the modified setpoint should be zero velocity
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
//THEN: the modified setpoint should be zero acceleration
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f);
|
||||
|
||||
//WHEN: we change the parameter CP_GO_NO_DATA to allow flying ouside the FOV
|
||||
@ -561,7 +553,7 @@ TEST_F(CollisionPreventionTest, goNoData)
|
||||
|
||||
//THEN: When all bins contain UINT_16MAX the setpoint should be zero even if CP_GO_NO_DATA=1
|
||||
modified_setpoint = original_setpoint;
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f);
|
||||
|
||||
//THEN: As soon as the range data contains any valid number, flying outside the FOV is allowed
|
||||
@ -570,7 +562,7 @@ TEST_F(CollisionPreventionTest, goNoData)
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
|
||||
|
||||
modified_setpoint = original_setpoint;
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
EXPECT_FLOAT_EQ(modified_setpoint.norm(), original_setpoint.norm());
|
||||
orb_unadvertise(obstacle_distance_pub);
|
||||
}
|
||||
@ -580,8 +572,6 @@ TEST_F(CollisionPreventionTest, jerkLimit)
|
||||
// GIVEN: a simple setup condition
|
||||
TestCollisionPrevention cp;
|
||||
matrix::Vector2f original_setpoint(10, 0);
|
||||
float max_speed = 3;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
|
||||
// AND: distance set to 5m
|
||||
@ -608,7 +598,7 @@ TEST_F(CollisionPreventionTest, jerkLimit)
|
||||
orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
|
||||
matrix::Vector2f modified_setpoint_default_jerk = original_setpoint;
|
||||
cp.modifySetpoint(modified_setpoint_default_jerk, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint_default_jerk, curr_vel);
|
||||
orb_unadvertise(obstacle_distance_pub);
|
||||
|
||||
// AND: we now set max jerk to 0.1
|
||||
@ -619,11 +609,39 @@ TEST_F(CollisionPreventionTest, jerkLimit)
|
||||
|
||||
// WHEN: we run the setpoint modification again
|
||||
matrix::Vector2f modified_setpoint_limited_jerk = original_setpoint;
|
||||
cp.modifySetpoint(modified_setpoint_limited_jerk, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint_limited_jerk, curr_vel);
|
||||
|
||||
// THEN: the new setpoint should be much slower than the one with default jerk
|
||||
EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm());
|
||||
}
|
||||
TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData)
|
||||
{
|
||||
// GIVEN: a vehicle attitude and a distance sensor message
|
||||
TestCollisionPrevention cp;
|
||||
cp.getObstacleMap().increment = 10.f;
|
||||
matrix::Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
|
||||
distance_sensor_s distance_sensor {};
|
||||
distance_sensor.min_distance = 0.2f;
|
||||
distance_sensor.max_distance = 20.f;
|
||||
distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
|
||||
// Distance is out of Range
|
||||
distance_sensor.current_distance = -1.f;
|
||||
distance_sensor.signal_quality = 0;
|
||||
|
||||
uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
|
||||
|
||||
cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
|
||||
|
||||
//THEN: the correct bins in the map should be filled
|
||||
for (uint32_t i = 0; i < distances_array_size; i++) {
|
||||
if (i == 0) {
|
||||
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], distance_sensor.max_distance * 100.f);
|
||||
|
||||
} else {
|
||||
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(CollisionPreventionTest, addDistanceSensorData)
|
||||
{
|
||||
@ -1062,8 +1080,6 @@ TEST_F(CollisionPreventionTest, overlappingSensors)
|
||||
// GIVEN: a simple setup condition
|
||||
TestCollisionPrevention cp;
|
||||
matrix::Vector2f original_setpoint(10, 0);
|
||||
float max_speed = 3;
|
||||
matrix::Vector2f curr_pos(0, 0);
|
||||
matrix::Vector2f curr_vel(2, 0);
|
||||
vehicle_attitude_s attitude;
|
||||
attitude.timestamp = hrt_absolute_time();
|
||||
@ -1115,7 +1131,7 @@ TEST_F(CollisionPreventionTest, overlappingSensors)
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg);
|
||||
orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
|
||||
matrix::Vector2f modified_setpoint = original_setpoint;
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
|
||||
// THEN: the internal map data should contain the long range measurement
|
||||
EXPECT_EQ(500, cp.getObstacleMap().distances[2]);
|
||||
@ -1124,10 +1140,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors)
|
||||
// WHEN: we publish the short range message followed by a long range message
|
||||
short_range_msg.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg);
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
long_range_msg.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg);
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
|
||||
// THEN: the internal map data should contain the short range measurement
|
||||
EXPECT_EQ(150, cp.getObstacleMap().distances[2]);
|
||||
@ -1136,10 +1152,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors)
|
||||
// WHEN: we publish the short range message with values out of range followed by a long range message
|
||||
short_range_msg_no_obstacle.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg_no_obstacle);
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
long_range_msg.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg);
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
cp.modifySetpoint(modified_setpoint, curr_vel);
|
||||
|
||||
// THEN: the internal map data should contain the short range measurement
|
||||
EXPECT_EQ(500, cp.getObstacleMap().distances[2]);
|
||||
|
||||
@ -94,11 +94,6 @@ void FlightTaskManualPosition::_scaleSticks()
|
||||
// Rotate setpoint into local frame
|
||||
Sticks::rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint);
|
||||
|
||||
// collision prevention
|
||||
if (_collision_prevention.is_active()) {
|
||||
_collision_prevention.modifySetpoint(vel_sp_xy, velocity_scale, _position.xy(), _velocity.xy());
|
||||
}
|
||||
|
||||
_velocity_setpoint.xy() = vel_sp_xy;
|
||||
}
|
||||
|
||||
|
||||
@ -96,6 +96,14 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
|
||||
Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp);
|
||||
_acceleration_setpoint = stick_xy.emult(acceleration_scale);
|
||||
|
||||
if (_collision_prevention.is_active()) {
|
||||
matrix::Vector2f accel_setpoint_xy = _acceleration_setpoint;
|
||||
matrix::Vector2f vel_setpoint_xy = _velocity_setpoint;
|
||||
_collision_prevention.modifySetpoint(accel_setpoint_xy, vel_setpoint_xy);
|
||||
_acceleration_setpoint = accel_setpoint_xy;
|
||||
|
||||
}
|
||||
|
||||
// Add drag to limit speed and brake again
|
||||
Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint);
|
||||
|
||||
|
||||
@ -40,6 +40,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <lib/collision_prevention/CollisionPrevention.hpp>
|
||||
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
@ -65,6 +66,8 @@ public:
|
||||
void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); };
|
||||
|
||||
private:
|
||||
CollisionPrevention _collision_prevention{this};
|
||||
|
||||
void applyJerkLimit(const float dt);
|
||||
matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy,
|
||||
const matrix::Vector2f &vel_sp);
|
||||
|
||||
@ -90,6 +90,7 @@ if(gz-transport_FOUND)
|
||||
lawn
|
||||
aruco
|
||||
rover
|
||||
walls
|
||||
)
|
||||
|
||||
# find corresponding airframes
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user