diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 8b82ada39d34..f4b25807a1bf 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -195,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() { @@ -242,6 +272,38 @@ 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 Quatf &vehicle_attitude) { @@ -381,213 +443,132 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist } void -CollisionPrevention::_constrainAccelerationSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) +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(); + const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get()); const hrt_abstime now = getTime(); - int num_fov_bins = 0; - - float acc_vel_constraint = INFINITY; - Vector2f acc_vel_constraint_dir{}; - Vector2f acc_vel_constraint_setpoint{}; - - if ((now - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { - - const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get()); - bool setpoint_possible = true; // False when moving into direction with no sensor - Vector2f new_setpoint{}; - - const bool is_stick_deflected = setpoint_length > 0.001f; - - if (is_stick_deflected) { - Vector2f setpoint_dir = setpoint_accel / setpoint_length; - - 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); - - // 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); - - float closest_distance = INFINITY; - Vector2f bin_closest_dist{}; - - - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - // delete stale values - const hrt_abstime data_age = now - _data_timestamps[i]; - const float max_range = _data_maxranges[i] * 0.01f; - - if (data_age > RANGE_STREAM_TIMEOUT_US) { - _obstacle_map_body_frame.distances[i] = UINT16_MAX; - } - - if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) { - num_fov_bins ++; - } - - float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); - angle = wrap_2pi(vehicle_yaw_angle_rad + angle); - - // get direction of current bin - const Vector2f bin_direction = {cosf(angle), sinf(angle)}; - // only consider bins which are between min and max values - if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance - && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { - const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; + float vel_comp_accel = INFINITY; + Vector2f vel_comp_accel_dir{}; + Vector2f constr_accel_setpoint{}; - // Assume current velocity is sufficiently close to the setpoint velocity - 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 bool is_stick_deflected = setpoint_length > 0.001f; - if (distance < max_range) { - delay_distance += curr_vel_parallel * (data_age * 1e-6f); - } + if (_obstacle_data_present && is_stick_deflected) { - const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); - const float vel_max_posctrl = _param_mpc_xy_p.get() * stop_distance; - const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), - _param_mpc_acc_hor.get(), stop_distance, 0.f); + _transformSetpoint(setpoint_accel); - const float vel_max = math::min(vel_max_posctrl, vel_max_smooth); - const float acc_max_postrl = _param_mpc_vel_p_acc.get() * math::min((vel_max - curr_vel_parallel), 0.f); + _getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now, min_dist_to_keep, + vel_comp_accel, vel_comp_accel_dir); - if (acc_max_postrl < acc_vel_constraint) { - acc_vel_constraint = acc_max_postrl; - acc_vel_constraint_dir = bin_direction; - } + if (_checkSetpointDirectionFeasability()) { + constr_accel_setpoint = _constrainAccelerationSetpoint(min_dist_to_keep, setpoint_length); + } - if (distance < closest_distance) { - closest_distance = distance; - bin_closest_dist = bin_direction; - } + setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir; - // calculate closest distance for acceleration constraint + } else if (!_obstacle_data_present) + { + // allow no movement + PX4_WARN("No obstacle data, not moving..."); + setpoint_accel.setZero(); - } else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) { - if (!_param_cp_go_nodata.get() || (_param_cp_go_nodata.get() && _data_fov[i])) { - setpoint_possible = false; - } - } + // 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(); } - const Vector2f normal_component = bin_closest_dist * (setpoint_dir.dot(bin_closest_dist)); - const Vector2f tangential_component = setpoint_dir - normal_component; - - - if (closest_distance < min_dist_to_keep && setpoint_possible) { // Already closer than allowed, push away - float scale = (closest_distance - min_dist_to_keep); // always negative meaning it will push us away from the obstacle - new_setpoint = tangential_component * setpoint_length + bin_closest_dist * _param_mpc_xy_p.get() * - _param_mpc_vel_p_acc.get() * - scale; // scale is on the closest distance vector, as thats the critical direction - - } else if (closest_distance >= min_dist_to_keep - && setpoint_possible) { // Enough distance but constrain input towards obstacle - const float scale_distance = math::max(min_dist_to_keep, _param_mpc_vel_manual.get() / _param_mpc_xy_p.get()); - float scale = (closest_distance - min_dist_to_keep) / scale_distance; - scale *= scale; // square the scale to lower commanded accelerations close to the obstacle - scale = math::min(scale, 1.0f); - - - // only scale accelerations towards the obstacle - if (bin_closest_dist.dot(setpoint_dir) > 0) { - new_setpoint = (tangential_component + normal_component * scale) * setpoint_length; + _last_timeout_warning = getTime(); + } + } +} - } else { - new_setpoint = setpoint_dir * setpoint_length ; - } +Vector2f +CollisionPrevention::_constrainAccelerationSetpoint(const float &min_dist_to_keep, 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; - } + float scale = (_closest_dist - 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 (num_fov_bins == 0) { - setpoint_accel.setZero(); - PX4_WARN("No fov bins"); + // 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); - } else { - acc_vel_constraint_setpoint = acc_vel_constraint_dir * acc_vel_constraint; - setpoint_accel = new_setpoint + acc_vel_constraint_setpoint; - } + // only scale accelerations towards the obstacle + if (_closest_dist_dir.dot(_setpoint_dir) > 0) { + new_setpoint = (tangential_component + normal_component * scale) * setpoint_length; + } else { + new_setpoint = _setpoint_dir * setpoint_length; + } - } else { - // If no setpoint is provided, still apply force when you are close to an obstacle - float closest_distance = INFINITY; - Vector2f bin_closest_dist; + return new_setpoint; +} - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - if (now - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) { - _obstacle_map_body_frame.distances[i] = UINT16_MAX; - } +void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, + const matrix::Vector2f &setpoint_vel, + const hrt_abstime now, const float min_dist_to_keep, 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; - //count number of bins in the field of valid_new - if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) { - num_fov_bins ++; - } + // 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)); - float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); - angle = wrap_2pi(vehicle_yaw_angle_rad + angle); + const Vector2f bin_direction = { cosf(bin_angle), sinf(bin_angle) }; + float bin_distance = _obstacle_map_body_frame.distances[i]; - // get direction of current bin - const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + // 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; - if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance - && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { - const float distance = _obstacle_map_body_frame.distances[i] * 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(); - if (distance < closest_distance) { - closest_distance = distance; - bin_closest_dist = bin_direction; - } - } - } + const hrt_abstime data_age = now - _data_timestamps[i]; - if (closest_distance < min_dist_to_keep) { - float scale = (closest_distance - min_dist_to_keep); - new_setpoint = bin_closest_dist * _param_mpc_xy_p.get() * _param_mpc_vel_p_acc.get() * scale; + if (distance < max_range) { + delay_distance += curr_vel_parallel * (data_age * 1e-6f); } - if (num_fov_bins == 0) { - setpoint_accel.setZero(); - PX4_WARN("No fov bins"); - - } else { - setpoint_accel = new_setpoint; - } - } + const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); - } else { - //allow no movement - PX4_WARN("No movement"); - setpoint_accel.setZero(); + const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), + _param_mpc_acc_hor.get(), stop_distance, 0.f); - // if distance data is stale, switch to Loiter - if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { + const float curr_acc_vel_constraint = _param_mpc_vel_p_acc.get() * math::min((max_vel - curr_vel_parallel), 0.f); - if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US - && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { - _publishVehicleCmdDoLoiter(); + if (curr_acc_vel_constraint < vel_comp_accel) { + vel_comp_accel = curr_acc_vel_constraint; + vel_comp_accel_dir = bin_direction; } - - _last_timeout_warning = getTime(); } } - } + void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { //calculate movement constraints based on range data Vector2f original_setpoint = setpoint_accel; - _constrainAccelerationSetpoint(setpoint_accel, setpoint_vel); + _calculateConstrainedSetpoint(setpoint_accel, setpoint_vel); // publish constraints collision_constraints_s constraints{}; diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index e3b92e73a1e3..eb592d62eeb3 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -34,6 +34,7 @@ /** * @file CollisionPrevention.hpp * @author Tanja Baumann + * @author Claudio Chies * * CollisionPrevention controller. * @@ -109,9 +110,33 @@ class CollisionPrevention : public ModuleParams void _adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); /** - * Constrains the Acceleration setpoint cosdering the current obstacle distances, the current acceleration setpoint and velocity setpoint + * Calculate the constrained setpoint cosdering the current obstacle distances, the current acceleration setpoint and velocity setpoint */ - void _constrainAccelerationSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel); + 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 &min_dist_to_keep, const float &setpoint_length); + + void _getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, const matrix::Vector2f &setpoint_vel, + const hrt_abstime now, const float min_dist_to_keep, float &vel_comp_accel, Vector2f &vel_comp_accel_dir); /** * Determines whether a new sensor measurement is used @@ -121,6 +146,10 @@ class CollisionPrevention : public ModuleParams */ 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(); @@ -129,9 +158,18 @@ class CollisionPrevention : public ModuleParams private: + 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{0}; /**< 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 */ orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ + Vector2f _DEBUG; uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ @@ -148,15 +186,15 @@ class CollisionPrevention : public ModuleParams hrt_abstime _time_activated{0}; DEFINE_PARAMETERS( - (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ - (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ - (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ - (ParamBool) _param_cp_go_nodata, /**< movement allowed where no data*/ - (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ - (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ - (ParamFloat) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/ + (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ + (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ + (ParamBool) _param_cp_go_nodata, /**< movement allowed where no data*/ + (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ + (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ + (ParamFloat) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/ (ParamFloat) _param_mpc_vel_p_acc, /**< p gain from velocity controller*/ - (ParamFloat) _param_mpc_vel_manual /**< p gain from velocity controller*/ + (ParamFloat) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/ ) /** @@ -193,6 +231,11 @@ class CollisionPrevention : public ModuleParams */ void _updateObstacleMap(); + /** + * Updates the obstacle data based on stale data and calculates values from the map + */ + void _updateObstacleData(); + /** * Publishes vehicle command. */ diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 86f5327deaae..24d5f37894bb 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -245,6 +245,8 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); + printf("modified_setpoint1: %f, %f\n", (double)modified_setpoint1(0), (double)modified_setpoint1(1)); + printf("modified_setpoint2: %f, %f\n", (double)modified_setpoint2(0), (double)modified_setpoint2(1)); 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);