mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Guidance feature for Collision Prevention (#13017)
* add guidance * remove COL_PREV_ANG and replace with COL_PREV_CNG * safe max ranges per bin * increase default value for colprev delay to account for tracking delay * update parameter description * fix and extend testing * add handling for overlapping sensor data * fix decision process for overlapping sensors
This commit is contained in:
Julian Kent
GitHub
parent
6139812293
commit
07d656e971
@ -79,6 +79,7 @@ CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
|
||||
|
||||
for (uint32_t i = 0 ; i < internal_bins; i++) {
|
||||
_data_timestamps[i] = current_time;
|
||||
_data_maxranges[i] = 0;
|
||||
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
|
||||
}
|
||||
}
|
||||
@ -118,8 +119,11 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
|
||||
|
||||
//add all data points inside to FOV
|
||||
if (obstacle.distances[msg_index] != UINT16_MAX) {
|
||||
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
|
||||
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -134,8 +138,12 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
|
||||
|
||||
//add all data points inside to FOV
|
||||
if (obstacle.distances[msg_index] != UINT16_MAX) {
|
||||
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
|
||||
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -146,6 +154,37 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_reading)
|
||||
{
|
||||
//use data from this sensor if:
|
||||
//1. this sensor data is in range, the bin contains already valid data and this data is coming from the same or less range sensor
|
||||
//2. this sensor data is in range, and the last reading was out of range
|
||||
//3. this sensor data is out of range, the last reading was as well and this is the sensor with longest range
|
||||
//4. this sensor data is out of range, the last reading was valid and coming from the same sensor
|
||||
|
||||
uint16_t sensor_range_cm = (int)(100 * sensor_range); //convert to cm
|
||||
|
||||
if (sensor_reading < sensor_range) {
|
||||
if ((_obstacle_map_body_frame.distances[map_index] < _data_maxranges[map_index]
|
||||
&& sensor_range_cm <= _data_maxranges[map_index])
|
||||
|| _obstacle_map_body_frame.distances[map_index] >= _data_maxranges[map_index]) {
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
} else {
|
||||
if ((_obstacle_map_body_frame.distances[map_index] >= _data_maxranges[map_index]
|
||||
&& sensor_range_cm >= _data_maxranges[map_index])
|
||||
|| (_obstacle_map_body_frame.distances[map_index] < _data_maxranges[map_index]
|
||||
&& sensor_range_cm == _data_maxranges[map_index])) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void CollisionPrevention::_updateObstacleMap()
|
||||
{
|
||||
_sub_vehicle_attitude.update();
|
||||
@ -223,16 +262,66 @@ void CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sen
|
||||
attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
|
||||
float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
|
||||
|
||||
if (distance_reading < distance_sensor.max_distance) {
|
||||
distance_reading = distance_reading * sensor_dist_scale;
|
||||
}
|
||||
|
||||
uint16_t sensor_range = (int)(100 * distance_sensor.max_distance); //convert to cm
|
||||
|
||||
for (int bin = lower_bound; bin <= upper_bound; ++bin) {
|
||||
int wrapped_bin = wrap_bin(bin);
|
||||
|
||||
// compensate measurement for vehicle tilt and convert to cm
|
||||
_obstacle_map_body_frame.distances[wrapped_bin] = (int)(100 * distance_reading * sensor_dist_scale);
|
||||
_data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp;
|
||||
if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) {
|
||||
_obstacle_map_body_frame.distances[wrapped_bin] = (int)(100 * distance_reading);
|
||||
_data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp;
|
||||
_data_maxranges[wrapped_bin] = sensor_range;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index,
|
||||
float vehicle_yaw_angle_rad)
|
||||
{
|
||||
float col_prev_d = _param_mpc_col_prev_d.get();
|
||||
int guidance_bins = floor(_param_mpc_col_prev_cng.get() / INTERNAL_MAP_INCREMENT_DEG);
|
||||
int sp_index_original = setpoint_index;
|
||||
float best_cost = 9999.f;
|
||||
|
||||
for (int i = sp_index_original - guidance_bins; i <= sp_index_original + guidance_bins; i++) {
|
||||
|
||||
//apply moving average filter to the distance array to be able to center in larger gaps
|
||||
int filter_size = 1;
|
||||
float mean_dist = 0;
|
||||
|
||||
for (int j = i - filter_size; j <= i + filter_size; j++) {
|
||||
int bin = wrap_bin(j);
|
||||
|
||||
if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
|
||||
mean_dist += col_prev_d * 100.f;
|
||||
|
||||
} else {
|
||||
mean_dist += _obstacle_map_body_frame.distances[bin];
|
||||
}
|
||||
}
|
||||
|
||||
int bin = wrap_bin(i);
|
||||
mean_dist = mean_dist / (2.f * filter_size + 1.f);
|
||||
float deviation_cost = col_prev_d * 50.f * std::abs(i - sp_index_original);
|
||||
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) {
|
||||
best_cost = bin_cost;
|
||||
|
||||
float angle = math::radians((float)bin * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
|
||||
angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
|
||||
setpoint_dir = {cosf(angle), sinf(angle)};
|
||||
setpoint_index = bin;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
|
||||
const Vector2f &curr_pos, const Vector2f &curr_vel)
|
||||
@ -242,7 +331,6 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
|
||||
//read parameters
|
||||
float col_prev_d = _param_mpc_col_prev_d.get();
|
||||
float col_prev_dly = _param_mpc_col_prev_dly.get();
|
||||
float col_prev_ang_rad = math::radians(_param_mpc_col_prev_ang.get());
|
||||
float xy_p = _param_mpc_xy_p.get();
|
||||
float max_jerk = _param_mpc_jerk_max.get();
|
||||
float max_accel = _param_mpc_acc_hor.get();
|
||||
@ -264,6 +352,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
|
||||
float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
|
||||
int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
|
||||
|
||||
//change setpoint direction slightly (max by _param_mpc_col_prev_cng degrees) to help guide through narrow gaps
|
||||
_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
|
||||
|
||||
//limit speed for safe flight
|
||||
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { //disregard unused bins at the end of the message
|
||||
|
||||
//delete stale values
|
||||
@ -274,6 +366,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
|
||||
}
|
||||
|
||||
float distance = _obstacle_map_body_frame.distances[i] * 0.01f; //convert to meters
|
||||
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]
|
||||
@ -285,16 +378,25 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
|
||||
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
|
||||
&& setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) {
|
||||
if (setpoint_dir.dot(bin_direction) > 0) {
|
||||
//calculate max allowed velocity with a P-controller (same gain as in the position controller)
|
||||
float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
|
||||
float delay_distance = curr_vel_parallel * (col_prev_dly + data_age * 1e-6f);
|
||||
float delay_distance = curr_vel_parallel * col_prev_dly;
|
||||
|
||||
if (distance < max_range) {
|
||||
delay_distance += curr_vel_parallel * (data_age * 1e-6f);
|
||||
}
|
||||
|
||||
float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
|
||||
float vel_max_posctrl = xy_p * stop_distance;
|
||||
|
||||
float vel_max_smooth = math::trajectory::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
|
||||
Vector2f vel_max_vec = bin_direction * math::min(vel_max_posctrl, vel_max_smooth);
|
||||
float vel_max_bin = vel_max_vec.dot(setpoint_dir);
|
||||
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) {
|
||||
@ -311,7 +413,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
|
||||
}
|
||||
|
||||
} else {
|
||||
// if distance data are stale, switch to Loiter
|
||||
// if distance data is stale, switch to Loiter
|
||||
_publishVehicleCmdDoLoiter();
|
||||
mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
|
||||
}
|
||||
|
||||
@ -84,8 +84,10 @@ protected:
|
||||
|
||||
obstacle_distance_s _obstacle_map_body_frame {};
|
||||
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 */
|
||||
|
||||
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude);
|
||||
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude);
|
||||
|
||||
/**
|
||||
* Updates obstacle distance message with measurement from offboard
|
||||
@ -93,6 +95,24 @@ protected:
|
||||
*/
|
||||
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude);
|
||||
|
||||
/**
|
||||
* Computes an adaption to the setpoint direction to guide towards free space
|
||||
* @param setpoint_dir, setpoint direction before collision prevention intervention
|
||||
* @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);
|
||||
|
||||
/**
|
||||
* Determines whether a new sensor measurement is used
|
||||
* @param map_index, index of the bin in the internal map the measurement belongs in
|
||||
* @param sensor_range, max range of the sensor in meters
|
||||
* @param sensor_reading, distance measurement in meters
|
||||
*/
|
||||
bool _enterData(int map_index, float sensor_range, float sensor_reading);
|
||||
|
||||
|
||||
//Timing functions. Necessary to mock time in the tests
|
||||
virtual hrt_abstime getTime();
|
||||
virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr);
|
||||
|
||||
@ -115,7 +135,7 @@ private:
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */
|
||||
(ParamFloat<px4::params::MPC_COL_PREV_ANG>) _param_mpc_col_prev_ang, /**< collision prevention detection angle */
|
||||
(ParamFloat<px4::params::MPC_COL_PREV_CNG>) _param_mpc_col_prev_cng, /**< collision prevention change setpoint angle */
|
||||
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
|
||||
(ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly, /**< delay of the range measurement data*/
|
||||
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
|
||||
@ -153,8 +173,6 @@ private:
|
||||
return offset;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Computes collision free setpoints
|
||||
* @param setpoint, setpoint before collision prevention intervention
|
||||
|
||||
@ -60,6 +60,15 @@ public:
|
||||
{
|
||||
_addObstacleSensorData(obstacle, attitude);
|
||||
}
|
||||
void test_adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index,
|
||||
float vehicle_yaw_angle_rad)
|
||||
{
|
||||
_adaptSetpointDirection(setpoint_dir, setpoint_index, vehicle_yaw_angle_rad);
|
||||
}
|
||||
bool test_enterData(int map_index, float sensor_range, float sensor_reading)
|
||||
{
|
||||
return _enterData(map_index, sensor_range, sensor_reading);
|
||||
}
|
||||
};
|
||||
|
||||
class TestTimingCollisionPrevention : public TestCollisionPrevention
|
||||
@ -176,7 +185,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage)
|
||||
EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
|
||||
|
||||
EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1);
|
||||
EXPECT_EQ(original_setpoint2, modified_setpoint2);
|
||||
EXPECT_FLOAT_EQ(original_setpoint2.norm(), modified_setpoint2.norm());
|
||||
}
|
||||
|
||||
TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
|
||||
@ -303,7 +312,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
|
||||
|
||||
if (i < 6) {
|
||||
// THEN: If the data is new enough, the velocity setpoint should stay the same as the input
|
||||
EXPECT_EQ(original_setpoint, modified_setpoint);
|
||||
// Note: direction will change slightly due to guidance
|
||||
EXPECT_EQ(original_setpoint.norm(), modified_setpoint.norm());
|
||||
|
||||
} else {
|
||||
// THEN: If the data is expired, the velocity setpoint should be cut down to zero because there is no data
|
||||
@ -404,16 +414,16 @@ TEST_F(CollisionPreventionTest, outsideFOV)
|
||||
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
|
||||
if (angle_deg > 50.f && angle_deg < 230.f) {
|
||||
// THEN: inside the FOV the setpoint should be limited
|
||||
EXPECT_GT(modified_setpoint.norm(), 0.f);
|
||||
EXPECT_LT(modified_setpoint.norm(), 10.f);
|
||||
//THEN: if the resulting setpoint demands velocities bigger zero, it must lie inside the FOV
|
||||
float setpoint_length = modified_setpoint.norm();
|
||||
|
||||
} else {
|
||||
// THEN: outside the FOV the setpoint should be clamped to zero
|
||||
EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f);
|
||||
if (setpoint_length > 0.f) {
|
||||
matrix::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));
|
||||
EXPECT_GT(sp_angle_deg, 45.f);
|
||||
EXPECT_LT(sp_angle_deg, 225.f);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
orb_unadvertise(obstacle_distance_pub);
|
||||
@ -804,3 +814,249 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset)
|
||||
cp.getObstacleMap().distances[i] = UINT16_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
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.min_distance = 20;
|
||||
obstacle_msg.max_distance = 2000;
|
||||
obstacle_msg.angle_offset = 0.f;
|
||||
|
||||
matrix::Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
|
||||
float vehicle_yaw_angle_rad = matrix::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));
|
||||
|
||||
for (int i = 0; i < 7 ; i++) {
|
||||
obstacle_msg.distances[i] = 500;
|
||||
}
|
||||
|
||||
obstacle_msg.distances[2] = 1000;
|
||||
|
||||
//define setpoint
|
||||
matrix::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);
|
||||
int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment);
|
||||
|
||||
//set parameter
|
||||
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
|
||||
float value = 3; // try to keep 10m away from obstacles
|
||||
param_set(param, &value);
|
||||
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);
|
||||
|
||||
//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);
|
||||
}
|
||||
|
||||
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.min_distance = 20;
|
||||
obstacle_msg.max_distance = 2000;
|
||||
obstacle_msg.angle_offset = 0.f;
|
||||
|
||||
matrix::Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
|
||||
float vehicle_yaw_angle_rad = matrix::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));
|
||||
|
||||
for (int i = 0; i < 7 ; i++) {
|
||||
obstacle_msg.distances[i] = 500;
|
||||
}
|
||||
|
||||
obstacle_msg.distances[1] = 1000;
|
||||
obstacle_msg.distances[2] = 1000;
|
||||
obstacle_msg.distances[3] = 1000;
|
||||
|
||||
//define setpoint
|
||||
matrix::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);
|
||||
int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment);
|
||||
|
||||
//set parameter
|
||||
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
|
||||
float value = 3; // try to keep 10m away from obstacles
|
||||
param_set(param, &value);
|
||||
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);
|
||||
|
||||
//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);
|
||||
}
|
||||
|
||||
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();
|
||||
attitude.q[0] = 1.0f;
|
||||
attitude.q[1] = 0.0f;
|
||||
attitude.q[2] = 0.0f;
|
||||
attitude.q[3] = 0.0f;
|
||||
|
||||
// AND: a parameter handle
|
||||
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
|
||||
float value = 10; // try to keep 10m distance
|
||||
param_set(param, &value);
|
||||
cp.paramsChanged();
|
||||
|
||||
// AND: an obstacle message for a short range and a long range sensor
|
||||
obstacle_distance_s short_range_msg, short_range_msg_no_obstacle, long_range_msg;
|
||||
memset(&short_range_msg, 0xDEAD, sizeof(short_range_msg));
|
||||
short_range_msg.frame = short_range_msg.MAV_FRAME_GLOBAL; //north aligned
|
||||
short_range_msg.angle_offset = 0;
|
||||
short_range_msg.timestamp = hrt_absolute_time();
|
||||
int distances_array_size = sizeof(short_range_msg.distances) / sizeof(short_range_msg.distances[0]);
|
||||
short_range_msg.increment = 360 / distances_array_size;
|
||||
long_range_msg = short_range_msg;
|
||||
long_range_msg.min_distance = 100;
|
||||
long_range_msg.max_distance = 1000;
|
||||
short_range_msg.min_distance = 20;
|
||||
short_range_msg.max_distance = 200;
|
||||
short_range_msg_no_obstacle = short_range_msg;
|
||||
|
||||
|
||||
for (int i = 0; i < distances_array_size; i++) {
|
||||
if (i < 10) {
|
||||
short_range_msg_no_obstacle.distances[i] = 201;
|
||||
short_range_msg.distances[i] = 150;
|
||||
long_range_msg.distances[i] = 500;
|
||||
|
||||
} else {
|
||||
short_range_msg_no_obstacle.distances[i] = UINT16_MAX;
|
||||
short_range_msg.distances[i] = UINT16_MAX;
|
||||
long_range_msg.distances[i] = UINT16_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// CASE 1
|
||||
//WHEN: we publish the long range sensor message
|
||||
orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &long_range_msg);
|
||||
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;
|
||||
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
|
||||
|
||||
// THEN: the internal map data should contain the long range measurement
|
||||
EXPECT_EQ(500, cp.getObstacleMap().distances[2]);
|
||||
|
||||
// CASE 2
|
||||
// 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);
|
||||
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);
|
||||
|
||||
// THEN: the internal map data should contain the short range measurement
|
||||
EXPECT_EQ(150, cp.getObstacleMap().distances[2]);
|
||||
|
||||
// CASE 3
|
||||
// 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);
|
||||
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);
|
||||
|
||||
// THEN: the internal map data should contain the short range measurement
|
||||
EXPECT_EQ(500, cp.getObstacleMap().distances[2]);
|
||||
|
||||
orb_unadvertise(obstacle_distance_pub);
|
||||
orb_unadvertise(vehicle_attitude_pub);
|
||||
}
|
||||
|
||||
TEST_F(CollisionPreventionTest, enterData)
|
||||
{
|
||||
// GIVEN: a simple setup condition
|
||||
TestCollisionPrevention cp;
|
||||
cp.getObstacleMap().increment = 10.f;
|
||||
matrix::Quaternion<float> 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
|
||||
|
||||
//WHEN: we add distance sensor data to the right with a valid reading
|
||||
distance_sensor_s distance_sensor {};
|
||||
distance_sensor.min_distance = 0.2f;
|
||||
distance_sensor.max_distance = 20.f;
|
||||
distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING;
|
||||
distance_sensor.h_fov = math::radians(19.99f);
|
||||
distance_sensor.current_distance = 5.f;
|
||||
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]);
|
||||
|
||||
//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
|
||||
|
||||
//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]);
|
||||
|
||||
//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
|
||||
}
|
||||
|
||||
@ -42,7 +42,7 @@
|
||||
/**
|
||||
* Minimum distance the vehicle should keep to all obstacles
|
||||
*
|
||||
* Only used in Position mode. Collision avoidace is disabled by setting this parameter to a negative value
|
||||
* Only used in Position mode. Collision avoidance is disabled by setting this parameter to a negative value
|
||||
*
|
||||
* @min -1
|
||||
* @max 15
|
||||
@ -52,7 +52,7 @@
|
||||
PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);
|
||||
|
||||
/**
|
||||
* Average delay of the range sensor message in seconds
|
||||
* Average delay of the range sensor message plus the tracking delay of the position controller in seconds
|
||||
*
|
||||
* Only used in Position mode.
|
||||
*
|
||||
@ -61,10 +61,10 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);
|
||||
* @unit seconds
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f);
|
||||
PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.4f);
|
||||
|
||||
/**
|
||||
* Angle left/right from the commanded setpoint in which the range data is used to calculate speed limitations. All data further from the commanded direction is not considered.
|
||||
* Angle left/right from the commanded setpoint by which the collision prevention algorithm can choose to change the setpoint direction
|
||||
*
|
||||
* Only used in Position mode.
|
||||
*
|
||||
@ -73,4 +73,4 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f);
|
||||
* @unit [deg]
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_COL_PREV_ANG, 45.f);
|
||||
PARAM_DEFINE_FLOAT(MPC_COL_PREV_CNG, 30.f);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user