diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index dd0049f5e9..5fdd9c8010 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -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{}; diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 547c63c3aa..1c52724d9b 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -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. */