Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

sf45/collision-prevention: replace repeated code with ObstacleMath library functions.

Browse Source
This commit is contained in:
mahimayoga
2025-02-07 10:59:25 +01:00
committed by Silvan Fuhrer
parent e7e76e2e21
commit 093b379b6b
7 changed files with 78 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
+2 -16
View File
@@ -620,7 +620,8 @@ void SF45LaserSerial::sf45_process_replies()
float distance_m = raw_distance * SF45_SCALE_FACTOR;
_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;
uint8_t current_bin = sf45_convert_angle(scaled_yaw);
// Find bin index for the current sensor yaw angle (in sensor frame)
const int current_bin = ObstacleMath::get_bin_at_angle(_obstacle_distance.increment, scaled_yaw_sensor_frame);
if (current_bin != _previous_bin) {
PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
@@ -702,21 +703,6 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
}
}
uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
{
uint8_t mapped_sector = 0;
float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment);
return mapped_sector;
}
float SF45LaserSerial::sf45_wrap_360(float f)
{
return matrix::wrap(f, 0.f, 360.f);
}
uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val)
{
uint32_t i;
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
-6
View File
@@ -74,10 +74,6 @@ enum SF45_PARSED_STATE {
};
enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
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
ROTATION_UPWARD_FACING = 24, // MAV_SENSOR_ROTATION_PITCH_90
ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270
};
@@ -95,8 +91,6 @@ public:
void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value);
void sf45_process_replies();
uint8_t sf45_convert_angle(const int16_t yaw);
float sf45_wrap_360(float f);
private:
obstacle_distance_s _obstacle_distance{};
src/lib/collision_prevention/CollisionPrevention.cpp
+24 -85
View File
@@ -236,14 +236,14 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
// corresponding data index (convert to world frame and shift by msg offset)
for (int i = 0; i < BIN_COUNT; i++) {
for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) {
float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
- (float)_obstacle_map_body_frame.increment / 2.f);
float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+ (float)_obstacle_map_body_frame.increment / 2.f);
float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg -
obstacle.increment / 2.f);
float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg +
obstacle.increment / 2.f);
float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment,
_obstacle_map_body_frame.angle_offset);
float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment,
_obstacle_map_body_frame.angle_offset);
float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment,
obstacle.angle_offset - vehicle_orientation_deg);
float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment,
obstacle.angle_offset - vehicle_orientation_deg);
// if a bin stretches over the 0/360 degree line, adjust the angles
if (bin_lower_angle > bin_upper_angle) {
@@ -277,12 +277,12 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
// corresponding data index (shift by msg offset)
for (int i = 0; i < BIN_COUNT; i++) {
for (int j = 0; j < 360 / obstacle.increment; j++) {
float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
- (float)_obstacle_map_body_frame.increment / 2.f);
float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+ (float)_obstacle_map_body_frame.increment / 2.f);
float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f);
float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f);
float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment,
_obstacle_map_body_frame.angle_offset);
float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment,
_obstacle_map_body_frame.angle_offset);
float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, obstacle.angle_offset);
float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, obstacle.angle_offset);
// if a bin stretches over the 0/360 degree line, adjust the angles
if (bin_lower_angle > bin_upper_angle) {
@@ -373,7 +373,7 @@ void
CollisionPrevention::_transformSetpoint(const Vector2f &setpoint)
{
const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw;
const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) -
const float sp_angle_with_offset_deg = ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) -
_obstacle_map_body_frame.angle_offset);
_setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
@@ -394,7 +394,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
// discard values below min range
if (distance_reading > distance_sensor.min_distance) {
float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset);
float sensor_yaw_body_rad = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
(distance_sensor.orientation), distance_sensor.q);
float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));
// calculate the field of view boundary bin indices
@@ -408,7 +409,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm
for (int bin = lower_bound; bin <= upper_bound; ++bin) {
int wrapped_bin = _wrap_bin(bin);
int wrapped_bin = ObstacleMath::wrap_bin(bin, BIN_COUNT);
if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) {
_obstacle_map_body_frame.distances[wrapped_bin] = static_cast<uint16_t>(100.0f * distance_reading + 0.5f);
@@ -435,7 +436,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
float mean_dist = 0;
for (int j = i - filter_size; j <= i + filter_size; j++) {
int bin = _wrap_bin(j);
int bin = ObstacleMath::wrap_bin(j, BIN_COUNT);
if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
mean_dist += _param_cp_dist.get() * 100.f;
@@ -445,7 +446,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
}
}
const int bin = _wrap_bin(i);
const int bin = ObstacleMath::wrap_bin(i, BIN_COUNT);
mean_dist = mean_dist / (2.f * filter_size + 1.f);
const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original);
const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];
@@ -465,52 +466,6 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
}
}
float
CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
{
float offset = math::max(math::radians(angle_offset), 0.f);
switch (distance_sensor.orientation) {
case distance_sensor_s::ROTATION_YAW_0:
offset = 0.0f;
break;
case distance_sensor_s::ROTATION_YAW_45:
offset = M_PI_F / 4.0f;
break;
case distance_sensor_s::ROTATION_YAW_90:
offset = M_PI_F / 2.0f;
break;
case distance_sensor_s::ROTATION_YAW_135:
offset = 3.0f * M_PI_F / 4.0f;
break;
case distance_sensor_s::ROTATION_YAW_180:
offset = M_PI_F;
break;
case distance_sensor_s::ROTATION_YAW_225:
offset = -3.0f * M_PI_F / 4.0f;
break;
case distance_sensor_s::ROTATION_YAW_270:
offset = -M_PI_F / 2.0f;
break;
case distance_sensor_s::ROTATION_YAW_315:
offset = -M_PI_F / 4.0f;
break;
case distance_sensor_s::ROTATION_CUSTOM:
offset = Eulerf(Quatf(distance_sensor.q)).psi();
break;
}
return offset;
}
float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
{
float obstacle_distance = 0.f;
@@ -520,10 +475,10 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
Vector2f dir = direction / direction_norm;
const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw;
const float sp_angle_with_offset_deg =
_wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1);
obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f;
ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
const int dir_index = ObstacleMath::get_bin_at_angle(BIN_SIZE, sp_angle_with_offset_deg);
obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f;
}
return obstacle_distance;
@@ -632,19 +587,3 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter()
command.timestamp = getTime();
_vehicle_command_pub.publish(command);
}
float CollisionPrevention::_wrap_360(const float f)
{
return wrap(f, 0.f, 360.f);
}
int CollisionPrevention::_wrap_bin(int i)
{
i = i % BIN_COUNT;
while (i < 0) {
i += BIN_COUNT;
}
return i;
}
src/lib/collision_prevention/CollisionPrevention.hpp
-9
View File
@@ -204,13 +204,6 @@ private:
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/
)
/**
* Transforms the sensor orientation into a yaw in the local frame
* @param distance_sensor, distance sensor message
* @param angle_offset, sensor body frame offset
*/
float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const;
/**
* Computes collision free setpoints
* @param setpoint, setpoint before collision prevention intervention
@@ -238,6 +231,4 @@ private:
*/
void _publishVehicleCmdDoLoiter();
static float _wrap_360(const float f);
static int _wrap_bin(int i);
};
src/lib/collision_prevention/ObstacleMath.cpp
+19 -1
View File
@@ -57,13 +57,19 @@ int get_bin_at_angle(float bin_width, float angle)
return wrap_bin(bin_at_angle, 360 / bin_width);
}
float get_lower_bound_angle(int bin, float bin_width, float angle_offset)
{
bin = wrap_bin(bin, 360 / bin_width);
return wrap_360(bin * bin_width + angle_offset - bin_width / 2.f);
}
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 sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4])
{
float offset = 0.0f;
@@ -99,6 +105,13 @@ float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
case SensorOrientation::ROTATION_YAW_315:
offset = -M_PI_F / 4.0f;
break;
case SensorOrientation::ROTATION_CUSTOM:
if (q != nullptr) {
offset = Eulerf(Quatf(q)).psi();
}
break;
}
return offset;
@@ -109,4 +122,9 @@ int wrap_bin(int bin, int bin_count)
return (bin + bin_count) % bin_count;
}
float wrap_360(const float angle)
{
return matrix::wrap(angle, 0.0f, 360.0f);
}
} // ObstacleMath
src/lib/collision_prevention/ObstacleMath.hpp
+17 -1
View File
@@ -45,6 +45,7 @@ enum SensorOrientation {
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_CUSTOM = 100, // MAV_SENSOR_ROTATION_CUSTOM
ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE
ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90
@@ -56,7 +57,7 @@ enum SensorOrientation {
* Converts a sensor orientation to a yaw offset
* @param orientation sensor orientation
*/
float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4] = nullptr);
/**
* Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
@@ -73,6 +74,14 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons
*/
int get_bin_at_angle(float bin_width, float angle);
/**
* Returns lower bound angle of a bin
* @param bin bin index
* @param bin_width width of a bin in degrees
* @param angle_offset clockwise angle offset in degrees
*/
float get_lower_bound_angle(int bin, float bin_width, float angle_offset);
/**
* Returns bin index for the current bin after an angle offset
* @param bin current bin index
@@ -88,4 +97,11 @@ int get_offset_bin_index(int bin, float bin_width, float angle_offset);
*/
int wrap_bin(int bin, int bin_count);
/**
* Wraps an angle to the range [0, 360)
* @param angle angle in degrees
*/
float wrap_360(const float angle);
} // ObstacleMath
src/lib/collision_prevention/ObstacleMathTest.cpp
+16
View File
@@ -133,6 +133,22 @@ TEST(ObstacleMathTest, GetBinAtAngle)
EXPECT_EQ(bin_index, 18);
}
TEST(ObstacleMathTest, GetLowerBound)
{
// GIVEN: an invalid bin index, non-integer bin width, and a negative non-integer angle offset
int bin = -1;
float bin_width = 7.5f;
float angle_offset = -4.3f;
// WHEN: we calculate the lower bound angle of the bin
float lower_bound = ObstacleMath::get_lower_bound_angle(bin, bin_width, angle_offset);
// THEN: the lower bound angle should be correct. The bin index is wrapped to the end and
// the angle offset is applied in the counter-clockwise direction.
EXPECT_FLOAT_EQ(lower_bound, 344.45);
}
TEST(ObstacleMathTest, OffsetBinIndex)
{