fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 23:24:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

obstacle-math: add standard obstacle map functions.

Browse Source 
These functions help simplify repeated calculations accross driver and collision prevention files that are computing bins, angles and sensor offsets in obstacle maps.
This commit is contained in:
mahimayoga 2025-02-03 16:31:31 +01:00 committed by Silvan Fuhrer
parent f7dadd9b89
commit cb332e047d
3 changed files with 251 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
src/lib/collision_prevention/ObstacleMath.cpp
View File

@ -51,4 +51,62 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons
distance *= horizontal_projection_scale;
}
int get_bin_at_angle(float bin_width, float angle)
{
int bin_at_angle = (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(bin_width, angle_offset);
return wrap_bin(bin - offset, 360 / bin_width);
}
float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
{
float offset = 0.0f;
switch (orientation) {
case SensorOrientation::ROTATION_YAW_0:
offset = 0.0f;
break;
case SensorOrientation::ROTATION_YAW_45:
offset = M_PI_F / 4.0f;
break;
case SensorOrientation::ROTATION_YAW_90:
offset = M_PI_F / 2.0f;
break;
case SensorOrientation::ROTATION_YAW_135:
offset = 3.0f * M_PI_F / 4.0f;
break;
case SensorOrientation::ROTATION_YAW_180:
offset = M_PI_F;
break;
case SensorOrientation::ROTATION_YAW_225:
offset = -3.0f * M_PI_F / 4.0f;
break;
case SensorOrientation::ROTATION_YAW_270:
offset = -M_PI_F / 2.0f;
break;
case SensorOrientation::ROTATION_YAW_315:
offset = -M_PI_F / 4.0f;
break;
}
return offset;
}
int wrap_bin(int bin, int bin_count)
{
return (bin + bin_count) % bin_count;
}
} // ObstacleMath

44
src/lib/collision_prevention/ObstacleMath.hpp
View File

@ -36,6 +36,28 @@
namespace ObstacleMath
{
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
ROTATION_YAW_135 = 3, // MAV_SENSOR_ROTATION_YAW_135
ROTATION_YAW_180 = 4, // MAV_SENSOR_ROTATION_YAW_180
ROTATION_YAW_225 = 5, // MAV_SENSOR_ROTATION_YAW_225
ROTATION_YAW_270 = 6, // MAV_SENSOR_ROTATION_YAW_270
ROTATION_YAW_315 = 7, // MAV_SENSOR_ROTATION_YAW_315
ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE
ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90
ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
ROTATION_LEFT_FACING = 6 // MAV_SENSOR_ROTATION_YAW_270
};
/**
* Converts a sensor orientation to a yaw offset
* @param orientation sensor orientation
*/
float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
/**
* Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
* @param distance measurement which is scaled down
@ -44,4 +66,26 @@ namespace ObstacleMath
*/
void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle);
/**
* Returns bin index at a given angle from the 0th bin
* @param bin_width width of a bin in degrees
* @param angle clockwise angle from start bin in degrees
*/
int get_bin_at_angle(float bin_width, float angle);
/**
* Returns bin index for the current bin after an angle offset
* @param bin current bin index
* @param bin_width width of a bin in degrees
* @param angle_offset clockwise angle offset in degrees
*/
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

150
src/lib/collision_prevention/ObstacleMathTest.cpp
View File

@ -33,6 +33,7 @@
#include <gtest/gtest.h>
#include <matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
#include "ObstacleMath.hpp"
using namespace matrix;
@ -89,5 +90,152 @@ TEST(ObstacleMathTest, ProjectDistanceOnHorizontalPlane)
expected_distance = 1.0f * expected_scale;
EXPECT_NEAR(distance, expected_distance, 1e-5);
}
TEST(ObstacleMathTest, GetBinAtAngle)
{
float bin_width = 5.0f;
// GIVEN: a start bin, bin width, and angle
float angle = 0.0f;
// WHEN: we calculate the bin index at the angle
uint16_t bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
// THEN: the bin index should be correct
EXPECT_EQ(bin_index, 0);
// GIVEN: a start bin, bin width, and angle
angle = 90.0f;
// WHEN: we calculate the bin index at the angle
bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
// THEN: the bin index should be correct
EXPECT_EQ(bin_index, 18);
// GIVEN: a start bin, bin width, and angle
angle = -90.0f;
// WHEN: we calculate the bin index at the angle
bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
// THEN: the bin index should be correct
EXPECT_EQ(bin_index, 54);
// GIVEN: a start bin, bin width, and angle
angle = 450.0f;
// WHEN: we calculate the bin index at the angle
bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
// THEN: the bin index should be correct
EXPECT_EQ(bin_index, 18);
}
TEST(ObstacleMathTest, OffsetBinIndex)
{
// In this test, we want to offset the bin index by a negative and positive angle.
// We take the output of the first offset and offset it by the same angle in the
// opposite direction to return back to the original bin index.
// GIVEN: a bin index, bin width, and a negative angle offset
uint16_t bin = 0;
float bin_width = 5.0f;
float angle_offset = -120.0f;
// WHEN: we offset the bin index by the negative angle
uint16_t new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset);
// THEN: the new bin index should be correctly offset by the wrapped angle
EXPECT_EQ(new_bin_index, 24);
// GIVEN: the output bin index of the previous offset, bin width, and the same angle
// offset in positive direction
bin = 24;
bin_width = 5.0f;
angle_offset = 120.0f;
// WHEN: we offset the bin index by the positive angle
new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset);
// THEN: the new bin index should return back to the original bin index
EXPECT_EQ(new_bin_index, 0);
}
TEST(ObstacleMathTest, WrapBin)
{
// GIVEN: a bin index within bounds and the number of bins
int bin = 0;
int bin_count = 72;
// WHEN: we wrap a bin index within the bounds
int wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
// THEN: the wrapped bin index should stay 0
EXPECT_EQ(wrapped_bin, 0);
// GIVEN: a bin index that is out of bounds, and the number of bins
bin = 73;
bin_count = 72;
// WHEN: we wrap a bin index that is larger than the number of bins
wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
// THEN: the wrapped bin index should be wrapped back to the beginning
EXPECT_EQ(wrapped_bin, 1);
// GIVEN: a negative bin index and the number of bins
bin = -1;
bin_count = 72;
// WHEN: we wrap a bin index that is negative
wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
// THEN: the wrapped bin index should be wrapped back to the end
EXPECT_EQ(wrapped_bin, 71);
}
TEST(ObstacleMathTest, HandleMissedBins)
{
// In this test, the current and previous bin are adjacent to the bins that are outside
// the sensor field of view. The missed bins (0,1,6 & 7) should be populated, and no
// data should be filled in the bins outside the FOV.
// 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 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 start = math::min(current_bin_offset, previous_bin_offset) + 1;
int end = math::max(current_bin_offset, previous_bin_offset);
EXPECT_EQ(start, 1);
EXPECT_EQ(end, 5);
for (uint16_t i = start; i < end; i++) {
uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, bin_width, -fov_offset);
measurements[bin_index] = measurement;
}
// THEN: the correct missed bins should be populated with the measurement
EXPECT_EQ(measurements[0], 1);
EXPECT_EQ(measurements[1], 1);
EXPECT_EQ(measurements[2], 1);
EXPECT_EQ(measurements[3], 0);
EXPECT_EQ(measurements[4], 0);
EXPECT_EQ(measurements[5], 2);
EXPECT_EQ(measurements[6], 1);
EXPECT_EQ(measurements[7], 1);
}