From 413236d281250359f5a68425bf8cb72d17c3e0b6 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 3 Feb 2025 16:31:31 +0100 Subject: [PATCH 1/3] obstacle-math: add standard obstacle map functions. These functions help simplify repeated calculations accross driver and collision prevention files that are computing bins, angles and sensor offsets in obstacle maps. --- src/lib/collision_prevention/ObstacleMath.cpp | 58 +++++++ src/lib/collision_prevention/ObstacleMath.hpp | 44 ++++++ .../collision_prevention/ObstacleMathTest.cpp | 148 ++++++++++++++++++ 3 files changed, 250 insertions(+) diff --git a/src/lib/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp index 22d67c64ae61..0605b664ec38 100644 --- a/src/lib/collision_prevention/ObstacleMath.cpp +++ b/src/lib/collision_prevention/ObstacleMath.cpp @@ -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 diff --git a/src/lib/collision_prevention/ObstacleMath.hpp b/src/lib/collision_prevention/ObstacleMath.hpp index 3c19eb911062..f5317aeaa707 100644 --- a/src/lib/collision_prevention/ObstacleMath.hpp +++ b/src/lib/collision_prevention/ObstacleMath.hpp @@ -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 diff --git a/src/lib/collision_prevention/ObstacleMathTest.cpp b/src/lib/collision_prevention/ObstacleMathTest.cpp index e24b95134c96..cea54aa4a25f 100644 --- a/src/lib/collision_prevention/ObstacleMathTest.cpp +++ b/src/lib/collision_prevention/ObstacleMathTest.cpp @@ -33,6 +33,7 @@ #include #include +#include #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); } From 8ae081ccb3b18993c70992bd2f34b37ef7073b73 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 3 Feb 2025 16:37:00 +0100 Subject: [PATCH 2/3] sf45: change handle_missed_bins() function logic. To simplify logic for wrap-around cases and cases in which bins outside the FOV may be filled. Bin indices are offset such that the 0th bin is the first bin within the sensor FOV. This way we can always fill bins from smallest to largest index. --- .../lightware_sf45_serial.cpp | 50 +++++++------------ .../lightware_sf45_serial.hpp | 1 + 2 files changed, 18 insertions(+), 33 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index a7a94c67915e..e6fbe3e4b9bc 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -664,6 +664,7 @@ void SF45LaserSerial::sf45_process_replies() hrt_abstime now = hrt_absolute_time(); + _obstacle_distance.distances[current_bin] = _current_bin_dist; _handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now); _publish_obstacle_msg(now); @@ -701,43 +702,26 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_ { // if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement. // in this case we assume the measurement to be valid for all bins between the previous and the current bin. - uint8_t start; - uint8_t end; - - if (abs(current_bin - previous_bin) > BIN_COUNT / 4) { - // wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins - // THis is simplyfied as we are not considering the scaning direction - start = math::max(previous_bin, current_bin); - end = math::min(previous_bin, current_bin); - - } else if (previous_bin < current_bin) { // Scanning clockwise - start = previous_bin + 1; - end = current_bin; - - } else { // scanning counter-clockwise - start = current_bin; - end = previous_bin - 1; - } - - if (start <= end) { - for (uint8_t i = start; i <= end; i++) { - _obstacle_distance.distances[i] = measurement; - _data_timestamps[i] = now; - } - } else { // wrap-around case - for (uint8_t i = start; i < BIN_COUNT; i++) { - _obstacle_distance.distances[i] = measurement; - _data_timestamps[i] = now; - } - - for (uint8_t i = 0; i <= end; i++) { - _obstacle_distance.distances[i] = measurement; - _data_timestamps[i] = now; - } + // Shift bin indices such that we can never have the wrap-around case. + const float fov_offset_angle = 360.0f - SF45_FIELDOF_VIEW / 2.f; + const uint16_t current_bin_offset = ObstacleMath::get_offset_bin_index(current_bin, _obstacle_distance.increment, + fov_offset_angle); + const uint16_t previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment, + fov_offset_angle); + + const uint16_t start = math::min(current_bin_offset, previous_bin_offset) + 1; + const uint16_t end = math::max(current_bin_offset, previous_bin_offset); + + // populate the missed bins with the measurement + for (uint16_t i = start; i < end; i++) { + uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, _obstacle_distance.increment, -fov_offset_angle); + _obstacle_distance.distances[bin_index] = measurement; + _data_timestamps[bin_index] = now; } } + uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) { uint8_t mapped_sector = 0; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index bfe44f0da2a5..d308a474d4cc 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -105,6 +105,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem obstacle_distance_s::distances[0]); static constexpr uint64_t SF45_MEAS_TIMEOUT{100_ms}; static constexpr float SF45_SCALE_FACTOR = 0.01f; + static constexpr float SF45_FIELDOF_VIEW = 320.f; // degrees void start(); void stop(); From 2164a48ff608e3a4f0144394f892070e3e3fd191 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 3 Feb 2025 16:41:37 +0100 Subject: [PATCH 3/3] sf45: refactor how sensor orientation (yaw_cfg) correction is applied to incoming sensor data. yaw_cfg is now read into the obstacle_distance message as the angle_offset. The offset is computed once at init and applied to each measurement. --- .../lightware_sf45_serial.cpp | 39 +++++-------------- 1 file changed, 10 insertions(+), 29 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index e6fbe3e4b9bc..7f16fa5d26ec 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -92,6 +92,11 @@ int SF45LaserSerial::init() param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); + // set the sensor orientation + const float yaw_cfg_angle = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast + (_yaw_cfg)); + _obstacle_distance.angle_offset = math::degrees(matrix::wrap_2pi(yaw_cfg_angle)); + start(); return PX4_OK; } @@ -594,7 +599,6 @@ void SF45LaserSerial::sf45_process_replies() case SF_DISTANCE_DATA_CM: { const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8); int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8)); - int16_t scaled_yaw = 0; // The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float if (raw_yaw > 32000) { @@ -607,33 +611,10 @@ void SF45LaserSerial::sf45_process_replies() } // SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}" - scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; - - switch (_yaw_cfg) { - case ROTATION_FORWARD_FACING: - break; - - case ROTATION_BACKWARD_FACING: - if (scaled_yaw > 180) { - scaled_yaw = scaled_yaw - 180; - - } else { - scaled_yaw = scaled_yaw + 180; // rotation facing aft - } + float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; - break; - - case ROTATION_RIGHT_FACING: - scaled_yaw = scaled_yaw + 90; // rotation facing right - break; - - case ROTATION_LEFT_FACING: - scaled_yaw = scaled_yaw - 90; // rotation facing left - break; - - default: - break; - } + // Adjust for sensor orientation + scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset); // Convert to meters for the debug message float distance_m = raw_distance * SF45_SCALE_FACTOR; @@ -642,7 +623,7 @@ void SF45LaserSerial::sf45_process_replies() uint8_t current_bin = sf45_convert_angle(scaled_yaw); if (current_bin != _previous_bin) { - PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin, + PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin, (double)distance_m); if (_vehicle_attitude_sub.updated()) { @@ -726,7 +707,7 @@ 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 = round(adjusted_yaw / _obstacle_distance.increment); + mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment); return mapped_sector; }