diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d4e4273b69..8dfc27edf1 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 (data_age > RANGE_STREAM_TIMEOUT_US) { - _obstacle_map_body_frame.distances[i] = UINT16_MAX; - } - - const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters - const float max_range = _data_maxranges[i] * 0.01f; // convert to meters - float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); - - // convert from body to local frame in the range [0, 2*pi] - angle = wrap_2pi(vehicle_yaw_angle_rad + angle); - - // get direction of current bin - const Vector2f bin_direction = {cosf(angle), sinf(angle)}; - - //count number of bins in the field of valid_new - if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) { - num_fov_bins ++; - } - - if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance - && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { - - if (setpoint_dir.dot(bin_direction) > 0) { - // calculate max allowed velocity with a P-controller (same gain as in the position controller) - const float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction)); - float delay_distance = curr_vel_parallel * col_prev_dly; - - if (distance < max_range) { - delay_distance += curr_vel_parallel * (data_age * 1e-6f); - } - - const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); - const float vel_max_posctrl = xy_p * stop_distance; - - const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f); - const float projection = bin_direction.dot(setpoint_dir); - float vel_max_bin = vel_max; - - if (projection > 0.01f) { - vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection; - } - - // constrain the velocity - if (vel_max_bin >= 0) { - vel_max = math::min(vel_max, vel_max_bin); - } - } - - } else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) { - if (!move_no_data || (move_no_data && _data_fov[i])) { - vel_max = 0.f; - } - } - } - - //if the sensor field of view is zero, never allow to move (even if move_no_data=1) - if (num_fov_bins == 0) { - vel_max = 0.f; - } - - setpoint = setpoint_dir * vel_max; + if (_checkSetpointDirectionFeasability()) { + constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length); } - } else { - //allow no movement - float vel_max = 0.f; - setpoint = setpoint * vel_max; + setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir; + + } else if (!_obstacle_data_present) + + { + // allow no movement + PX4_WARN("No obstacle data, not moving..."); + setpoint_accel.setZero(); // if distance data is stale, switch to Loiter if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { - - if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US - && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { + if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US && + getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { _publishVehicleCmdDoLoiter(); } _last_timeout_warning = getTime(); } + } +} + +float +CollisionPrevention::_getObstacleDistance(const Vector2f &direction) +{ + const float vehicle_yaw_angle_rad = Eulerf(Quatf(_sub_vehicle_attitude.get().q)).psi(); + Vector2f dir = direction / direction.norm(); + const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - vehicle_yaw_angle_rad; + const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - + _obstacle_map_body_frame.angle_offset); + int dir_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + + return _obstacle_map_body_frame.distances[dir_index] * 0.01f; +} + +Vector2f +CollisionPrevention::_constrainAccelerationSetpoint(const float &setpoint_length) +{ + Vector2f new_setpoint{}; + const Vector2f normal_component = _closest_dist_dir * (_setpoint_dir.dot(_closest_dist_dir)); + const Vector2f tangential_component = _setpoint_dir - normal_component; + + const float normal_scale = _getScale(_closest_dist); + const float closest_dist_tangential = _getObstacleDistance(tangential_component); + const float tangential_scale = _getScale(closest_dist_tangential); + + + // only scale accelerations towards the obstacle + if (_closest_dist_dir.dot(_setpoint_dir) > 0) { + new_setpoint = (tangential_component * tangential_scale + normal_component * normal_scale) * setpoint_length; + + } else { + new_setpoint = _setpoint_dir * setpoint_length; + } + + return new_setpoint; +} + +float +CollisionPrevention::_getScale(const float &reference_distance) +{ + float scale = (reference_distance - _min_dist_to_keep); + const float scale_distance = math::max(_min_dist_to_keep, _param_mpc_vel_manual.get() / _param_mpc_xy_p.get()); + + // if scale is positive, square it and scale it with the scale_distance + scale = scale > 0 ? powf(scale / scale_distance, 2) : scale; + scale = math::min(scale, 1.0f); + return scale; +} + +void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, + const matrix::Vector2f &setpoint_vel, + const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir) +{ + for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + const float max_range = _data_maxranges[i] * 0.01f; + + // get the vector pointing into the direction of current bin + float bin_angle = wrap_2pi(vehicle_yaw_angle_rad + math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + + _obstacle_map_body_frame.angle_offset)); + + const Vector2f bin_direction = { cosf(bin_angle), sinf(bin_angle) }; + float bin_distance = _obstacle_map_body_frame.distances[i]; + + // only consider bins which are between min and max values + if (bin_distance > _obstacle_map_body_frame.min_distance && bin_distance < UINT16_MAX) { + const float distance = bin_distance * 0.01f; + + // Assume current velocity is sufficiently close to the setpoint velocity, this breaks down if flying high + // acceleration maneuvers + const float curr_vel_parallel = math::max(0.f, setpoint_vel.dot(bin_direction)); + float delay_distance = curr_vel_parallel * _param_cp_delay.get(); + + const hrt_abstime data_age = now - _data_timestamps[i]; + + if (distance < max_range) { + delay_distance += curr_vel_parallel * (data_age * 1e-6f); + } + + const float stop_distance = math::max(0.f, distance - _min_dist_to_keep - delay_distance); + + const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), + _param_mpc_acc_hor.get(), stop_distance, 0.f); + // we dont take the minimum of the last term because of stop_distance is zero but current velocity is not, we want the acceleration to become negative and slow us down. + const float curr_acc_vel_constraint = _param_mpc_vel_p_acc.get() * (max_vel - curr_vel_parallel); + + if (curr_acc_vel_constraint < vel_comp_accel) { + vel_comp_accel = curr_acc_vel_constraint; + vel_comp_accel_dir = bin_direction; + } + } } } void -CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos, - const Vector2f &curr_vel) +CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { //calculate movement constraints based on range data - Vector2f new_setpoint = original_setpoint; - _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel); - - //warn user if collision prevention starts to interfere - bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed - || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed - || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed - || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed); - - _interfering = currently_interfering; + Vector2f original_setpoint = setpoint_accel; + _calculateConstrainedSetpoint(setpoint_accel, setpoint_vel); // publish constraints collision_constraints_s constraints{}; - constraints.timestamp = getTime(); original_setpoint.copyTo(constraints.original_setpoint); - new_setpoint.copyTo(constraints.adapted_setpoint); + setpoint_accel.copyTo(constraints.adapted_setpoint); + constraints.timestamp = getTime(); _constraints_pub.publish(constraints); - - original_setpoint = new_setpoint; } void CollisionPrevention::_publishVehicleCmdDoLoiter() diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index aaff04e2cb..651e6372f4 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 @@ public: /** * Computes collision free setpoints - * @param original_setpoint, setpoint before collision prevention intervention - * @param max_speed, maximum xy speed - * @param curr_pos, current vehicle position - * @param curr_vel, current vehicle velocity + * @param setpoint_accel setpoint purely based on sticks, to be modified + * @param setpoint_vel current velocity setpoint as information to be able to stop in time, does not get changed */ - void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, - const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); + void modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel); protected: @@ -90,13 +94,13 @@ protected: uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof( _obstacle_map_body_frame.distances[0])]; /**< in cm */ - void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude); + void _addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude); /** * Updates obstacle distance message with measurement from offboard * @param obstacle, obstacle_distance message to be updated */ - void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude); + void _addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude); /** * Computes an adaption to the setpoint direction to guide towards free space @@ -104,7 +108,40 @@ protected: * @param setpoint_index, index of the setpoint in the internal obstacle map * @param vehicle_yaw_angle_rad, vehicle orientation */ - void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); + void _adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); + + /** + * Calculate the constrained setpoint cosdering the current obstacle distances, the current acceleration setpoint and velocity setpoint + */ + void _calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel); + + /** + * Constrain the acceleration setpoint based on the distance to the obstacle + * The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above + * +1 ________ _ _ + * ┌─┐ │ // + * │X│ │ // + * │X│ │ // + * │X│ │ /// + * │X│ │ // + * │X│ │///// + * │X│──────┼─────────────┬───────────── + * │X│ /│ scale_distance + * │X│ / │ + * │X│ / │ + * │X│ / │ + * │X│ / │ + * └─┘/ │ + * -1 + */ + Vector2f _constrainAccelerationSetpoint(const float &setpoint_length); + + void _getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, const matrix::Vector2f &setpoint_vel, + const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir); + + float _getObstacleDistance(const Vector2f &direction); + + float _getScale(const float &reference_distance); /** * Determines whether a new sensor measurement is used @@ -114,6 +151,10 @@ protected: */ bool _enterData(int map_index, float sensor_range, float sensor_reading); + bool _checkSetpointDirectionFeasability(); + + void _transformSetpoint(const Vector2f &setpoint); + //Timing functions. Necessary to mock time in the tests virtual hrt_abstime getTime(); @@ -122,10 +163,20 @@ protected: private: - bool _interfering{false}; /**< states if the collision prevention interferes with the user input */ + bool _data_stale{true}; /**< states if the data is stale */ bool _was_active{false}; /**< states if the collision prevention interferes with the user input */ + bool _obstacle_data_present{false}; /**< states if obstacle data is present */ + + int _setpoint_index{}; /**< index of the setpoint*/ + Vector2f _setpoint_dir{}; /**< direction of the setpoint*/ + + float _closest_dist{}; /**< closest distance to an obstacle */ + Vector2f _closest_dist_dir{NAN, NAN}; /**< direction of the closest obstacle */ + + float _min_dist_to_keep{}; orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ + Vector2f _DEBUG; uORB::Publication _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 @@ private: 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 @@ private: * @param curr_pos, current vehicle position * @param curr_vel, current vehicle velocity */ - void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, - const matrix::Vector2f &curr_vel); + void _calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, + const Vector2f &curr_vel); /** * Publishes collision_constraints message * @param original_setpoint, setpoint before collision prevention intervention * @param adapted_setpoint, collision prevention adaped setpoint */ - void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + void _publishConstrainedSetpoint(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint); /** * Publishes obstacle_distance message with fused data from offboard and from distance sensors @@ -185,6 +238,11 @@ private: */ void _updateObstacleMap(); + /** + * Updates the obstacle data based on stale data and calculates values from the map + */ + void _updateObstacleData(); + /** * Publishes vehicle command. */ diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 9de0a208aa..45b01b2a7e 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 71dedaf056..352875764e 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 c0fe9b315b..e9e9ef0dbf 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 8bc2bb7531..9fff1da1c4 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 @@ public: void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); }; private: + CollisionPrevention _collision_prevention{this}; + void applyJerkLimit(const float dt); matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy, const matrix::Vector2f &vel_sp); diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 358a4e7032..a094298fab 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