Skip to content

Commit

Permalink
Collision prevention: Option to enable flying outside FOV and rename …
Browse files Browse the repository at this point in the history 
…parameters (new CP group)

* rename parameters to allow more descriptive names under CP group
* add option to enable moving where there is no data
* add test for param CP_GO_NO_DATA

Co-Authored-By: Martina Rivizzigno <martina@rivizzigno.it>
  • Loading branch information
@baumanta @mrivi @dagar
2 people authored and dagar committed Oct 17, 2019
1 parent 2fcddd9 commit be1f966
Show file tree
Hide file tree
Showing 5 changed files with 69 additions and 24 deletions.
13 changes: 7 additions & 6 deletions src/lib/CollisionPrevention/CollisionPrevention.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -285,8 +285,8 @@ 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_mpc_col_prev_d.get();
const int guidance_bins = floor(_param_mpc_col_prev_cng.get() / INTERNAL_MAP_INCREMENT_DEG);
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;

Expand Down Expand Up @@ -376,8 +376,9 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
_updateObstacleMap();

// read parameters
const float col_prev_d = _param_mpc_col_prev_d.get();
const float col_prev_dly = _param_mpc_col_prev_dly.get();
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() > 0;
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();
Expand All @@ -400,7 +401,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
_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
// 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);

// limit speed for safe flight
Expand Down Expand Up @@ -452,7 +453,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
}
}

} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index && (!move_no_data)) {
vel_max = 0.f;
}
}
Expand Down
9 changes: 5 additions & 4 deletions src/lib/CollisionPrevention/CollisionPrevention.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ class CollisionPrevention : public ModuleParams
/**
* Returs true if Collision Prevention is running
*/
bool is_active() { return _param_mpc_col_prev_d.get() > 0; }
bool is_active() { return _param_cp_dist.get() > 0; }

/**
* Computes collision free setpoints
Expand Down Expand Up @@ -138,10 +138,11 @@ class CollisionPrevention : public ModuleParams
hrt_abstime _last_collision_warning{0};

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_CNG>) _param_mpc_col_prev_cng, /**< collision prevention change setpoint angle */
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
(ParamFloat<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
(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*/
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/
)
Expand Down
53 changes: 43 additions & 10 deletions src/lib/CollisionPrevention/CollisionPreventionTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ TEST_F(CollisionPreventionTest, noSensorData)
matrix::Vector2f curr_vel(2, 0);

// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);

// WHEN: we set the parameter check then apply the setpoint modification
float value = 10; // try to keep 10m away from obstacles
Expand Down Expand Up @@ -140,7 +140,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage)
attitude.q[3] = 0.0f;

// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
Expand Down Expand Up @@ -205,7 +205,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
attitude.q[3] = 0.0f;

// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
Expand Down Expand Up @@ -266,7 +266,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
attitude.q[3] = 0.0f;

// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
Expand Down Expand Up @@ -335,7 +335,7 @@ TEST_F(CollisionPreventionTest, noBias)
matrix::Vector2f curr_vel(2, 0);

// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
Expand Down Expand Up @@ -375,7 +375,7 @@ TEST_F(CollisionPreventionTest, outsideFOV)
matrix::Vector2f curr_vel(2, 0);

// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
Expand Down Expand Up @@ -429,6 +429,39 @@ TEST_F(CollisionPreventionTest, outsideFOV)
orb_unadvertise(obstacle_distance_pub);
}

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: a parameter handle
param_t param = param_handle(px4::params::CP_DIST);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();

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);
EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f);

//WHEN: we change the parameter CP_GO_NO_DATA to allow flying ouside the FOV
param_t param_allow = param_handle(px4::params::CP_GO_NO_DATA);
float value_allow = 1;
param_set(param_allow, &value_allow);
cp.paramsChanged();

//THEN: the modified setpoint should stay the same as the input
modified_setpoint = original_setpoint;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
EXPECT_FLOAT_EQ(modified_setpoint.norm(), original_setpoint.norm());
}

TEST_F(CollisionPreventionTest, jerkLimit)
{
// GIVEN: a simple setup condition
Expand All @@ -439,7 +472,7 @@ TEST_F(CollisionPreventionTest, jerkLimit)
matrix::Vector2f curr_vel(2, 0);

// AND: distance set to 5m
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
Expand Down Expand Up @@ -847,7 +880,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
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);
param_t param = param_handle(px4::params::CP_DIST);
float value = 3; // try to keep 10m away from obstacles
param_set(param, &value);
cp.paramsChanged();
Expand Down Expand Up @@ -896,7 +929,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
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);
param_t param = param_handle(px4::params::CP_DIST);
float value = 3; // try to keep 10m away from obstacles
param_set(param, &value);
cp.paramsChanged();
Expand Down Expand Up @@ -927,7 +960,7 @@ TEST_F(CollisionPreventionTest, overlappingSensors)
attitude.q[3] = 0.0f;

// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
Expand Down
16 changes: 13 additions & 3 deletions src/lib/CollisionPrevention/collisionprevention_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@
* @unit meters
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);
PARAM_DEFINE_FLOAT(CP_DIST, -1.0f);

/**
* Average delay of the range sensor message plus the tracking delay of the position controller in seconds
Expand All @@ -61,7 +61,7 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);
* @unit seconds
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.4f);
PARAM_DEFINE_FLOAT(CP_DELAY, 0.4f);

/**
* Angle left/right from the commanded setpoint by which the collision prevention algorithm can choose to change the setpoint direction
Expand All @@ -73,4 +73,14 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.4f);
* @unit [deg]
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_COL_PREV_CNG, 30.f);
PARAM_DEFINE_FLOAT(CP_GUIDE_ANG, 30.f);

/**
* Boolean to allow moving into directions where there is no sensor data (outside FOV)
*
* Only used in Position mode.
*
* @boolean
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(CP_GO_NO_DATA, 0);
2 changes: 1 addition & 1 deletion src/lib/parameters/ParameterTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ class ParameterTest : public ::testing::Test
TEST_F(ParameterTest, testParamReadWrite)
{
// GIVEN a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);

// WHEN: we get the parameter
float value = -999.f;
Expand Down

0 comments on commit be1f966

Please sign in to comment.