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

* don't allow moving outside FOV

* Update parameters in tests

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 &&
+				if ((float)i * obstacle.increment < 360.f) { //disregard unused bins at the end of the message
-				    obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
+
 					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
+					if (obstacle.distances[i] < obstacle.max_distance &&
-					    && setpoint_dir.dot(bin_direction) > cosf(math::radians(_param_mpc_col_prev_ang.get()))) {
+					    obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
-						//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);
 
-						//constrain the velocity
+						if (setpoint_dir.dot(bin_direction) > 0
-						if (vel_max_bin >= 0) {
+						    && setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) {
-							vel_max = math::min(vel_max, vel_max_bin);
+							//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

@@ -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
+	// THEN: it should be cut down to zero
-	EXPECT_GT(original_setpoint.norm() * 0.5f, modified_setpoint.norm()); //FIXME: this should actually be constrained to 0
+	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);
 }