fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 12:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Collision Prevention: don't allow moving outside sensor FOV (#12741)

Browse Source 
* don't allow moving outside FOV

* Update parameters in tests
This commit is contained in:
Tanja Baumann
2019-08-20 17:01:13 +02:00
committed by Julian Kent
parent d38dfcfcd3
commit be233f6bc7
2 changed files with 148 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+33 -15
View File
@@ -205,6 +205,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
_updateDistanceSensor(obstacle);
float setpoint_length = setpoint.norm();
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();
if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) {
if (setpoint_length > 0.001f) {
@@ -212,11 +216,12 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
Vector2f setpoint_dir = setpoint / setpoint_length;
float vel_max = setpoint_length;
int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);
float min_dist_to_keep = math::max(obstacle.min_distance / 100.0f, col_prev_d);
for (int i = 0; i < distances_array_size; i++) {
if (obstacle.distances[i] < obstacle.max_distance &&
obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
if ((float)i * obstacle.increment < 360.f) { //disregard unused bins at the end of the message
float distance = obstacle.distances[i] / 100.0f; //convert to meters
float angle = math::radians((float)i * obstacle.increment);
@@ -224,23 +229,36 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
angle += math::radians(obstacle.angle_offset);
}
//check if the bin must be considered regarding the given stick input
//get direction of current bin
Vector2f bin_direction = {cos(angle), sin(angle)};
if (setpoint_dir.dot(bin_direction) > 0
&& setpoint_dir.dot(bin_direction) > cosf(math::radians(_param_mpc_col_prev_ang.get()))) {
//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 * _param_mpc_col_prev_dly.get();
float vel_max_posctrl = math::max(0.f,
_param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance));
Vector2f vel_max_vec = bin_direction * vel_max_posctrl;
float vel_max_bin = vel_max_vec.dot(setpoint_dir);
if (obstacle.distances[i] < obstacle.max_distance &&
obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
//constrain the velocity
if (vel_max_bin >= 0) {
vel_max = math::min(vel_max, vel_max_bin);
if (setpoint_dir.dot(bin_direction) > 0
&& setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) {
//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;
float vel_max_posctrl = math::max(0.f, xy_p * (distance - min_dist_to_keep - delay_distance));
Vector2f vel_max_vec = bin_direction * vel_max_posctrl;
float vel_max_bin = vel_max_vec.dot(setpoint_dir);
//constrain the velocity
if (vel_max_bin >= 0) {
vel_max = math::min(vel_max, vel_max_bin);
}
}
} else if (obstacle.distances[i] == UINT16_MAX) {
float sp_bin = setpoint_dir.dot(bin_direction);
float ang_half_bin = cosf(math::radians(obstacle.increment) / 2.f);
//if the setpoint lies outside the FOV set velocity to zero
if (sp_bin > ang_half_bin) {
vel_max = 0.f;
}
}
}
}
src/lib/CollisionPrevention/CollisionPreventionTest.cpp
+115 -8
View File
@@ -52,12 +52,19 @@ public:
}
};
class TestCollisionPrevention : public CollisionPrevention
{
public:
TestCollisionPrevention() : CollisionPrevention(nullptr) {}
void paramsChanged() {CollisionPrevention::updateParamsImpl();}
};
TEST_F(CollisionPreventionTest, instantiation) { CollisionPrevention cp(nullptr); }
TEST_F(CollisionPreventionTest, behaviorOff)
{
// GIVEN: a simple setup condition
CollisionPrevention cp(nullptr);
TestCollisionPrevention cp;
matrix::Vector2f original_setpoint(10, 0);
float max_speed = 3.f;
matrix::Vector2f curr_pos(0, 0);
@@ -74,7 +81,7 @@ TEST_F(CollisionPreventionTest, behaviorOff)
TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing)
{
// GIVEN: a simple setup condition
CollisionPrevention cp(nullptr);
TestCollisionPrevention cp;
matrix::Vector2f original_setpoint(10, 0);
float max_speed = 3.f;
matrix::Vector2f curr_pos(0, 0);
@@ -87,6 +94,7 @@ TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing)
// WHEN: we set the parameter check then apply the setpoint modification
float value = 10; // try to keep 10m away from obstacles
param_set(param, &value);
cp.paramsChanged();
matrix::Vector2f modified_setpoint = original_setpoint;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
@@ -98,7 +106,7 @@ TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing)
TEST_F(CollisionPreventionTest, testBehaviorOnWithAnObstacle)
{
// GIVEN: a simple setup condition
CollisionPrevention cp(nullptr);
TestCollisionPrevention cp;
matrix::Vector2f original_setpoint(10, 0);
float max_speed = 3;
matrix::Vector2f curr_pos(0, 0);
@@ -108,6 +116,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithAnObstacle)
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
obstacle_distance_s message;
@@ -122,14 +131,112 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithAnObstacle)
message.distances[i] = 101;
}
orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
// 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;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
orb_unadvertise(obstacle_distance_pub);
// THEN: it should be cut down a lot
EXPECT_GT(original_setpoint.norm() * 0.5f, modified_setpoint.norm()); //FIXME: this should actually be constrained to 0
// THEN: it should be cut down to zero
EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1);
}
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::MPC_COL_PREV_D);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
// AND: an obstacle message
obstacle_distance_s message;
memset(&message, 0xDEAD, sizeof(message));
message.min_distance = 100;
message.max_distance = 2000;
message.timestamp = hrt_absolute_time();
int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
message.increment = 360 / distances_array_size;
for (int i = 0; i < distances_array_size; i++) {
message.distances[i] = 700;
}
// 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;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
orb_unadvertise(obstacle_distance_pub);
// THEN: setpoint should go into the same direction as the stick input
EXPECT_FLOAT_EQ(original_setpoint.normalized()(0), modified_setpoint.normalized()(0));
EXPECT_FLOAT_EQ(original_setpoint.normalized()(1), modified_setpoint.normalized()(1));
}
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
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
// AND: an obstacle message
obstacle_distance_s message;
memset(&message, 0xDEAD, sizeof(message));
message.min_distance = 100;
message.max_distance = 2000;
int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
message.increment = 360 / distances_array_size;
//fov from i = 1/4 * distances_array_size to i = 3/4 * distances_array_size
for (int i = 0; i < distances_array_size; i++) {
if (i > 0.25f * distances_array_size && i < 0.75f * distances_array_size) {
message.distances[i] = 700;
} else {
message.distances[i] = UINT16_MAX;
}
}
// WHEN: we publish the message and modify the setpoint for different demanded setpoints
orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
for (int i = 0; i < distances_array_size; i++) {
float angle = math::radians((float)i * message.increment);
matrix::Vector2f original_setpoint = {10.f *(float)cos(angle), 10.f *(float)sin(angle)};
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);
if (i > 0.25f * distances_array_size && i < 0.75f * distances_array_size) {
// THEN: inside the FOV the setpoint should be limited
EXPECT_GT(modified_setpoint.norm(), 0.f);
EXPECT_LT(modified_setpoint.norm(), 10.f);
} else {
// THEN: outside the FOV the setpoint should be clamped to zero
EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f);
}
}
orb_unadvertise(obstacle_distance_pub);
}