From 91637f31c827c86b402376a62ca4b75f94478bf7 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 9 Aug 2024 14:58:13 +0200 Subject: [PATCH 01/18] CollisionPrevention: rewrite for Acceleration based manual flight mode --- .../CollisionPrevention.cpp | 298 +++++++++++------- .../CollisionPrevention.hpp | 98 ++++-- .../CollisionPreventionTest.cpp | 128 ++++---- .../FlightTaskManualPosition.cpp | 5 - .../tasks/Utility/StickAccelerationXY.cpp | 8 + .../tasks/Utility/StickAccelerationXY.hpp | 3 + .../simulation/gz_bridge/CMakeLists.txt | 1 + 7 files changed, 348 insertions(+), 193 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d4e4273b6982..8dfc27edf189 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -40,8 +40,6 @@ #include "CollisionPrevention.hpp" #include -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 (_checkSetpointDirectionFeasability()) { + constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length); + } - if (data_age > RANGE_STREAM_TIMEOUT_US) { - _obstacle_map_body_frame.distances[i] = UINT16_MAX; - } + setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir; - 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); + } else if (!_obstacle_data_present) - // convert from body to local frame in the range [0, 2*pi] - angle = wrap_2pi(vehicle_yaw_angle_rad + angle); + { + // allow no movement + PX4_WARN("No obstacle data, not moving..."); + setpoint_accel.setZero(); - // get direction of current bin - const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + // 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(); + } - //count number of bins in the field of valid_new - if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) { - num_fov_bins ++; - } + _last_timeout_warning = getTime(); + } + } +} - if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance - && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { +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; +} - 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; +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; - if (distance < max_range) { - delay_distance += curr_vel_parallel * (data_age * 1e-6f); - } + const float normal_scale = _getScale(_closest_dist); - 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; + const float closest_dist_tangential = _getObstacleDistance(tangential_component); + const float tangential_scale = _getScale(closest_dist_tangential); - 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); - } - } + // 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 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; - } - } - } + } else { + new_setpoint = _setpoint_dir * setpoint_length; + } - //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; - } + return new_setpoint; +} - setpoint = setpoint_dir * vel_max; - } +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()); - } else { - //allow no movement - float vel_max = 0.f; - setpoint = setpoint * vel_max; + // 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; +} - // if distance data is stale, switch to Loiter - if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { +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; - if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US - && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { - _publishVehicleCmdDoLoiter(); + // 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); } - _last_timeout_warning = getTime(); - } + 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() diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index aaff04e2cbdc..651e6372f4db 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. * @@ -60,6 +61,12 @@ #include 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 @@ class CollisionPrevention : public ModuleParams /** * 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 @@ class CollisionPrevention : public ModuleParams 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 @@ class CollisionPrevention : public ModuleParams * @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 @@ 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(); @@ -122,10 +163,20 @@ class CollisionPrevention : public ModuleParams 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 _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ @@ -142,13 +193,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 /**< maximum velocity in manual flight mode*/ ) /** @@ -164,15 +217,15 @@ class CollisionPrevention : public ModuleParams * @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 @@ 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 9de0a208aa00..45b01b2a7ed0 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -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 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]); diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 71dedaf0566a..352875764e30 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -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; } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index c0fe9b315b67..e9e9ef0dbfdf 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -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); diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 8bc2bb75313d..9fff1da1c426 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -65,6 +66,8 @@ class StickAccelerationXY : public ModuleParams 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); diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 358a4e703226..a094298fab90 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -90,6 +90,7 @@ if(gz-transport_FOUND) lawn aruco rover + walls ) # find corresponding airframes From ba6e0e9d758d569be6f712b601562feb0a8dfcfe Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 11 Nov 2024 15:53:30 +0100 Subject: [PATCH 02/18] StickAccelerationXY: Simplify collision prevention call --- .../tasks/Utility/StickAccelerationXY.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index e9e9ef0dbfdf..f823f929e819 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -97,11 +97,7 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, _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; - + _collision_prevention.modifySetpoint(_acceleration_setpoint, _velocity_setpoint); } // Add drag to limit speed and brake again From ddc5e5a46edb6c7f5c0871e956d3d2940ea604d6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 11 Nov 2024 15:56:07 +0100 Subject: [PATCH 03/18] CollisionPrevention: fix matrix namespace don't use it in header such that clients are free to redefine the names but include it in cpp files and make use of that. --- .../CollisionPrevention.cpp | 3 +- .../CollisionPrevention.hpp | 36 ++--- .../CollisionPreventionTest.cpp | 142 +++++++++--------- 3 files changed, 87 insertions(+), 94 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 8dfc27edf189..2bc3b6626664 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -40,6 +40,7 @@ #include "CollisionPrevention.hpp" #include +using namespace matrix; namespace { @@ -550,7 +551,7 @@ CollisionPrevention::_getScale(const float &reference_distance) } void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, - const matrix::Vector2f &setpoint_vel, + const 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++) { diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 651e6372f4db..b7720b047e00 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -61,12 +61,6 @@ #include 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 { @@ -84,7 +78,7 @@ class CollisionPrevention : public ModuleParams * @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(Vector2f &setpoint_accel, const Vector2f &setpoint_vel); + void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); protected: @@ -94,13 +88,13 @@ class CollisionPrevention : public ModuleParams 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 Quatf &vehicle_attitude); + void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::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 Quatf &vehicle_attitude); + void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude); /** * Computes an adaption to the setpoint direction to guide towards free space @@ -108,12 +102,12 @@ class CollisionPrevention : public ModuleParams * @param setpoint_index, index of the setpoint in the internal obstacle map * @param vehicle_yaw_angle_rad, vehicle orientation */ - void _adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); + 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(Vector2f &setpoint_accel, const Vector2f &setpoint_vel); + void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); /** * Constrain the acceleration setpoint based on the distance to the obstacle @@ -134,12 +128,12 @@ class CollisionPrevention : public ModuleParams * └─┘/ │ * -1 */ - Vector2f _constrainAccelerationSetpoint(const float &setpoint_length); + matrix::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); + const hrt_abstime now, float &vel_comp_accel, matrix::Vector2f &vel_comp_accel_dir); - float _getObstacleDistance(const Vector2f &direction); + float _getObstacleDistance(const matrix::Vector2f &direction); float _getScale(const float &reference_distance); @@ -153,7 +147,7 @@ class CollisionPrevention : public ModuleParams bool _checkSetpointDirectionFeasability(); - void _transformSetpoint(const Vector2f &setpoint); + void _transformSetpoint(const matrix::Vector2f &setpoint); //Timing functions. Necessary to mock time in the tests @@ -168,15 +162,15 @@ class CollisionPrevention : public ModuleParams 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*/ + matrix::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 */ + matrix::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; + matrix::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 */ @@ -217,15 +211,15 @@ class CollisionPrevention : public ModuleParams * @param curr_pos, current vehicle position * @param curr_vel, current vehicle velocity */ - void _calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, - const Vector2f &curr_vel); + void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, + const matrix::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 Vector2f &original_setpoint, const Vector2f &adapted_setpoint); + void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); /** * Publishes obstacle_distance message with fused data from offboard and from distance sensors diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 45b01b2a7ed0..a75cd57662d3 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -34,6 +34,8 @@ #include #include "CollisionPrevention.hpp" +using namespace matrix; + // to run: make tests TESTFILTER=CollisionPrevention hrt_abstime mocked_time = 0; @@ -53,15 +55,15 @@ class TestCollisionPrevention : public CollisionPrevention TestCollisionPrevention() : CollisionPrevention(nullptr) {} void paramsChanged() {CollisionPrevention::updateParamsImpl();} obstacle_distance_s &getObstacleMap() {return _obstacle_map_body_frame;} - void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude) + void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &attitude) { _addDistanceSensorData(distance_sensor, attitude); } - void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &attitude) + void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &attitude) { _addObstacleSensorData(obstacle, attitude); } - void test_adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, + void test_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { _adaptSetpointDirection(setpoint_dir, setpoint_index, vehicle_yaw_angle_rad); @@ -103,8 +105,8 @@ TEST_F(CollisionPreventionTest, noSensorData) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -114,7 +116,7 @@ TEST_F(CollisionPreventionTest, noSensorData) param_set(param, &value); cp.paramsChanged(); - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: collision prevention should be enabled and limit the speed to zero @@ -126,9 +128,9 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint1(10, 0); - matrix::Vector2f original_setpoint2(-10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint1(10, 0); + Vector2f original_setpoint2(-10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -163,7 +165,6 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) } else { message.distances[i] = 10001; } - } // WHEN: we publish the message and set the parameter and then run the setpoint modification @@ -171,8 +172,8 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); - matrix::Vector2f modified_setpoint1 = original_setpoint1; - matrix::Vector2f modified_setpoint2 = original_setpoint2; + Vector2f modified_setpoint1 = original_setpoint1; + Vector2f modified_setpoint2 = original_setpoint2; cp.modifySetpoint(modified_setpoint1, curr_vel); cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -193,9 +194,9 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint1(10, 0); - matrix::Vector2f original_setpoint2(-10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint1(10, 0); + Vector2f original_setpoint2(-10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -230,8 +231,8 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); //WHEN: We run the setpoint modification - matrix::Vector2f modified_setpoint1 = original_setpoint1; - matrix::Vector2f modified_setpoint2 = original_setpoint2; + Vector2f modified_setpoint1 = original_setpoint1; + Vector2f modified_setpoint2 = original_setpoint2; cp.modifySetpoint(modified_setpoint1, curr_vel); cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(distance_sensor_pub); @@ -257,8 +258,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) TestTimingCollisionPrevention cp; hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; attitude.q[0] = 1.0f; @@ -306,8 +307,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { - - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); mocked_time = mocked_time + 100000; //advance time by 0.1 seconds @@ -344,8 +344,8 @@ TEST_F(CollisionPreventionTest, testNoRangeData) TestTimingCollisionPrevention cp; hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; attitude.q[0] = 1.0f; @@ -382,8 +382,7 @@ TEST_F(CollisionPreventionTest, testNoRangeData) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { - - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); //advance time by 0.1 seconds but no new message comes in @@ -408,8 +407,8 @@ TEST_F(CollisionPreventionTest, noBias) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -434,7 +433,7 @@ TEST_F(CollisionPreventionTest, noBias) // WHEN: we publish the message and set the parameter and then run the setpoint modification 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; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -447,7 +446,7 @@ TEST_F(CollisionPreventionTest, outsideFOV) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f curr_vel(2, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -480,11 +479,10 @@ TEST_F(CollisionPreventionTest, outsideFOV) orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); for (int i = 0; i < distances_array_size; i++) { - float angle_deg = (float)i * message.increment; float angle_rad = math::radians(angle_deg); - matrix::Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; + Vector2f modified_setpoint = original_setpoint; message.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); cp.modifySetpoint(modified_setpoint, curr_vel); @@ -493,9 +491,9 @@ TEST_F(CollisionPreventionTest, outsideFOV) float setpoint_length = modified_setpoint.norm(); if (setpoint_length > 0.f) { - matrix::Vector2f setpoint_dir = modified_setpoint / setpoint_length; + Vector2f setpoint_dir = modified_setpoint / setpoint_length; float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0)); - float sp_angle_deg = math::degrees(matrix::wrap_2pi(sp_angle_body_frame)); + float sp_angle_deg = math::degrees(wrap_2pi(sp_angle_body_frame)); EXPECT_GE(sp_angle_deg, 45.f); EXPECT_LE(sp_angle_deg, 225.f); } @@ -508,7 +506,7 @@ TEST_F(CollisionPreventionTest, goNoData) { // GIVEN: a simple setup condition with the initial state (no distance data) TestCollisionPrevention cp; - matrix::Vector2f curr_vel(2, 0); + Vector2f curr_vel(2, 0); // AND: an obstacle message obstacle_distance_s message; @@ -538,8 +536,8 @@ TEST_F(CollisionPreventionTest, goNoData) cp.paramsChanged(); // AND: a setpoint outside the field of view - matrix::Vector2f original_setpoint = {-5, 0}; - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f original_setpoint = {-5, 0}; + Vector2f modified_setpoint = original_setpoint; //THEN: the modified setpoint should be zero acceleration cp.modifySetpoint(modified_setpoint, curr_vel); @@ -571,8 +569,8 @@ TEST_F(CollisionPreventionTest, jerkLimit) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: distance set to 5m param_t param = param_handle(px4::params::CP_DIST); @@ -597,7 +595,7 @@ TEST_F(CollisionPreventionTest, jerkLimit) // AND: we publish the message and set the parameter and then run the setpoint modification 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; + Vector2f modified_setpoint_default_jerk = original_setpoint; cp.modifySetpoint(modified_setpoint_default_jerk, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -608,7 +606,7 @@ TEST_F(CollisionPreventionTest, jerkLimit) cp.paramsChanged(); // WHEN: we run the setpoint modification again - matrix::Vector2f modified_setpoint_limited_jerk = original_setpoint; + Vector2f modified_setpoint_limited_jerk = original_setpoint; cp.modifySetpoint(modified_setpoint_limited_jerk, curr_vel); // THEN: the new setpoint should be much slower than the one with default jerk @@ -619,7 +617,7 @@ TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData) // GIVEN: a vehicle attitude and a distance sensor message TestCollisionPrevention cp; cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion 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; @@ -648,7 +646,7 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) // GIVEN: a vehicle attitude and a distance sensor message TestCollisionPrevention cp; cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion 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; @@ -732,13 +730,13 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); - matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); - matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - matrix::Euler attitude4_euler(0, 0, M_PI); - matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw + Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform + Euler attitude2_euler(0, 0, M_PI / 2.0); + Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw + Euler attitude3_euler(0, 0, -M_PI / 4.0); + Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw + Euler attitude4_euler(0, 0, M_PI); + Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw //obstacle at 10-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -833,13 +831,13 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); - matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); - matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - matrix::Euler attitude4_euler(0, 0, M_PI); - matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw + Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform + Euler attitude2_euler(0, 0, M_PI / 2.0); + Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw + Euler attitude3_euler(0, 0, -M_PI / 4.0); + Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw + Euler attitude4_euler(0, 0, M_PI); + Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw //obstacle at 10-30 deg body frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -934,7 +932,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -991,8 +989,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi(); + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1004,10 +1002,10 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) obstacle_msg.distances[2] = 1000; //define setpoint - matrix::Vector2f setpoint_dir(1, 0); + Vector2f setpoint_dir(1, 0); float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, - 0.f, 360.f); + float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, + 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); //set parameter @@ -1038,8 +1036,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi(); + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1053,10 +1051,10 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) obstacle_msg.distances[3] = 1000; //define setpoint - matrix::Vector2f setpoint_dir(1, 0); + Vector2f setpoint_dir(1, 0); float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, - 0.f, 360.f); + float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, + 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); //set parameter @@ -1079,8 +1077,8 @@ TEST_F(CollisionPreventionTest, overlappingSensors) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -1130,7 +1128,7 @@ TEST_F(CollisionPreventionTest, overlappingSensors) orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); 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; + Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the long range measurement @@ -1169,7 +1167,7 @@ TEST_F(CollisionPreventionTest, enterData) // GIVEN: a simple setup condition TestCollisionPrevention cp; cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform //THEN: just after initialization all bins are at UINT16_MAX and any data should be accepted EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range From fd46ac8cc2e58a25d6388da67efe488d07003dd8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 11 Nov 2024 18:31:39 +0100 Subject: [PATCH 04/18] CollisionPrevention: Sanitize input of _getObstacleDistance() It could cause array out of bound problems before. --- .../CollisionPrevention.cpp | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 2bc3b6626664..eaa0c011725c 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -500,17 +500,23 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, con } } -float -CollisionPrevention::_getObstacleDistance(const Vector2f &direction) +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); + const float direction_norm = direction.norm(); + + if (direction_norm > FLT_EPSILON) { + Vector2f dir = direction / direction_norm; + const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); + 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); + dir_index = math::constrain(dir_index, 0, 71); + return _obstacle_map_body_frame.distances[dir_index] * 0.01f; - return _obstacle_map_body_frame.distances[dir_index] * 0.01f; + } else { + return 0.f; + } } Vector2f From 970e500d5b97f5025e5b39eff35d581267e78537 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Nov 2024 18:44:09 +0100 Subject: [PATCH 05/18] CollisionPrevention: clarify mode switch command to hold/loiter --- src/lib/collision_prevention/CollisionPrevention.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index eaa0c011725c..8dacd66723e5 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -618,19 +618,16 @@ CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &se void CollisionPrevention::_publishVehicleCmdDoLoiter() { vehicle_command_s command{}; - command.timestamp = getTime(); command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode + command.param1 = 1.f; // base mode VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; command.target_system = 1; command.target_component = 1; command.source_system = 1; command.source_component = 1; command.confirmation = false; command.from_external = false; - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - // publish the vehicle command + command.timestamp = getTime(); _vehicle_command_pub.publish(command); } From d5e652ca407283eb3ac5253003be436996ef2e60 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Nov 2024 20:16:36 +0100 Subject: [PATCH 06/18] CollisionPrevention: Clarify bin size definitions, move wrap functions into class --- .../CollisionPrevention.cpp | 101 ++++++++---------- .../CollisionPrevention.hpp | 18 ++-- 2 files changed, 57 insertions(+), 62 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 8dacd66723e5..4f6ff22974a3 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -42,46 +42,24 @@ using namespace matrix; -namespace -{ -static constexpr int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly -static constexpr int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG; - -static float wrap_360(float f) -{ - return wrap(f, 0.f, 360.f); -} - -static int wrap_bin(int i) -{ - i = i % INTERNAL_MAP_USED_BINS; - - while (i < 0) { - i += INTERNAL_MAP_USED_BINS; - } - - return i; -} - -} // namespace - CollisionPrevention::CollisionPrevention(ModuleParams *parent) : ModuleParams(parent) { - static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5"); - static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly"); + static_assert(BIN_SIZE >= 5, "BIN_SIZE must be at least 5"); + static_assert(360 % BIN_SIZE == 0, "BIN_SIZE must divide 360 evenly"); + static_assert(sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]) + >= BIN_COUNT, "BIN_COUNT must not overflow obstacle_distance.distances"); // initialize internal obstacle map _obstacle_map_body_frame.timestamp = getTime(); _obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; - _obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG; + _obstacle_map_body_frame.increment = BIN_SIZE; _obstacle_map_body_frame.min_distance = UINT16_MAX; _obstacle_map_body_frame.max_distance = 0; _obstacle_map_body_frame.angle_offset = 0.f; - uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); - uint64_t current_time = getTime(); + const uint64_t current_time = getTime(); - for (uint32_t i = 0 ; i < internal_bins; i++) { + for (uint32_t i = 0 ; i < BIN_COUNT_EXTERNAL; i++) { _data_timestamps[i] = current_time; _data_maxranges[i] = 0; _data_fov[i] = 0; @@ -121,9 +99,9 @@ CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { // Obstacle message arrives in local_origin frame (north aligned) // corresponding data index (convert to world frame and shift by msg offset) - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); + for (int i = 0; i < BIN_COUNT; i++) { + float bin_angle_deg = (float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset; + msg_index = ceil(_wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); //add all data points inside to FOV if (obstacle.distances[msg_index] != UINT16_MAX) { @@ -139,10 +117,10 @@ CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, } else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) { // Obstacle message arrives in body frame (front aligned) // corresponding data index (shift by msg offset) - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + + for (int i = 0; i < BIN_COUNT; i++) { + float bin_angle_deg = (float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); + msg_index = ceil(_wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); //add all data points inside to FOV if (obstacle.distances[msg_index] != UINT16_MAX) { @@ -201,7 +179,7 @@ CollisionPrevention::_checkSetpointDirectionFeasability() { bool setpoint_feasible = true; - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + for (int i = 0; i < BIN_COUNT; 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]))) { @@ -219,9 +197,9 @@ 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) - + 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); + _setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); // 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); } @@ -280,14 +258,13 @@ void CollisionPrevention::_updateObstacleData() _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++) { - + 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 * INTERNAL_MAP_INCREMENT_DEG + + 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]; @@ -323,10 +300,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices - int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / - INTERNAL_MAP_INCREMENT_DEG); - int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / - INTERNAL_MAP_INCREMENT_DEG); + int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); + int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); // floor values above zero, ceil values below zero if (lower_bound < 0) { lower_bound++; } @@ -345,7 +320,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, uint16_t sensor_range = static_cast(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm for (int bin = lower_bound; bin <= upper_bound; ++bin) { - int wrapped_bin = wrap_bin(bin); + int wrapped_bin = _wrap_bin(bin); if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) { _obstacle_map_body_frame.distances[wrapped_bin] = static_cast(100.0f * distance_reading + 0.5f); @@ -360,7 +335,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { - const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG); + const int guidance_bins = floor(_param_cp_guide_ang.get() / BIN_SIZE); const int sp_index_original = setpoint_index; float best_cost = 9999.f; int new_sp_index = setpoint_index; @@ -372,7 +347,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi float mean_dist = 0; for (int j = i - filter_size; j <= i + filter_size; j++) { - int bin = wrap_bin(j); + int bin = _wrap_bin(j); if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) { mean_dist += _param_cp_dist.get() * 100.f; @@ -382,7 +357,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi } } - const int bin = wrap_bin(i); + const int bin = _wrap_bin(i); mean_dist = mean_dist / (2.f * filter_size + 1.f); 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]; @@ -395,7 +370,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi //only change setpoint direction if it was moved to a different bin if (new_sp_index != setpoint_index) { - float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); + float angle = math::radians((float)new_sp_index * BIN_SIZE + _obstacle_map_body_frame.angle_offset); angle = wrap_2pi(vehicle_yaw_angle_rad + angle); setpoint_dir = {cosf(angle), sinf(angle)}; setpoint_index = new_sp_index; @@ -509,8 +484,8 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); 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); + _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); + int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE); dir_index = math::constrain(dir_index, 0, 71); return _obstacle_map_body_frame.distances[dir_index] * 0.01f; @@ -560,12 +535,12 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic const 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++) { + for (int i = 0; i < BIN_COUNT; 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)); + float bin_angle = wrap_2pi(vehicle_yaw_angle_rad + + math::radians((float)i * BIN_SIZE + _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]; @@ -631,3 +606,19 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter() command.timestamp = getTime(); _vehicle_command_pub.publish(command); } + +float CollisionPrevention::_wrap_360(const float f) +{ + return wrap(f, 0.f, 360.f); +} + +int CollisionPrevention::_wrap_bin(int i) +{ + i = i % BIN_COUNT; + + while (i < 0) { + i += BIN_COUNT; + } + + return i; +} diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index b7720b047e00..bf30c7a4559d 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -81,12 +81,16 @@ class CollisionPrevention : public ModuleParams void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); protected: + static constexpr int BIN_COUNT = 36; + static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly - obstacle_distance_s _obstacle_map_body_frame {}; - bool _data_fov[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])]; - uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])]; - uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof( - _obstacle_map_body_frame.distances[0])]; /**< in cm */ + obstacle_distance_s _obstacle_map_body_frame{}; + static constexpr int BIN_COUNT_EXTERNAL = + sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); + + bool _data_fov[BIN_COUNT_EXTERNAL]; + uint64_t _data_timestamps[BIN_COUNT_EXTERNAL]; + uint16_t _data_maxranges[BIN_COUNT_EXTERNAL]; /**< in cm */ void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude); @@ -154,9 +158,7 @@ class CollisionPrevention : public ModuleParams virtual hrt_abstime getTime(); virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr); - 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 */ @@ -242,4 +244,6 @@ class CollisionPrevention : public ModuleParams */ void _publishVehicleCmdDoLoiter(); + static float _wrap_360(const float f); + static int _wrap_bin(int i); }; From 3c7d07f90ee298af7e57c3f0a74fc90f8f7c5bb6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 12 Nov 2024 21:25:09 +0100 Subject: [PATCH 07/18] CollisionPrevention: reduce internal array size, zero initialize what's possible --- src/lib/collision_prevention/CollisionPrevention.cpp | 10 +++------- src/lib/collision_prevention/CollisionPrevention.hpp | 9 +++------ 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 4f6ff22974a3..dd0049f5e96e 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -51,18 +51,14 @@ CollisionPrevention::CollisionPrevention(ModuleParams *parent) : >= BIN_COUNT, "BIN_COUNT must not overflow obstacle_distance.distances"); // initialize internal obstacle map - _obstacle_map_body_frame.timestamp = getTime(); _obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; _obstacle_map_body_frame.increment = BIN_SIZE; _obstacle_map_body_frame.min_distance = UINT16_MAX; - _obstacle_map_body_frame.max_distance = 0; - _obstacle_map_body_frame.angle_offset = 0.f; - const uint64_t current_time = getTime(); + + static constexpr int BIN_COUNT_EXTERNAL = + sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); for (uint32_t i = 0 ; i < BIN_COUNT_EXTERNAL; i++) { - _data_timestamps[i] = current_time; - _data_maxranges[i] = 0; - _data_fov[i] = 0; _obstacle_map_body_frame.distances[i] = UINT16_MAX; } } diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index bf30c7a4559d..547c63c3aa8a 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -85,12 +85,9 @@ class CollisionPrevention : public ModuleParams static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly obstacle_distance_s _obstacle_map_body_frame{}; - static constexpr int BIN_COUNT_EXTERNAL = - sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); - - bool _data_fov[BIN_COUNT_EXTERNAL]; - uint64_t _data_timestamps[BIN_COUNT_EXTERNAL]; - uint16_t _data_maxranges[BIN_COUNT_EXTERNAL]; /**< in cm */ + bool _data_fov[BIN_COUNT] {}; + uint64_t _data_timestamps[BIN_COUNT] {}; + uint16_t _data_maxranges[BIN_COUNT] {}; /**< in cm */ void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude); From 3378c9b32c3d8d5995c26d81a2020192e04fa349 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 00:06:19 +0100 Subject: [PATCH 08/18] CollisionPrevention: move main functions to the top in the order they get called --- .../CollisionPrevention.cpp | 286 +++++++++--------- .../CollisionPrevention.hpp | 24 +- 2 files changed, 150 insertions(+), 160 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index dd0049f5e96e..5fdd9c801016 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 547c63c3aa8a..1c52724d9b70 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -81,6 +81,15 @@ class CollisionPrevention : public ModuleParams 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 @@ class CollisionPrevention : public ModuleParams */ 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 @@ class CollisionPrevention : public ModuleParams */ 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. */ From 0cdff14a40a0423ef2282bbaaf26508b9b5c8c11 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 00:56:04 +0100 Subject: [PATCH 09/18] CollisionPrevention: only save quaternion and yaw on attitude update --- .../CollisionPrevention.cpp | 36 +++++------ .../CollisionPrevention.hpp | 8 ++- .../CollisionPreventionTest.cpp | 60 +++++++------------ 3 files changed, 43 insertions(+), 61 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 5fdd9c801016..d5c19b3d09e5 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -87,6 +87,15 @@ bool CollisionPrevention::is_active() void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude = Quatf(vehicle_attitude.q); + _vehicle_yaw = Eulerf(_vehicle_attitude).psi(); + } + } + //calculate movement constraints based on range data const Vector2f original_setpoint = setpoint_accel; _updateObstacleMap(); @@ -103,8 +112,6 @@ void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2 void CollisionPrevention::_updateObstacleMap() { - _sub_vehicle_attitude.update(); - // add distance sensor data for (auto &dist_sens_sub : _distance_sensor_subs) { distance_sensor_s distance_sensor; @@ -122,7 +129,7 @@ void CollisionPrevention::_updateObstacleMap() _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)); + _addDistanceSensorData(distance_sensor, _vehicle_attitude); } } } @@ -139,7 +146,7 @@ void CollisionPrevention::_updateObstacleMap() 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)); + _addObstacleSensorData(obstacle_distance, _vehicle_yaw); } } @@ -152,7 +159,6 @@ 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 @@ -160,7 +166,7 @@ void CollisionPrevention::_updateObstacleData() _obstacle_map_body_frame.distances[i] = UINT16_MAX; } - float angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * BIN_SIZE + + float angle = wrap_2pi(_vehicle_yaw + 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]; @@ -180,9 +186,6 @@ void CollisionPrevention::_updateObstacleData() 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()); @@ -198,7 +201,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel _transformSetpoint(setpoint_accel); - _getVelocityCompensationAcceleration(vehicle_yaw_angle_rad, setpoint_vel, now, + _getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now, vel_comp_accel, vel_comp_accel_dir); if (_checkSetpointDirectionFeasability()) { @@ -226,11 +229,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel } } -void -CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude) +void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { int msg_index = 0; - float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi()); + float vehicle_orientation_deg = math::degrees(vehicle_yaw); float increment_factor = 1.f / obstacle.increment; if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { @@ -331,14 +333,13 @@ CollisionPrevention::_checkSetpointDirectionFeasability() 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_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - _vehicle_yaw; 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 / BIN_SIZE); // 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); + _adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw); } void @@ -488,8 +489,7 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) if (direction_norm > FLT_EPSILON) { Vector2f dir = direction / direction_norm; - const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); - const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw; 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 / BIN_SIZE); diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 1c52724d9b70..0023f7716c27 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -104,7 +104,7 @@ class CollisionPrevention : public ModuleParams * 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 float vehicle_yaw); /** * Computes an adaption to the setpoint direction to guide towards free space @@ -173,14 +173,16 @@ class CollisionPrevention : public ModuleParams float _min_dist_to_keep{}; orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ - matrix::Vector2f _DEBUG; + + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + matrix::Quatf _vehicle_attitude{}; + float _vehicle_yaw{0.f}; uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ - uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms}; diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index a75cd57662d3..955293dad5ef 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -59,9 +59,9 @@ class TestCollisionPrevention : public CollisionPrevention { _addDistanceSensorData(distance_sensor, attitude); } - void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &attitude) + void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { - _addObstacleSensorData(obstacle, attitude); + _addObstacleSensorData(obstacle, vehicle_yaw); } void test_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) @@ -730,14 +730,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - Euler attitude2_euler(0, 0, M_PI / 2.0); - Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - Euler attitude3_euler(0, 0, -M_PI / 4.0); - Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - Euler attitude4_euler(0, 0, M_PI); - Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -754,7 +746,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -771,7 +763,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) //WHEN: we add distance sensor data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -787,7 +779,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -803,7 +795,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -831,14 +823,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - Euler attitude2_euler(0, 0, M_PI / 2.0); - Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - Euler attitude3_euler(0, 0, -M_PI / 4.0); - Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - Euler attitude4_euler(0, 0, M_PI); - Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg body frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -855,7 +839,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -871,7 +855,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -887,7 +871,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -903,7 +887,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -932,8 +916,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -942,7 +924,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) } //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); @@ -961,7 +943,7 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) //WHEN: we add distance sensor data with an angle offset obstacle_msg.angle_offset = 30.f; - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { @@ -989,8 +971,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1003,7 +984,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) //define setpoint Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); @@ -1015,8 +996,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); @@ -1036,8 +1017,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1052,7 +1032,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) //define setpoint Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); @@ -1064,8 +1044,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); From 0092ac33a39af469bb1185387e18bc1614db9576 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 02:03:52 +0100 Subject: [PATCH 10/18] CollisionPrevention: restore rate limited warning for no data, minor cleanup --- .../CollisionPrevention.cpp | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d5c19b3d09e5..e3aff2531093 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -169,7 +169,7 @@ void CollisionPrevention::_updateObstacleData() float angle = wrap_2pi(_vehicle_yaw + 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]; + const uint16_t 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 @@ -191,30 +191,28 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel 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); + float vel_comp_accel = INFINITY; + Vector2f vel_comp_accel_dir{}; + _getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now, vel_comp_accel, vel_comp_accel_dir); + Vector2f constr_accel_setpoint{}; + 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) - - { + } 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 @@ -224,7 +222,8 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel _publishVehicleCmdDoLoiter(); } - _last_timeout_warning = getTime(); + PX4_WARN("No obstacle data, not moving..."); + _last_timeout_warning = now; } } } @@ -263,7 +262,6 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst //add all data points inside to FOV if (obstacle.distances[msg_index] != UINT16_MAX) { - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; _data_timestamps[i] = _obstacle_map_body_frame.timestamp; @@ -354,8 +352,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, } // discard values below min range - if ((distance_reading > distance_sensor.min_distance)) { - + if (distance_reading > distance_sensor.min_distance) { float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); @@ -440,7 +437,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi float CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const { - float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f; + float offset = math::max(math::radians(angle_offset), 0.f); switch (distance_sensor.orientation) { case distance_sensor_s::ROTATION_YAW_0: From d0a5faf6007cf24d0c818f7a381513c4c2e0fcf2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 02:30:52 +0100 Subject: [PATCH 11/18] Remove FlightTaskManualPositionSmoothVel The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation. --- .../init.d-posix/airframes/4006_gz_px4vision | 1 - .../flight_mode_manager/CMakeLists.txt | 1 - .../flight_mode_manager/FlightModeManager.cpp | 4 - .../ManualPositionSmoothVel/CMakeLists.txt | 39 ---- .../FlightTaskManualPositionSmoothVel.cpp | 185 ------------------ .../FlightTaskManualPositionSmoothVel.hpp | 92 --------- .../MulticopterPositionControl.hpp | 1 - .../multicopter_position_mode_params.c | 7 +- 8 files changed, 1 insertion(+), 329 deletions(-) delete mode 100644 src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt delete mode 100644 src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp delete mode 100644 src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index c5909381780f..65d290e0cc79 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -75,7 +75,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 81ff6a7a334e..84ccba6ac052 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -47,7 +47,6 @@ list(APPEND flight_tasks_all ManualAltitude ManualAltitudeSmoothVel ManualPosition - ManualPositionSmoothVel Transition ) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 3d7072607358..c58a7fa1572b 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -213,10 +213,6 @@ void FlightModeManager::start_flight_task() error = switchTask(FlightTaskIndex::ManualPosition); break; - case 3: - error = switchTask(FlightTaskIndex::ManualPositionSmoothVel); - break; - case 4: default: if (_param_mpc_pos_mode.get() != 4) { diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt deleted file mode 100644 index 8d4489fa3e55..000000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(FlightTaskManualPositionSmoothVel - FlightTaskManualPositionSmoothVel.cpp -) - -target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility) -target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp deleted file mode 100644 index a89283e14619..000000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "FlightTaskManualPositionSmoothVel.hpp" - -#include -#include - -using namespace matrix; - -bool FlightTaskManualPositionSmoothVel::activate(const trajectory_setpoint_s &last_setpoint) -{ - bool ret = FlightTaskManualPosition::activate(last_setpoint); - - // Check if the previous FlightTask provided setpoints - Vector3f accel_prev{last_setpoint.acceleration}; - Vector3f vel_prev{last_setpoint.velocity}; - Vector3f pos_prev{last_setpoint.position}; - - for (int i = 0; i < 3; i++) { - // If the position setpoint is unknown, set to the current postion - if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } - - // If the velocity setpoint is unknown, set to the current velocity - if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } - - // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } - } - - _smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev}); - _smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2)); - - return ret; -} - -void FlightTaskManualPositionSmoothVel::reActivate() -{ - FlightTaskManualPosition::reActivate(); - // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly - // using the generated jerk, reset the z derivatives to zero - _smoothing_xy.reset(Vector2f(), _velocity.xy(), _position.xy()); - _smoothing_z.reset(0.f, 0.f, _position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) -{ - _smoothing_xy.setCurrentPosition(_position.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) -{ - _smoothing_xy.setCurrentVelocity(_velocity.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ(float delta_z) -{ - _smoothing_z.setCurrentPosition(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz) -{ - _smoothing_z.setCurrentVelocity(_velocity(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateSetpoints() -{ - // Set max accel/vel/jerk - // Has to be done before _updateTrajectories() - _updateTrajConstraints(); - _updateTrajVelFeedback(); - _updateTrajCurrentPositionEstimate(); - - // Get yaw setpoint, un-smoothed position setpoints - FlightTaskManualPosition::_updateSetpoints(); - - _updateTrajectories(_velocity_setpoint); - - // Fill the jerk, acceleration, velocity and position setpoint vectors - _setOutputState(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() -{ - _updateTrajConstraintsXY(); - _updateTrajConstraintsZ(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY() -{ - _smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get()); - _smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get()); - _smoothing_xy.setMaxVel(_param_mpc_vel_manual.get()); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ() -{ - _smoothing_z.setMaxJerk(_param_mpc_jerk_max.get()); - - _smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get()); - _smoothing_z.setMaxVelUp(_constraints.speed_up); - - _smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get()); - _smoothing_z.setMaxVelDown(_constraints.speed_down); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback() -{ - _smoothing_xy.setVelSpFeedback(_velocity_setpoint_feedback.xy()); - _smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate() -{ - _smoothing_xy.setCurrentPositionEstimate(_position.xy()); - _smoothing_z.setCurrentPositionEstimate(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target) -{ - _smoothing_xy.update(_deltatime, vel_target.xy()); - _smoothing_z.update(_deltatime, vel_target(2)); -} - -void FlightTaskManualPositionSmoothVel::_setOutputState() -{ - _setOutputStateXY(); - _setOutputStateZ(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateXY() -{ - _jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk(); - _acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration(); - _velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity(); - _position_setpoint.xy() = _smoothing_xy.getCurrentPosition(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateZ() -{ - _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); - _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); - _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); - - if (!_terrain_hold) { - if (_terrain_hold_previous) { - // Reset position setpoint to current position when switching from terrain hold to non-terrain hold - _smoothing_z.setCurrentPosition(_position(2)); - } - - _position_setpoint(2) = _smoothing_z.getCurrentPosition(); - } - - _terrain_hold_previous = _terrain_hold; -} diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp deleted file mode 100644 index c8fbdf42c95a..000000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FlightTaskManualPositionSmoothVel.hpp - * - * Flight task for smooth manual controlled position. - */ - -#pragma once - -#include "FlightTaskManualPosition.hpp" -#include -#include - -using matrix::Vector2f; -using matrix::Vector3f; - -class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition -{ -public: - FlightTaskManualPositionSmoothVel() = default; - virtual ~FlightTaskManualPositionSmoothVel() = default; - - bool activate(const trajectory_setpoint_s &last_setpoint) override; - void reActivate() override; - -protected: - - virtual void _updateSetpoints() override; - - /** Reset position or velocity setpoints in case of EKF reset event */ - void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; - void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; - void _ekfResetHandlerPositionZ(float delta_z) override; - void _ekfResetHandlerVelocityZ(float delta_vz) override; - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition, - (ParamFloat) _param_mpc_jerk_max, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_down_max - ) - -private: - void _updateTrajConstraints(); - void _updateTrajConstraintsXY(); - void _updateTrajConstraintsZ(); - - void _updateTrajVelFeedback(); - void _updateTrajCurrentPositionEstimate(); - - void _updateTrajectories(Vector3f vel_target); - - void _setOutputState(); - void _setOutputStateXY(); - void _setOutputStateZ(); - - ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions - ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction - - bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ -}; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index f7cd9bbbe09a..6d58b85cd7ad 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -167,7 +167,6 @@ class MulticopterPositionControl : public ModuleBase (ParamFloat) _param_mpc_vel_man_side, (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ - (ParamInt) _param_mpc_pos_mode, (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ (ParamFloat) _param_mpc_thr_min, diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c index a65ef4f5713d..9fce344319c8 100644 --- a/src/modules/mc_pos_control/multicopter_position_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -39,14 +39,10 @@ * Sticks directly map to velocity setpoints without smoothing. * Also applies to vertical direction and Altitude mode. * Useful for velocity control tuning. - * - "Smoothed velocity": - * Sticks map to velocity but with maximum acceleration and jerk limits based on - * jerk optimized trajectory generator (different algorithm than 1). * - "Acceleration based": * Sticks map to acceleration and there's a virtual brake drag * * @value 0 Direct velocity - * @value 3 Smoothed velocity * @value 4 Acceleration based * @group Multicopter Position Control */ @@ -104,7 +100,6 @@ PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.f); * * MPC_POS_MODE * 1 just deceleration - * 3 acceleration and deceleration * 4 not used, use MPC_ACC_HOR instead * * @unit m/s^2 @@ -125,7 +120,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); * * Setting this to the maximum value essentially disables the limit. * - * Only used with smooth MPC_POS_MODE Smoothed velocity and Acceleration based. + * Only used with MPC_POS_MODE Acceleration based. * * @unit m/s^3 * @min 0.5 From 2200159c873b2ff77573f6bcb9641503652e6e4f Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Thu, 14 Nov 2024 14:32:59 +0100 Subject: [PATCH 12/18] CollisionPrevention: changed to resolution of 5 degrees, and adapted tests to reflect the change rewrite of obstacle_distance merging methods, and fix of various issues --- .../CollisionPrevention.cpp | 103 +++-- .../CollisionPrevention.hpp | 8 +- .../CollisionPreventionTest.cpp | 358 +++++++++++++----- src/modules/simulation/gz_bridge/GZBridge.cpp | 2 +- 4 files changed, 341 insertions(+), 130 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index e3aff2531093..324be7357f40 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -151,7 +151,7 @@ void CollisionPrevention::_updateObstacleMap() } // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor - _obstacle_distance_pub.publish(_obstacle_map_body_frame); + _obstacle_distance_fused_pub.publish(_obstacle_map_body_frame); } void CollisionPrevention::_updateObstacleData() @@ -228,27 +228,51 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel } } +// TODO this gives false output if the offset is not a multiple of the resolution. to be fixed... void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { - int msg_index = 0; + float vehicle_orientation_deg = math::degrees(vehicle_yaw); - float increment_factor = 1.f / obstacle.increment; + if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { // Obstacle message arrives in local_origin frame (north aligned) // corresponding data index (convert to world frame and shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { - float bin_angle_deg = (float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(_wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); - - //add all data points inside to FOV - if (obstacle.distances[msg_index] != UINT16_MAX) { - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { - _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; - _data_timestamps[i] = _obstacle_map_body_frame.timestamp; - _data_maxranges[i] = obstacle.max_distance; - _data_fov[i] = 1; + for (int j = 0; j < 360 / obstacle.increment; j++) { + float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset + - (float)_obstacle_map_body_frame.increment / 2.f); + float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset + + (float)_obstacle_map_body_frame.increment / 2.f); + float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg - + obstacle.increment / 2.f); + float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg + + obstacle.increment / 2.f); + + // if a bin stretches over the 0/360 degree line, adjust the angles + if (bin_lower_angle > bin_upper_angle) { + bin_lower_angle -= 360; + } + + if (msg_lower_angle > msg_upper_angle) { + msg_lower_angle -= 360; } + + // Check for overlaps. + if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) || + (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) || + (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) || + (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) { + if (obstacle.distances[j] != UINT16_MAX) { + if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[j]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + _data_maxranges[i] = obstacle.max_distance; + _data_fov[i] = 1; + } + } + } + } } @@ -256,18 +280,39 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst // Obstacle message arrives in body frame (front aligned) // corresponding data index (shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { - float bin_angle_deg = (float)i * BIN_SIZE + - _obstacle_map_body_frame.angle_offset; - msg_index = ceil(_wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); - - //add all data points inside to FOV - if (obstacle.distances[msg_index] != UINT16_MAX) { - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { - _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; - _data_timestamps[i] = _obstacle_map_body_frame.timestamp; - _data_maxranges[i] = obstacle.max_distance; - _data_fov[i] = 1; + for (int j = 0; j < 360 / obstacle.increment; j++) { + float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset + - (float)_obstacle_map_body_frame.increment / 2.f); + float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset + + (float)_obstacle_map_body_frame.increment / 2.f); + float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f); + float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f); + + // if a bin stretches over the 0/360 degree line, adjust the angles + if (bin_lower_angle > bin_upper_angle) { + bin_lower_angle -= 360; } + + if (msg_lower_angle > msg_upper_angle) { + msg_lower_angle -= 360; + } + + // Check for overlaps. + if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) || + (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) || + (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) || + (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) { + if (obstacle.distances[j] != UINT16_MAX) { + + if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[j]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + _data_maxranges[i] = obstacle.max_distance; + _data_fov[i] = 1; + } + } + } + } } @@ -357,18 +402,14 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices - int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - - // floor values above zero, ceil values below zero - if (lower_bound < 0) { lower_bound++; } + int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); + int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - if (upper_bound < 0) { upper_bound++; } // rotate vehicle attitude into the sensor body frame 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()); + float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); // verify if (distance_reading < distance_sensor.max_distance) { distance_reading = distance_reading * sensor_dist_scale; diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 0023f7716c27..80500ecc34ef 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -80,6 +80,9 @@ class CollisionPrevention : public ModuleParams */ void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); + static constexpr int BIN_COUNT = 72; + static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly + protected: /** Aggregates the sensor data into an internal obstacle map in body frame */ void _updateObstacleMap(); @@ -90,9 +93,6 @@ class CollisionPrevention : public ModuleParams /** 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 - obstacle_distance_s _obstacle_map_body_frame{}; bool _data_fov[BIN_COUNT] {}; uint64_t _data_timestamps[BIN_COUNT] {}; @@ -179,7 +179,7 @@ class CollisionPrevention : public ModuleParams float _vehicle_yaw{0.f}; uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ - uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ + uORB::Publication _obstacle_distance_fused_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 955293dad5ef..aa98ef821d0a 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -38,6 +38,8 @@ using namespace matrix; // to run: make tests TESTFILTER=CollisionPrevention hrt_abstime mocked_time = 0; +const uint bin_size = CollisionPrevention::BIN_SIZE; +const uint bin_count = CollisionPrevention::BIN_COUNT; class CollisionPreventionTest : public ::testing::Test { @@ -246,6 +248,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) 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); @@ -609,14 +612,14 @@ TEST_F(CollisionPreventionTest, jerkLimit) Vector2f modified_setpoint_limited_jerk = original_setpoint; 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()); + // THEN: the new setpoint should be much higher than the one with default jerk, as the rate of change in acceleration is more limmited + EXPECT_GT(modified_setpoint_limited_jerk.norm(), 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; Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform distance_sensor_s distance_sensor {}; distance_sensor.min_distance = 0.2f; @@ -641,11 +644,65 @@ TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData) } } +TEST_F(CollisionPreventionTest, addDistanceSensorDataNarrow) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion 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.current_distance = 5.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + distance_sensor.h_fov = math::radians(0.1 * bin_size); + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + // WHEN the sensor has a very narrow field of view + 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], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + } +} +TEST_F(CollisionPreventionTest, addDistanceSensorDataSlightlyLarger) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion 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.current_distance = 5.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + distance_sensor.h_fov = math::radians(1.1 * bin_size); + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + // WHEN the sensor has a very narrow field of view + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the the bins corresponding to -5°, 0° and 5° should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 71 || i <= 1) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + } +} + TEST_F(CollisionPreventionTest, addDistanceSensorData) { // GIVEN: a vehicle attitude and a distance sensor message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform distance_sensor_s distance_sensor {}; distance_sensor.min_distance = 0.2f; @@ -656,21 +713,24 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); for (uint32_t i = 0; i < distances_array_size; i++) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //WHEN: we add distance sensor data to the right distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING; distance_sensor.h_fov = math::radians(19.99f); cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + uint fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start = (90 - fov) / bin_size; + uint end = (90 + fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -679,17 +739,20 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) distance_sensor.h_fov = math::radians(50.f); distance_sensor.current_distance = 8.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start2 = (270 - fov) / bin_size; + uint end2 = (270 + fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; - } else if (i >= 24 && i <= 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + } else if (i >= start2 && i <= end2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -698,20 +761,23 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) distance_sensor.h_fov = math::radians(10.1f); distance_sensor.current_distance = 3.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start3 = (360 - fov) / bin_size; + uint end3 = (fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; - } else if (i >= 24 && i <= 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + } else if (i >= start2 && i <= end2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i; - } else if (i == 0) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300); + } else if (i >= start3 || i <= end3) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -722,7 +788,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned obstacle_msg.increment = 5.f; @@ -733,11 +798,15 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) //obstacle at 10-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 2; i < 6 ; i++) { + int start = 2; + int end = 6; + + for (int i = start; i <= end ; i++) { obstacle_msg.distances[i] = 500; } + //THEN: at initialization the internal obstacle map should only contain UINT16_MAX int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); @@ -750,11 +819,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -762,44 +831,50 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } - //WHEN: we add distance sensor data while vehicle yaw 90deg to the right + //WHEN: we add obstacle distance sensor data while vehicle yaw 90deg to the right cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled + int offset = bin_count - 90 / bin_size; + for (int i = 0; i < distances_array_size; i++) { - if (i == 28 || i == 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= offset + start && i <= offset + end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data while vehicle yaw 45deg to the left + //WHEN: we add obstacle distance sensor data while vehicle yaw 45deg to the left cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled + offset = 45 / bin_size; + for (int i = 0; i < distances_array_size; i++) { - if (i == 6 || i == 7) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= offset + start && i <= offset + end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data while vehicle yaw 180deg + //WHEN: we add obstacle distance sensor data while vehicle yaw 180deg cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled + offset = 180 / bin_size; + for (int i = 0; i < distances_array_size; i++) { - if (i == 19 || i == 20) { + if (i >= offset + start && i <= offset + end) { EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); } else { @@ -811,11 +886,84 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } } +TEST_F(CollisionPreventionTest, addObstacleSensorData_offset_bodyframe) +{ + // GIVEN: a vehicle attitude and obstacle distance message + TestCollisionPrevention cp; + obstacle_distance_s obstacle_msg {}; + obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; // Body Frame + obstacle_msg.increment = 6.f; + obstacle_msg.min_distance = 20; + obstacle_msg.max_distance = 2000; + obstacle_msg.angle_offset = 0.f; + + //obstacle at 363°-39° deg world frame, distance 5 meters + memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + + for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment + obstacle_msg.distances[i] = 500; + } + + //WHEN: we add distance sensor data + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5° + int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 0 && i <= 8) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5° + obstacle_msg.angle_offset = 30.5f; + // This then means our obstacle is between 27.5° and 69.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5° + for (int i = 0; i < distances_array_size; i++) { + if (i >= 6 && i <= 14) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we increase the offset to -30.5° + obstacle_msg.angle_offset = -30.5f; + // This then means our obstacle is between 326.5° and 8.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5° + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 65 || i <= 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } +} + TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; //north aligned obstacle_msg.increment = 5.f; @@ -825,8 +973,10 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) //obstacle at 10-30 deg body frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + int start = 2; + int end = 6; - for (int i = 2; i < 6 ; i++) { + for (int i = start; i <= end ; i++) { obstacle_msg.distances[i] = 500; } @@ -842,12 +992,13 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -859,11 +1010,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -875,11 +1026,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -891,16 +1042,17 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } + } @@ -908,7 +1060,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned obstacle_msg.increment = 6.f; @@ -916,42 +1067,62 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - //obstacle at 0-30 deg world frame, distance 5 meters + //obstacle at 363°-39° deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 0; i < 5 ; i++) { + for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment obstacle_msg.distances[i] = 500; } //WHEN: we add distance sensor data cp.test_addObstacleSensorData(obstacle_msg, 0.f); - //THEN: the correct bins in the map should be filled + //THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5° int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); for (int i = 0; i < distances_array_size; i++) { - if (i >= 0 && i <= 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 0 && i <= 8) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data with an angle offset - obstacle_msg.angle_offset = 30.f; + //WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5° + obstacle_msg.angle_offset = 30.5f; + // This then means our obstacle is between 27.5° and 69.5° cp.test_addObstacleSensorData(obstacle_msg, 0.f); - //THEN: the correct bins in the map should be filled + //THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5° for (int i = 0; i < distances_array_size; i++) { - if (i >= 3 && i <= 5) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 6 && i <= 14) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we increase the offset to -30.5° + obstacle_msg.angle_offset = -30.5f; + // This then means our obstacle is between 326.5° and 8.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5° + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 65 || i <= 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -963,10 +1134,9 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned - obstacle_msg.increment = 10.f; + obstacle_msg.increment = 5.f; obstacle_msg.min_distance = 20; obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; @@ -976,7 +1146,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 0; i < 7 ; i++) { + for (int i = 0; i <= 6 ; i++) { obstacle_msg.distances[i] = 500; } @@ -1001,18 +1171,17 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); - EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262); - EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012); + EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f); + EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f); } TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned - obstacle_msg.increment = 10.f; + obstacle_msg.increment = 5.f; obstacle_msg.min_distance = 20; obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; @@ -1049,8 +1218,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); - EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262f); - EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012f); + EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f); + EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f); } TEST_F(CollisionPreventionTest, overlappingSensors) @@ -1146,16 +1315,15 @@ TEST_F(CollisionPreventionTest, enterData) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform //THEN: just after initialization all bins are at UINT16_MAX and any data should be accepted - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range //WHEN: we add distance sensor data to the right with a valid reading distance_sensor_s distance_sensor {}; @@ -1167,36 +1335,38 @@ TEST_F(CollisionPreventionTest, enterData) cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); //THEN: the internal map should contain the distance sensor readings - EXPECT_EQ(500, cp.getObstacleMap().distances[8]); - EXPECT_EQ(500, cp.getObstacleMap().distances[9]); + for (int i = 16; i < 20; i++) { + EXPECT_EQ(500, cp.getObstacleMap().distances[i]) << i; + } //THEN: bins 8 & 9 contain valid readings // a valid reading should only be accepted from sensors with shorter or equal range // a out of range reading should only be accepted from sensors with the same range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_FALSE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_FALSE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range //WHEN: we add distance sensor data to the right with an out of range reading distance_sensor.current_distance = 21.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); //THEN: the internal map should contain the distance sensor readings - EXPECT_EQ(2000, cp.getObstacleMap().distances[8]); - EXPECT_EQ(2000, cp.getObstacleMap().distances[9]); + for (int i = 16; i < 20; i++) { + EXPECT_EQ(2000, cp.getObstacleMap().distances[i]) << i; + } //THEN: bins 8 & 9 contain readings out of range // a reading in range will be accepted in any case // out of range readings will only be accepted from sensors with bigger or equal range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range } diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 77858bb96452..95be696835e2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -816,7 +816,7 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) { - static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each + static constexpr int SECTOR_SIZE_DEG = 5; // PX4 Collision Prevention uses 5 degree sectors double angle_min_deg = scan.angle_min() * 180 / M_PI; double angle_step_deg = scan.angle_step() * 180 / M_PI; From 27818e2ee9ff19038c94d83df01950d9d98451b4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 02:30:52 +0100 Subject: [PATCH 13/18] Remove FlightTaskManualPositionSmoothVel The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation. --- ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision | 1 - ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 | 1 - 2 files changed, 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index 090415e5ec49..d60de46e7bea 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -67,7 +67,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index a73c357437f4..c9c3e5843ac7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -68,7 +68,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters From 6eeab5ea0e66822b2676c80ac14efec646f08df6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 17:09:46 +0100 Subject: [PATCH 14/18] CollisionPrevention: fix early return and use unified bin count + array size 72 --- .../CollisionPrevention.cpp | 17 ++++++----------- .../CollisionPrevention.hpp | 3 ++- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 324be7357f40..801e29e34da7 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -47,18 +47,13 @@ CollisionPrevention::CollisionPrevention(ModuleParams *parent) : { static_assert(BIN_SIZE >= 5, "BIN_SIZE must be at least 5"); static_assert(360 % BIN_SIZE == 0, "BIN_SIZE must divide 360 evenly"); - static_assert(sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]) - >= BIN_COUNT, "BIN_COUNT must not overflow obstacle_distance.distances"); // initialize internal obstacle map _obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; _obstacle_map_body_frame.increment = BIN_SIZE; _obstacle_map_body_frame.min_distance = UINT16_MAX; - static constexpr int BIN_COUNT_EXTERNAL = - sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); - - for (uint32_t i = 0 ; i < BIN_COUNT_EXTERNAL; i++) { + for (uint32_t i = 0 ; i < BIN_COUNT; i++) { _obstacle_map_body_frame.distances[i] = UINT16_MAX; } } @@ -523,6 +518,7 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) { + float obstacle_distance = 0.f; const float direction_norm = direction.norm(); if (direction_norm > FLT_EPSILON) { @@ -531,12 +527,11 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) 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 / BIN_SIZE); - dir_index = math::constrain(dir_index, 0, 71); - return _obstacle_map_body_frame.distances[dir_index] * 0.01f; - - } else { - return 0.f; + dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1); + obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f; } + + return obstacle_distance; } Vector2f diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 80500ecc34ef..176bcf46cda6 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -80,7 +80,8 @@ class CollisionPrevention : public ModuleParams */ void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); - static constexpr int BIN_COUNT = 72; + static constexpr int BIN_COUNT = + sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]); // 72 static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly protected: From 36dd39a3c2493273e405ec5c5d9a5e33a014b36b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 19 Nov 2024 19:59:09 +0100 Subject: [PATCH 15/18] CollisionPrevention: slightly simplify _transformSetpoint() --- src/lib/collision_prevention/CollisionPrevention.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 801e29e34da7..534426a30475 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -371,12 +371,12 @@ CollisionPrevention::_checkSetpointDirectionFeasability() void CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) { - _setpoint_dir = setpoint / setpoint.norm();; - const float sp_angle_body_frame = atan2f(_setpoint_dir(1), _setpoint_dir(0)) - _vehicle_yaw; + const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw; 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 / BIN_SIZE); // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps + _setpoint_dir = setpoint.unit_or_zero(); _adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw); } From 08f2f39fd5d59e44e64d1b4c059bdeae37650a39 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 19 Nov 2024 20:42:12 +0100 Subject: [PATCH 16/18] CollisionPrevention: follow parameter variable naming convention --- src/lib/collision_prevention/CollisionPrevention.cpp | 6 +++--- src/lib/collision_prevention/CollisionPrevention.hpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 534426a30475..7d34097644f2 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -358,8 +358,8 @@ CollisionPrevention::_checkSetpointDirectionFeasability() for (int i = 0; i < BIN_COUNT; 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]))) { + if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_no_data.get() + || (_param_cp_go_no_data.get() && _data_fov[i]))) { setpoint_feasible = false; } @@ -605,7 +605,7 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic 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); + const float curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * (max_vel - curr_vel_parallel); if (curr_acc_vel_constraint < vel_comp_accel) { vel_comp_accel = curr_acc_vel_constraint; diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 176bcf46cda6..8fcc7c28f3ec 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -196,11 +196,11 @@ class CollisionPrevention : public ModuleParams (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*/ + (ParamBool) _param_cp_go_no_data, /**< 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_xy_vel_p_acc, /**< p gain from velocity controller*/ (ParamFloat) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/ ) From ec1efcc8cb7ad3cbfc23afa0f1fd8ccbaa9f3cdf Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 20 Nov 2024 10:47:02 +0100 Subject: [PATCH 17/18] CollisionPrevention: prevent illegal array index with malicious obstacle_dsitance message --- src/lib/collision_prevention/CollisionPrevention.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 7d34097644f2..691f13c4dffb 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -234,7 +234,7 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst // Obstacle message arrives in local_origin frame (north aligned) // corresponding data index (convert to world frame and shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { - for (int j = 0; j < 360 / obstacle.increment; j++) { + for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) { float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - (float)_obstacle_map_body_frame.increment / 2.f); float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset From f05f032e76e266e988500261295873d4b6391a93 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 20 Nov 2024 16:39:18 +0100 Subject: [PATCH 18/18] CollisionPrevention: Added Case where velocity gets reduced to zero if we are closer to the obstacle than the minimal distance --- .../collision_prevention/CollisionPrevention.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 691f13c4dffb..0733f1b289d4 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -600,12 +600,18 @@ void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehic 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 stop_distance = 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_xy_vel_p_acc.get() * (max_vel - curr_vel_parallel); + float curr_acc_vel_constraint; + + if (stop_distance >= 0.f) { + const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), + _param_mpc_acc_hor.get(), stop_distance, 0.f); + curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * math::min(max_vel - curr_vel_parallel, 0.f); + + } else { + curr_acc_vel_constraint = -1.f * _param_mpc_xy_vel_p_acc.get() * curr_vel_parallel; + } if (curr_acc_vel_constraint < vel_comp_accel) { vel_comp_accel = curr_acc_vel_constraint;