obstacle-math: add wrap_bin function.

This function can be used to wrap a bin index in an obstacle map within the maximum number of bins.

src/lib/collision_prevention/ObstacleMath.cpp

@@ -51,18 +51,18 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons
 	distance *= horizontal_projection_scale;
 }
 
-int get_bin_at_angle(int start_bin, float bin_width, float angle)
+int get_bin_at_angle(float bin_width, float angle, int start_bin)
 {
-	return (start_bin + (int)round(matrix::wrap(angle, 0.f, 360.f) / bin_width));
+	int bin_at_angle = start_bin + (int)round(matrix::wrap(angle, 0.f, 360.f) / bin_width);
+
+	return wrap_bin(bin_at_angle, 360 / bin_width);
 }
 
 int get_offset_bin_index(int bin, float bin_width, float angle_offset)
 {
-	int offset = get_bin_at_angle(0, bin_width, angle_offset);
-
-	int new_bin_index = (bin - offset < 0) ? bin - offset + 360 / bin_width : bin - offset;
+	int offset = get_bin_at_angle(bin_width, angle_offset);
 
-	return new_bin_index;
+	return wrap_bin(bin - offset, 360 / bin_width);;
 }
 
 float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
@@ -107,4 +107,9 @@ float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
 	return offset;
 }
 
+int wrap_bin(int bin, int bin_count)
+{
+	return (bin + bin_count) % bin_count;
+}
+
 } // ObstacleMath

src/lib/collision_prevention/ObstacleMath.hpp

@@ -36,7 +36,7 @@
 namespace ObstacleMath
 {
 
-enum SensorOrientation{
+enum SensorOrientation {
 	ROTATION_YAW_0   = 0,     // MAV_SENSOR_ROTATION_NONE
 	ROTATION_YAW_45  = 1,	  // MAV_SENSOR_ROTATION_YAW_45
 	ROTATION_YAW_90  = 2,	  // MAV_SENSOR_ROTATION_YAW_90
@@ -72,7 +72,7 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons
  * @param bin_width width of a bin in degrees
  * @param angle clockwise angle from start bin in degrees
  */
-int get_bin_at_angle(int start_bin, float bin_width, float angle);
+int get_bin_at_angle(float bin_width, float angle, int start_bin = 0);
 
 /**
  * Returns bin index for the current bin after an angle offset
@@ -82,4 +82,11 @@ int get_bin_at_angle(int start_bin, float bin_width, float angle);
  */
 int get_offset_bin_index(int bin, float bin_width, float angle_offset);
 
+/**
+ * Wraps a bin index to the range [0, bin_count)
+ * @param bin bin index
+ * @param bin_count number of bins
+ */
+int wrap_bin(int bin, int bin_count);
+
 } // ObstacleMath

src/lib/collision_prevention/ObstacleMathTest.cpp

@@ -101,7 +101,7 @@ TEST(ObstacleMathTest, GetBinAtAngle)
 	float angle = 0.0f;
 
 	// WHEN: we calculate the bin index at the angle
-	uint16_t bin_index = ObstacleMath::get_bin_at_angle(start_bin, bin_width, angle);
+	uint16_t bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle, start_bin);
 
 	// THEN: the bin index should be correct
 	EXPECT_EQ(bin_index, 0);
@@ -110,7 +110,7 @@ TEST(ObstacleMathTest, GetBinAtAngle)
 	angle = 90.0f;
 
 	// WHEN: we calculate the bin index at the angle
-	bin_index = ObstacleMath::get_bin_at_angle(start_bin, bin_width, angle);
+	bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle, start_bin);
 
 	// THEN: the bin index should be correct
 	EXPECT_EQ(bin_index, 18);
@@ -119,7 +119,7 @@ TEST(ObstacleMathTest, GetBinAtAngle)
 	angle = -90.0f;
 
 	// WHEN: we calculate the bin index at the angle
-	bin_index = ObstacleMath::get_bin_at_angle(start_bin, bin_width, angle);
+	bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle, start_bin);
 
 	// THEN: the bin index should be correct
 	EXPECT_EQ(bin_index, 54);
@@ -128,7 +128,7 @@ TEST(ObstacleMathTest, GetBinAtAngle)
 	angle = 450.0f;
 
 	// WHEN: we calculate the bin index at the angle
-	bin_index = ObstacleMath::get_bin_at_angle(start_bin, bin_width, angle);
+	bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle, start_bin);
 
 	// THEN: the bin index should be correct
 	EXPECT_EQ(bin_index, 18);
@@ -160,21 +160,66 @@ TEST(ObstacleMathTest, OffsetBinIndex)
 	EXPECT_EQ(new_bin_index, 0);
 }
 
+
+TEST(ObstacleMathTest, WrapBin)
+{
+	// GIVEN: a bin index and the number of bins
+	int bin = 0;
+	int bin_count = 72;
+
+	// WHEN: we wrap the bin index
+	int wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should be correct
+	EXPECT_EQ(wrapped_bin, 0);
+
+	// GIVEN: a bin index and the number of bins
+	bin = 72;
+	bin_count = 72;
+
+	// WHEN: we wrap the bin index
+	wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should be correct
+	EXPECT_EQ(wrapped_bin, 0);
+
+	// GIVEN: a bin index and the number of bins
+	bin = 73;
+	bin_count = 72;
+
+	// WHEN: we wrap the bin index
+	wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should be correct
+	EXPECT_EQ(wrapped_bin, 1);
+
+	// GIVEN: a bin index and the number of bins
+	bin = -1;
+	bin_count = 72;
+
+	// WHEN: we wrap the bin index
+	wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should be correct
+	EXPECT_EQ(wrapped_bin, 71);
+}
+
 TEST(ObstacleMathTest, HandleMissedBins)
 {
 	// GIVEN: measurements, current bin, previous bin, bin width, and field of view offset
 	float measurements[8] = {0, 0, 1, 0, 0, 2, 0, 0};
 	int   current_bin     = 2;
 	int   previous_bin    = 5;
 	int   bin_width       = 45.0f;
+	float angle_offset    = 0.0f;
 	float fov             = 270.0f;
 	float fov_offset      = 360.0f - fov / 2;
 
 	float measurement     = measurements[current_bin];
 
 	// WHEN: we handle missed bins
-	int current_bin_offset  = ObstacleMath::get_offset_bin_index(current_bin, bin_width, fov_offset);
-	int previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, bin_width, fov_offset);
+	int current_bin_offset  = ObstacleMath::get_offset_bin_index(current_bin,  bin_width, fov_offset + angle_offset);
+	int previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, bin_width, fov_offset + angle_offset);
 
 	int start = math::min(current_bin_offset, previous_bin_offset) + 1;
 	int end   = math::max(current_bin_offset, previous_bin_offset);