From be1f966e5f793206cc418711d17d8b4431d7b6c2 Mon Sep 17 00:00:00 2001 From: Tanja Baumann Date: Thu, 17 Oct 2019 17:23:50 +0200 Subject: [PATCH] Collision prevention: Option to enable flying outside FOV and rename 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 --- .../CollisionPrevention.cpp | 13 ++--- .../CollisionPrevention.hpp | 9 ++-- .../CollisionPreventionTest.cpp | 53 +++++++++++++++---- .../collisionprevention_params.c | 16 ++++-- src/lib/parameters/ParameterTest.cpp | 2 +- 5 files changed, 69 insertions(+), 24 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index a5028d9e27..f2b4d3750e 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -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; @@ -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(); @@ -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 @@ -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; } } diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index af4afe2555..063be9a232 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -70,7 +70,7 @@ public: /** * 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 @@ -138,10 +138,11 @@ private: hrt_abstime _last_collision_warning{0}; DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */ - (ParamFloat) _param_mpc_col_prev_cng, /**< collision prevention change setpoint angle */ + (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 */ + (ParamFloat) _param_cp_go_nodata, /**< movement allowed where no data*/ (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ - (ParamFloat) _param_mpc_col_prev_dly, /**< delay of the range measurement data*/ (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ (ParamFloat) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/ ) diff --git a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp index fef858b775..3ab0d93487 100644 --- a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp +++ b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp @@ -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 @@ -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(); @@ -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(); @@ -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(); @@ -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(); @@ -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(); @@ -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 @@ -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(); @@ -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(); @@ -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(); @@ -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(); diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index cbc734b6dc..3f80cf84bf 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -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 @@ -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 @@ -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); diff --git a/src/lib/parameters/ParameterTest.cpp b/src/lib/parameters/ParameterTest.cpp index 1e9bec0336..cecb1a70da 100644 --- a/src/lib/parameters/ParameterTest.cpp +++ b/src/lib/parameters/ParameterTest.cpp @@ -51,7 +51,7 @@ public: 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;