fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

CollisionPrevention: changed to resolution of 5 degrees, and adapted tests to reflect the change

Browse Source 
rewrite of obstacle_distance merging methods, and fix of various issues
This commit is contained in:
Claudio Chies 2024-11-14 14:32:59 +01:00
parent 2ef2911c36
commit f41a08aea8
4 changed files with 339 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
src/lib/collision_prevention/CollisionPrevention.cpp
View File

@ -151,7 +151,7 @@ void CollisionPrevention::_updateObstacleMap()
}
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
_obstacle_distance_pub.publish(_obstacle_map_body_frame);
_obstacle_distance_fused_pub.publish(_obstacle_map_body_frame);
}
void CollisionPrevention::_updateObstacleData()
@ -228,27 +228,51 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel
}
}
// TODO this gives false output if the offset is not a multiple of the resolution. to be fixed...
void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw)
{
int msg_index = 0;
float vehicle_orientation_deg = math::degrees(vehicle_yaw);
float increment_factor = 1.f / obstacle.increment;
if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) {
// Obstacle message arrives in local_origin frame (north aligned)
// corresponding data index (convert to world frame and shift by msg offset)
for (int i = 0; i < BIN_COUNT; i++) {
float bin_angle_deg = (float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset;
msg_index = ceil(_wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor);
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 - 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);
//add all data points inside to FOV
if (obstacle.distances[msg_index] != UINT16_MAX) {
if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
_data_maxranges[i] = obstacle.max_distance;
_data_fov[i] = 1;
// if a bin stretches over the 0/360 degree line, adjust the angles
if (bin_lower_angle > bin_upper_angle) {
bin_lower_angle -= 360;
}
if (msg_lower_angle > msg_upper_angle) {
msg_lower_angle -= 360;
}
// Check for overlaps.
if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) ||
(msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) ||
(msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) ||
(msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) {
if (obstacle.distances[j] != UINT16_MAX) {
if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) {
_obstacle_map_body_frame.distances[i] = obstacle.distances[j];
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
_data_maxranges[i] = obstacle.max_distance;
_data_fov[i] = 1;
}
}
}
}
}
@ -256,18 +280,39 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
// Obstacle message arrives in body frame (front aligned)
// corresponding data index (shift by msg offset)
for (int i = 0; i < BIN_COUNT; i++) {
float bin_angle_deg = (float)i * BIN_SIZE +
_obstacle_map_body_frame.angle_offset;
msg_index = ceil(_wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor);
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);
//add all data points inside to FOV
if (obstacle.distances[msg_index] != UINT16_MAX) {
if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
_data_maxranges[i] = obstacle.max_distance;
_data_fov[i] = 1;
// if a bin stretches over the 0/360 degree line, adjust the angles
if (bin_lower_angle > bin_upper_angle) {
bin_lower_angle -= 360;
}
if (msg_lower_angle > msg_upper_angle) {
msg_lower_angle -= 360;
}
// Check for overlaps.
if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) ||
(msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) ||
(msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) ||
(msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) {
if (obstacle.distances[j] != UINT16_MAX) {
if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) {
_obstacle_map_body_frame.distances[i] = obstacle.distances[j];
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
_data_maxranges[i] = obstacle.max_distance;
_data_fov[i] = 1;
}
}
}
}
}
@ -357,18 +402,14 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));
// calculate the field of view boundary bin indices
int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
// floor values above zero, ceil values below zero
if (lower_bound < 0) { lower_bound++; }
if (upper_bound < 0) { upper_bound++; }
// rotate vehicle attitude into the sensor body frame
Quatf attitude_sensor_frame = vehicle_attitude;
attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); // verify
if (distance_reading < distance_sensor.max_distance) {
distance_reading = distance_reading * sensor_dist_scale;

8
src/lib/collision_prevention/CollisionPrevention.hpp
View File

@ -80,6 +80,9 @@ public:
*/
void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
static constexpr int BIN_COUNT = 72;
static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly
protected:
/** Aggregates the sensor data into an internal obstacle map in body frame */
void _updateObstacleMap();
@ -90,9 +93,6 @@ protected:
/** Calculate the constrained setpoint considering the current obstacle distances, acceleration setpoint and velocity setpoint */
void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
static constexpr int BIN_COUNT = 36;
static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly
obstacle_distance_s _obstacle_map_body_frame{};
bool _data_fov[BIN_COUNT] {};
uint64_t _data_timestamps[BIN_COUNT] {};
@ -179,7 +179,7 @@ private:
float _vehicle_yaw{0.f};
uORB::Publication<collision_constraints_s> _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */
uORB::Publication<obstacle_distance_s> _obstacle_distance_fused_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */

358
src/lib/collision_prevention/CollisionPreventionTest.cpp
View File

@ -38,6 +38,8 @@ using namespace matrix;
// to run: make tests TESTFILTER=CollisionPrevention
hrt_abstime mocked_time = 0;
const uint bin_size = CollisionPrevention::BIN_SIZE;
const uint bin_count = CollisionPrevention::BIN_COUNT;
class CollisionPreventionTest : public ::testing::Test
{
@ -246,6 +248,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100);
EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0);
EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1);
EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0);
@ -609,14 +612,14 @@ TEST_F(CollisionPreventionTest, jerkLimit)
Vector2f modified_setpoint_limited_jerk = original_setpoint;
cp.modifySetpoint(modified_setpoint_limited_jerk, curr_vel);
// THEN: the new setpoint should be much slower than the one with default jerk
EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm());
// THEN: the new setpoint should be much higher than the one with default jerk, as the rate of change in acceleration is more limmited
EXPECT_GT(modified_setpoint_limited_jerk.norm(), modified_setpoint_default_jerk.norm());
}
TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData)
{
// GIVEN: a vehicle attitude and a distance sensor message
TestCollisionPrevention cp;
cp.getObstacleMap().increment = 10.f;
Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
distance_sensor_s distance_sensor {};
distance_sensor.min_distance = 0.2f;
@ -641,11 +644,65 @@ TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData)
}
}
TEST_F(CollisionPreventionTest, addDistanceSensorDataNarrow)
{
// GIVEN: a vehicle attitude and a distance sensor message
TestCollisionPrevention cp;
Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
distance_sensor_s distance_sensor {};
distance_sensor.min_distance = 0.2f;
distance_sensor.max_distance = 20.f;
distance_sensor.current_distance = 5.f;
distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
distance_sensor.h_fov = math::radians(0.1 * bin_size);
uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
// WHEN the sensor has a very narrow field of view
cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
//THEN: the correct bins in the map should be filled
for (uint32_t i = 0; i < distances_array_size; i++) {
if (i == 0) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
}
}
TEST_F(CollisionPreventionTest, addDistanceSensorDataSlightlyLarger)
{
// GIVEN: a vehicle attitude and a distance sensor message
TestCollisionPrevention cp;
Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
distance_sensor_s distance_sensor {};
distance_sensor.min_distance = 0.2f;
distance_sensor.max_distance = 20.f;
distance_sensor.current_distance = 5.f;
distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
distance_sensor.h_fov = math::radians(1.1 * bin_size);
uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
// WHEN the sensor has a very narrow field of view
cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
//THEN: the the bins corresponding to -5°, 0° and 5° should be filled
for (uint32_t i = 0; i < distances_array_size; i++) {
if (i == 71 || i <= 1) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
}
}
TEST_F(CollisionPreventionTest, addDistanceSensorData)
{
// GIVEN: a vehicle attitude and a distance sensor message
TestCollisionPrevention cp;
cp.getObstacleMap().increment = 10.f;
Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
distance_sensor_s distance_sensor {};
distance_sensor.min_distance = 0.2f;
@ -656,21 +713,24 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData)
uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
for (uint32_t i = 0; i < distances_array_size; i++) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//WHEN: we add distance sensor data to the right
distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING;
distance_sensor.h_fov = math::radians(19.99f);
cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
uint fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2);
uint start = (90 - fov) / bin_size;
uint end = (90 + fov) / bin_size;
//THEN: the correct bins in the map should be filled
for (uint32_t i = 0; i < distances_array_size; i++) {
if (i == 8 || i == 9) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= start && i <= end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
}
@ -679,17 +739,20 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData)
distance_sensor.h_fov = math::radians(50.f);
distance_sensor.current_distance = 8.f;
cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2);
uint start2 = (270 - fov) / bin_size;
uint end2 = (270 + fov) / bin_size;
//THEN: the correct bins in the map should be filled
for (uint32_t i = 0; i < distances_array_size; i++) {
if (i == 8 || i == 9) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= start && i <= end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else if (i >= 24 && i <= 29) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800);
} else if (i >= start2 && i <= end2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
}
@ -698,20 +761,23 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData)
distance_sensor.h_fov = math::radians(10.1f);
distance_sensor.current_distance = 3.f;
cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2);
uint start3 = (360 - fov) / bin_size;
uint end3 = (fov) / bin_size;
//THEN: the correct bins in the map should be filled
for (uint32_t i = 0; i < distances_array_size; i++) {
if (i == 8 || i == 9) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= start && i <= end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else if (i >= 24 && i <= 29) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800);
} else if (i >= start2 && i <= end2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i;
} else if (i == 0) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300);
} else if (i >= start3 || i <= end3) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
}
@ -722,7 +788,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
{
// GIVEN: a vehicle attitude and obstacle distance message
TestCollisionPrevention cp;
cp.getObstacleMap().increment = 10.f;
obstacle_distance_s obstacle_msg {};
obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
obstacle_msg.increment = 5.f;
@ -733,11 +798,15 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
//obstacle at 10-30 deg world frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
for (int i = 2; i < 6 ; i++) {
int start = 2;
int end = 6;
for (int i = start; i <= end ; i++) {
obstacle_msg.distances[i] = 500;
}
//THEN: at initialization the internal obstacle map should only contain UINT16_MAX
int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
@ -750,11 +819,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
//THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) {
if (i == 1 || i == 2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= start && i <= end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
@ -762,44 +831,50 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
}
//WHEN: we add distance sensor data while vehicle yaw 90deg to the right
//WHEN: we add obstacle distance sensor data while vehicle yaw 90deg to the right
cp.test_addObstacleSensorData(obstacle_msg, M_PI_2);
//THEN: the correct bins in the map should be filled
int offset = bin_count - 90 / bin_size;
for (int i = 0; i < distances_array_size; i++) {
if (i == 28 || i == 29) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= offset + start && i <= offset + end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add distance sensor data while vehicle yaw 45deg to the left
//WHEN: we add obstacle distance sensor data while vehicle yaw 45deg to the left
cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4);
//THEN: the correct bins in the map should be filled
offset = 45 / bin_size;
for (int i = 0; i < distances_array_size; i++) {
if (i == 6 || i == 7) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= offset + start && i <= offset + end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add distance sensor data while vehicle yaw 180deg
//WHEN: we add obstacle distance sensor data while vehicle yaw 180deg
cp.test_addObstacleSensorData(obstacle_msg, M_PI);
//THEN: the correct bins in the map should be filled
offset = 180 / bin_size;
for (int i = 0; i < distances_array_size; i++) {
if (i == 19 || i == 20) {
if (i >= offset + start && i <= offset + end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
} else {
@ -811,11 +886,84 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude)
}
}
TEST_F(CollisionPreventionTest, addObstacleSensorData_offset_bodyframe)
{
// GIVEN: a vehicle attitude and obstacle distance message
TestCollisionPrevention cp;
obstacle_distance_s obstacle_msg {};
obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; // Body Frame
obstacle_msg.increment = 6.f;
obstacle_msg.min_distance = 20;
obstacle_msg.max_distance = 2000;
obstacle_msg.angle_offset = 0.f;
//obstacle at 363°-39° deg world frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment
obstacle_msg.distances[i] = 500;
}
//WHEN: we add distance sensor data
cp.test_addObstacleSensorData(obstacle_msg, 0.f);
//THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5°
int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
for (int i = 0; i < distances_array_size; i++) {
if (i >= 0 && i <= 8) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5°
obstacle_msg.angle_offset = 30.5f;
// This then means our obstacle is between 27.5° and 69.5°
cp.test_addObstacleSensorData(obstacle_msg, 0.f);
//THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5°
for (int i = 0; i < distances_array_size; i++) {
if (i >= 6 && i <= 14) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we increase the offset to -30.5°
obstacle_msg.angle_offset = -30.5f;
// This then means our obstacle is between 326.5° and 8.5°
cp.test_addObstacleSensorData(obstacle_msg, 0.f);
//THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5°
for (int i = 0; i < distances_array_size; i++) {
if (i >= 65 || i <= 2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
}
TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
{
// GIVEN: a vehicle attitude and obstacle distance message
TestCollisionPrevention cp;
cp.getObstacleMap().increment = 10.f;
obstacle_distance_s obstacle_msg {};
obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; //north aligned
obstacle_msg.increment = 5.f;
@ -825,8 +973,10 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
//obstacle at 10-30 deg body frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
int start = 2;
int end = 6;
for (int i = 2; i < 6 ; i++) {
for (int i = start; i <= end ; i++) {
obstacle_msg.distances[i] = 500;
}
@ -842,12 +992,13 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
cp.test_addObstacleSensorData(obstacle_msg, 0.f);
//THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) {
if (i == 1 || i == 2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= start && i <= end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
@ -859,11 +1010,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
//THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) {
if (i == 1 || i == 2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= start && i <= end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
@ -875,11 +1026,11 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
//THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) {
if (i == 1 || i == 2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= start && i <= end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
@ -891,16 +1042,17 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe)
//THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) {
if (i == 1 || i == 2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= start && i <= end) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
}
@ -908,7 +1060,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset)
{
// GIVEN: a vehicle attitude and obstacle distance message
TestCollisionPrevention cp;
cp.getObstacleMap().increment = 10.f;
obstacle_distance_s obstacle_msg {};
obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
obstacle_msg.increment = 6.f;
@ -916,42 +1067,62 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset)
obstacle_msg.max_distance = 2000;
obstacle_msg.angle_offset = 0.f;
//obstacle at 0-30 deg world frame, distance 5 meters
//obstacle at 363°-39° deg world frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
for (int i = 0; i < 5 ; i++) {
for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment
obstacle_msg.distances[i] = 500;
}
//WHEN: we add distance sensor data
cp.test_addObstacleSensorData(obstacle_msg, 0.f);
//THEN: the correct bins in the map should be filled
//THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5°
int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]);
for (int i = 0; i < distances_array_size; i++) {
if (i >= 0 && i <= 2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= 0 && i <= 8) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add distance sensor data with an angle offset
obstacle_msg.angle_offset = 30.f;
//WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5°
obstacle_msg.angle_offset = 30.5f;
// This then means our obstacle is between 27.5° and 69.5°
cp.test_addObstacleSensorData(obstacle_msg, 0.f);
//THEN: the correct bins in the map should be filled
//THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5°
for (int i = 0; i < distances_array_size; i++) {
if (i >= 3 && i <= 5) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
if (i >= 6 && i <= 14) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we increase the offset to -30.5°
obstacle_msg.angle_offset = -30.5f;
// This then means our obstacle is between 326.5° and 8.5°
cp.test_addObstacleSensorData(obstacle_msg, 0.f);
//THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5°
for (int i = 0; i < distances_array_size; i++) {
if (i >= 65 || i <= 2) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i;
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i;
}
//reset array to UINT16_MAX
@ -963,10 +1134,9 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
{
// GIVEN: a vehicle attitude and obstacle distance message
TestCollisionPrevention cp;
cp.getObstacleMap().increment = 10.f;
obstacle_distance_s obstacle_msg {};
obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
obstacle_msg.increment = 10.f;
obstacle_msg.increment = 5.f;
obstacle_msg.min_distance = 20;
obstacle_msg.max_distance = 2000;
obstacle_msg.angle_offset = 0.f;
@ -976,7 +1146,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
//obstacle at 0-30 deg world frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
for (int i = 0; i < 7 ; i++) {
for (int i = 0; i <= 6 ; i++) {
obstacle_msg.distances[i] = 500;
}
@ -1001,18 +1171,17 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
//THEN: the setpoint direction should be modified correctly
EXPECT_EQ(sp_index, 2);
EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262);
EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012);
EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f);
EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f);
}
TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
{
// GIVEN: a vehicle attitude and obstacle distance message
TestCollisionPrevention cp;
cp.getObstacleMap().increment = 10.f;
obstacle_distance_s obstacle_msg {};
obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned
obstacle_msg.increment = 10.f;
obstacle_msg.increment = 5.f;
obstacle_msg.min_distance = 20;
obstacle_msg.max_distance = 2000;
obstacle_msg.angle_offset = 0.f;
@ -1049,8 +1218,8 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
//THEN: the setpoint direction should be modified correctly
EXPECT_EQ(sp_index, 2);
EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262f);
EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012f);
EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f);
EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f);
}
TEST_F(CollisionPreventionTest, overlappingSensors)
@ -1146,16 +1315,15 @@ TEST_F(CollisionPreventionTest, enterData)
{
// GIVEN: a simple setup condition
TestCollisionPrevention cp;
cp.getObstacleMap().increment = 10.f;
Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
//THEN: just after initialization all bins are at UINT16_MAX and any data should be accepted
EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range
EXPECT_TRUE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range
EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range
EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range
EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range
EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range
EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range
EXPECT_TRUE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range
EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range
EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range
EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range
EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range
//WHEN: we add distance sensor data to the right with a valid reading
distance_sensor_s distance_sensor {};
@ -1167,36 +1335,38 @@ TEST_F(CollisionPreventionTest, enterData)
cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
//THEN: the internal map should contain the distance sensor readings
EXPECT_EQ(500, cp.getObstacleMap().distances[8]);
EXPECT_EQ(500, cp.getObstacleMap().distances[9]);
for (int i = 16; i < 20; i++) {
EXPECT_EQ(500, cp.getObstacleMap().distances[i]) << i;
}
//THEN: bins 8 & 9 contain valid readings
// a valid reading should only be accepted from sensors with shorter or equal range
// a out of range reading should only be accepted from sensors with the same range
EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range
EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range
EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range
EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range
EXPECT_FALSE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range
EXPECT_FALSE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range
EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range
EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range
EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range
EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range
EXPECT_FALSE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range
EXPECT_FALSE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range
//WHEN: we add distance sensor data to the right with an out of range reading
distance_sensor.current_distance = 21.f;
cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude);
//THEN: the internal map should contain the distance sensor readings
EXPECT_EQ(2000, cp.getObstacleMap().distances[8]);
EXPECT_EQ(2000, cp.getObstacleMap().distances[9]);
for (int i = 16; i < 20; i++) {
EXPECT_EQ(2000, cp.getObstacleMap().distances[i]) << i;
}
//THEN: bins 8 & 9 contain readings out of range
// a reading in range will be accepted in any case
// out of range readings will only be accepted from sensors with bigger or equal range
EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range
EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range
EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range
EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range
EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range
EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range
EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range
EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range
EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range
EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range
EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range
EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range
}

2
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@ -816,7 +816,7 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan)
void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan)
{
static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each
static constexpr int SECTOR_SIZE_DEG = 5; // PX4 Collision Prevention uses 5 degree sectors
double angle_min_deg = scan.angle_min() * 180 / M_PI;
double angle_step_deg = scan.angle_step() * 180 / M_PI;