mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 15:38:52 +08:00
CollisionPrevention: move main functions to the top in the order they get called
This commit is contained in:
Matthias Grob
committed by
Claudio Chies
Claudio Chies
parent
1fa76ac71d
commit
001d722abd
@@ -85,6 +85,147 @@ bool CollisionPrevention::is_active()
|
||||
return activated;
|
||||
}
|
||||
|
||||
void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
|
||||
{
|
||||
//calculate movement constraints based on range data
|
||||
const Vector2f original_setpoint = setpoint_accel;
|
||||
_updateObstacleMap();
|
||||
_updateObstacleData();
|
||||
_calculateConstrainedSetpoint(setpoint_accel, setpoint_vel);
|
||||
|
||||
// publish constraints
|
||||
collision_constraints_s constraints{};
|
||||
original_setpoint.copyTo(constraints.original_setpoint);
|
||||
setpoint_accel.copyTo(constraints.adapted_setpoint);
|
||||
constraints.timestamp = getTime();
|
||||
_constraints_pub.publish(constraints);
|
||||
}
|
||||
|
||||
void CollisionPrevention::_updateObstacleMap()
|
||||
{
|
||||
_sub_vehicle_attitude.update();
|
||||
|
||||
// add distance sensor data
|
||||
for (auto &dist_sens_sub : _distance_sensor_subs) {
|
||||
distance_sensor_s distance_sensor;
|
||||
|
||||
if (dist_sens_sub.update(&distance_sensor)) {
|
||||
// consider only instances with valid data and orientations useful for collision prevention
|
||||
if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
|
||||
(distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
|
||||
(distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
|
||||
|
||||
// update message description
|
||||
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp);
|
||||
_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
|
||||
(uint16_t)(distance_sensor.max_distance * 100.0f));
|
||||
_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
|
||||
(uint16_t)(distance_sensor.min_distance * 100.0f));
|
||||
|
||||
_addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// add obstacle distance data
|
||||
if (_sub_obstacle_distance.update()) {
|
||||
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
|
||||
|
||||
// Update map with obstacle data if the data is not stale
|
||||
if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) {
|
||||
//update message description
|
||||
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp);
|
||||
_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
|
||||
obstacle_distance.max_distance);
|
||||
_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
|
||||
obstacle_distance.min_distance);
|
||||
_addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q));
|
||||
}
|
||||
}
|
||||
|
||||
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
|
||||
_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 < BIN_COUNT; 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 * BIN_SIZE +
|
||||
_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::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
|
||||
{
|
||||
const Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
|
||||
const float vehicle_yaw_angle_rad = Eulerf(attitude).psi();
|
||||
|
||||
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 now = getTime();
|
||||
|
||||
float vel_comp_accel = INFINITY;
|
||||
Vector2f vel_comp_accel_dir{};
|
||||
Vector2f constr_accel_setpoint{};
|
||||
|
||||
const bool is_stick_deflected = setpoint_length > 0.001f;
|
||||
|
||||
if (_obstacle_data_present && is_stick_deflected) {
|
||||
|
||||
_transformSetpoint(setpoint_accel);
|
||||
|
||||
_getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now,
|
||||
vel_comp_accel, vel_comp_accel_dir);
|
||||
|
||||
if (_checkSetpointDirectionFeasability()) {
|
||||
constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length);
|
||||
}
|
||||
|
||||
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 ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US &&
|
||||
getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
|
||||
_publishVehicleCmdDoLoiter();
|
||||
}
|
||||
|
||||
_last_timeout_warning = getTime();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude)
|
||||
{
|
||||
@@ -200,84 +341,6 @@ CollisionPrevention::_transformSetpoint(const Vector2f &setpoint)
|
||||
_adaptSetpointDirection(_setpoint_dir, _setpoint_index, vehicle_yaw_angle_rad);
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_updateObstacleMap()
|
||||
{
|
||||
_sub_vehicle_attitude.update();
|
||||
|
||||
// add distance sensor data
|
||||
for (auto &dist_sens_sub : _distance_sensor_subs) {
|
||||
distance_sensor_s distance_sensor;
|
||||
|
||||
if (dist_sens_sub.update(&distance_sensor)) {
|
||||
// consider only instances with valid data and orientations useful for collision prevention
|
||||
if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
|
||||
(distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
|
||||
(distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
|
||||
|
||||
// update message description
|
||||
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp);
|
||||
_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
|
||||
(uint16_t)(distance_sensor.max_distance * 100.0f));
|
||||
_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
|
||||
(uint16_t)(distance_sensor.min_distance * 100.0f));
|
||||
|
||||
_addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// add obstacle distance data
|
||||
if (_sub_obstacle_distance.update()) {
|
||||
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
|
||||
|
||||
// Update map with obstacle data if the data is not stale
|
||||
if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) {
|
||||
//update message description
|
||||
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp);
|
||||
_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
|
||||
obstacle_distance.max_distance);
|
||||
_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
|
||||
obstacle_distance.min_distance);
|
||||
_addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q));
|
||||
}
|
||||
}
|
||||
|
||||
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
|
||||
_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 < BIN_COUNT; 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 * BIN_SIZE +
|
||||
_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 Quatf &vehicle_attitude)
|
||||
{
|
||||
@@ -419,58 +482,6 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist
|
||||
return offset;
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
|
||||
{
|
||||
_updateObstacleMap();
|
||||
_updateObstacleData();
|
||||
|
||||
const Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
|
||||
const float vehicle_yaw_angle_rad = Eulerf(attitude).psi();
|
||||
|
||||
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 now = getTime();
|
||||
|
||||
float vel_comp_accel = INFINITY;
|
||||
Vector2f vel_comp_accel_dir{};
|
||||
Vector2f constr_accel_setpoint{};
|
||||
|
||||
const bool is_stick_deflected = setpoint_length > 0.001f;
|
||||
|
||||
if (_obstacle_data_present && is_stick_deflected) {
|
||||
|
||||
_transformSetpoint(setpoint_accel);
|
||||
|
||||
_getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now,
|
||||
vel_comp_accel, vel_comp_accel_dir);
|
||||
|
||||
if (_checkSetpointDirectionFeasability()) {
|
||||
constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length);
|
||||
}
|
||||
|
||||
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 ((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 direction_norm = direction.norm();
|
||||
@@ -571,21 +582,6 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
|
||||
{
|
||||
//calculate movement constraints based on range data
|
||||
Vector2f original_setpoint = setpoint_accel;
|
||||
_calculateConstrainedSetpoint(setpoint_accel, setpoint_vel);
|
||||
|
||||
// publish constraints
|
||||
collision_constraints_s constraints{};
|
||||
original_setpoint.copyTo(constraints.original_setpoint);
|
||||
setpoint_accel.copyTo(constraints.adapted_setpoint);
|
||||
constraints.timestamp = getTime();
|
||||
_constraints_pub.publish(constraints);
|
||||
}
|
||||
|
||||
void CollisionPrevention::_publishVehicleCmdDoLoiter()
|
||||
{
|
||||
vehicle_command_s command{};
|
||||
|
||||
@@ -81,6 +81,15 @@ public:
|
||||
void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
|
||||
|
||||
protected:
|
||||
/** Aggregates the sensor data into an internal obstacle map in body frame */
|
||||
void _updateObstacleMap();
|
||||
|
||||
/** Updates the obstacle data based on stale data and calculates values from the map */
|
||||
void _updateObstacleData();
|
||||
|
||||
/** Calculate the constrained setpoint considering the current obstacle distances, acceleration setpoint and velocity setpoint */
|
||||
void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
|
||||
|
||||
static constexpr int BIN_COUNT = 36;
|
||||
static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly
|
||||
|
||||
@@ -105,11 +114,6 @@ protected:
|
||||
*/
|
||||
void _adaptSetpointDirection(matrix::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(matrix::Vector2f &setpoint_accel, const matrix::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
|
||||
@@ -226,16 +230,6 @@ private:
|
||||
*/
|
||||
void _publishObstacleDistance(obstacle_distance_s &obstacle);
|
||||
|
||||
/**
|
||||
* Aggregates the sensor data into a internal obstacle map in body frame
|
||||
*/
|
||||
void _updateObstacleMap();
|
||||
|
||||
/**
|
||||
* Updates the obstacle data based on stale data and calculates values from the map
|
||||
*/
|
||||
void _updateObstacleData();
|
||||
|
||||
/**
|
||||
* Publishes vehicle command.
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user