diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index d01a6b6517..8901f1022f 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -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; + } + } } } diff --git a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp index a96c06f9a1..11910ce724 100644 --- a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp +++ b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp @@ -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); }