Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Collision prevention: Option to enable flying outside FOV #13135

Merged
merged 5 commits into from
Oct 17, 2019
Merged

Collision prevention: Option to enable flying outside FOV #13135

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
703c488
rename parameters to allow more descriptive names
baumanta Oct 8, 2019
0ef5a94
add option to enable moving where there is no data
baumanta Oct 9, 2019
c7b385f
group collision prevention params under CP group
baumanta Oct 17, 2019
66fa0ce
make bool const
baumanta Oct 17, 2019
f3450a2
add test for param CP_GO_NO_DATA
baumanta Oct 17, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 7 additions & 6 deletions src/lib/CollisionPrevention/CollisionPrevention.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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