dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Collision Prevention: support multiple sensors and frames (#12883) 

* build internal sensor map

* Extend testing coverage

* Update matrix library
This commit is contained in:
Tanja Baumann 2019-09-06 08:38:56 +02:00 committed by Julian Kent
parent eb81f4bce2
commit f3c5ca6015
7 changed files with 795 additions and 163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cmake/px4_add_common_flags.cmake
View File

@ -51,6 +51,7 @@ function(px4_add_common_flags)
-fdata-sections
-ffunction-sections
-fomit-frame-pointer
-fmerge-all-constants
#-funsafe-math-optimizations # Enables -fno-signed-zeros, -fno-trapping-math, -fassociative-math and -freciprocal-math
-fno-signed-zeros # Allow optimizations for floating-point arithmetic that ignore the signedness of zero

5
msg/obstacle_distance.msg
View File

@ -1,6 +1,11 @@
# Obstacle distances in front of the sensor.
uint64 timestamp # time since system start (microseconds)
uint8 frame #Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is North aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned.
uint8 MAV_FRAME_GLOBAL = 0
uint8 MAV_FRAME_LOCAL_NED = 1
uint8 MAV_FRAME_BODY_FRD = 12
uint8 sensor_type # Type from MAV_DISTANCE_SENSOR enum.
uint8 MAV_DISTANCE_SENSOR_LASER = 0
uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1

331
src/lib/CollisionPrevention/CollisionPrevention.cpp
View File

@ -43,11 +43,46 @@
using namespace matrix;
using namespace time_literals;
namespace
{
static const int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly
static const int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG;
float wrap_360(float f)
{
return wrap(f, 0.f, 360.f);
}
int wrap_bin(int i)
{
i = i % INTERNAL_MAP_USED_BINS;
while (i < 0) {
i += INTERNAL_MAP_USED_BINS;
}
return i;
}
}
CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
ModuleParams(parent)
{
static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5");
static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly");
//initialize internal obstacle map
_obstacle_map_body_frame.timestamp = getTime();
_obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG;
_obstacle_map_body_frame.min_distance = UINT16_MAX;
_obstacle_map_body_frame.max_distance = 0;
_obstacle_map_body_frame.angle_offset = 0.f;
uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]);
uint64_t current_time = getTime();
for (uint32_t i = 0 ; i < internal_bins; i++) {
_data_timestamps[i] = current_time;
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
}
}
CollisionPrevention::~CollisionPrevention()
@ -58,171 +93,215 @@ CollisionPrevention::~CollisionPrevention()
}
}
void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle)
hrt_abstime CollisionPrevention::getTime()
{
_sub_obstacle_distance.update();
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
return hrt_absolute_time();
}
// Update with offboard data if the data is not stale
if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) {
obstacle = obstacle_distance;
hrt_abstime CollisionPrevention::getElapsedTime(const hrt_abstime *ptr)
{
return hrt_absolute_time() - *ptr;
}
void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle,
const matrix::Quatf &vehicle_attitude)
{
int msg_index = 0;
float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi());
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 < INTERNAL_MAP_USED_BINS; i++) {
float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset;
msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor);
//add all data points inside to FOV
if (obstacle.distances[msg_index] != UINT16_MAX) {
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
}
}
} else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) {
//Obstacle message arrives in body frame (front aligned)
//corresponding data index (shift by msg offset)
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG +
_obstacle_map_body_frame.angle_offset;
msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor);
//add all data points inside to FOV
if (obstacle.distances[msg_index] != UINT16_MAX) {
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
}
}
} else {
mavlink_log_critical(&_mavlink_log_pub, "Obstacle message received in unsupported frame %.0f\n",
(double)obstacle.frame);
}
}
void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
void CollisionPrevention::_updateObstacleMap()
{
_sub_vehicle_attitude.update();
// add distance sensor data
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
distance_sensor_s distance_sensor {};
_sub_distance_sensor[i].copy(&distance_sensor);
// consider only instaces with updated, valid data and orientations useful for collision prevention
if ((hrt_elapsed_time(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
(distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
(distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
//if a new distance sensor message has arrived
if (_sub_distance_sensor[i].updated()) {
distance_sensor_s distance_sensor {};
_sub_distance_sensor[i].copy(&distance_sensor);
// consider only instances with valid data and orientations useful for collision prevention
if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
(distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
(distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
if (obstacle.increment > 0) {
// data from companion
obstacle.timestamp = math::max(obstacle.timestamp, distance_sensor.timestamp);
obstacle.max_distance = math::max((int)obstacle.max_distance,
(int)distance_sensor.max_distance * 100);
obstacle.min_distance = math::min((int)obstacle.min_distance,
(int)distance_sensor.min_distance * 100);
// since the data from the companion are already in the distances data structure,
// keep the increment that is sent
obstacle.angle_offset = 0.f; //companion not sending this field (needs mavros update)
//update message description
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp);
_obstacle_map_body_frame.max_distance = math::max((int)_obstacle_map_body_frame.max_distance,
(int)distance_sensor.max_distance * 100);
_obstacle_map_body_frame.min_distance = math::min((int)_obstacle_map_body_frame.min_distance,
(int)distance_sensor.min_distance * 100);
} else {
obstacle.timestamp = distance_sensor.timestamp;
obstacle.max_distance = distance_sensor.max_distance * 100; // convert to cm
obstacle.min_distance = distance_sensor.min_distance * 100; // convert to cm
memset(&obstacle.distances[0], 0xff, sizeof(obstacle.distances));
obstacle.increment = math::degrees(distance_sensor.h_fov);
obstacle.angle_offset = 0.f;
}
if ((distance_sensor.current_distance > distance_sensor.min_distance) &&
(distance_sensor.current_distance < distance_sensor.max_distance)) {
float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset);
matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
// convert the sensor orientation from body to local frame in the range [0, 360]
float sensor_yaw_local_deg = math::degrees(wrap_2pi(Eulerf(attitude).psi() + sensor_yaw_body_rad));
// calculate the field of view boundary bin indices
int lower_bound = (int)floor((sensor_yaw_local_deg - math::degrees(distance_sensor.h_fov / 2.0f)) /
obstacle.increment);
int upper_bound = (int)floor((sensor_yaw_local_deg + math::degrees(distance_sensor.h_fov / 2.0f)) /
obstacle.increment);
// if increment is lower than 5deg, use an offset
const int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);
if (((lower_bound < 0 || upper_bound < 0) || (lower_bound >= distances_array_size
|| upper_bound >= distances_array_size)) && obstacle.increment < 5.f) {
obstacle.angle_offset = sensor_yaw_local_deg ;
upper_bound = abs(upper_bound - lower_bound);
lower_bound = 0;
}
// rotate vehicle attitude into the sensor body frame
matrix::Quatf attitude_sensor_frame = attitude;
attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
float attitude_sensor_frame_pitch = cosf(Eulerf(attitude_sensor_frame).theta());
for (int bin = lower_bound; bin <= upper_bound; ++bin) {
int wrap_bin = bin;
if (wrap_bin < 0) {
// wrap bin index around the array
wrap_bin = (int)floor(360.f / obstacle.increment) + bin;
}
if (wrap_bin >= distances_array_size) {
// wrap bin index around the array
wrap_bin = bin - distances_array_size;
}
// compensate measurement for vehicle tilt and convert to cm
obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin],
(int)(100 * distance_sensor.current_distance * attitude_sensor_frame_pitch));
}
_addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q));
}
}
}
// add obstacle distance data
if (_sub_obstacle_distance.update()) {
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
// Update map with obstacle data if the data is not stale
if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) {
//update message description
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp);
_obstacle_map_body_frame.max_distance = math::max((int)_obstacle_map_body_frame.max_distance,
(int)obstacle_distance.max_distance);
_obstacle_map_body_frame.min_distance = math::min((int)_obstacle_map_body_frame.min_distance,
(int)obstacle_distance.min_distance);
_addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q));
}
}
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
_obstacle_distance_pub.publish(obstacle);
_obstacle_distance_pub.publish(_obstacle_map_body_frame);
}
void CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
const matrix::Quatf &vehicle_attitude)
{
//clamp at maximum sensor range
float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance);
//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_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)) /
INTERNAL_MAP_INCREMENT_DEG);
int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) /
INTERNAL_MAP_INCREMENT_DEG);
//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
matrix::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());
for (int bin = lower_bound; bin <= upper_bound; ++bin) {
int wrapped_bin = wrap_bin(bin);
// compensate measurement for vehicle tilt and convert to cm
_obstacle_map_body_frame.distances[wrapped_bin] = (int)(100 * distance_reading * sensor_dist_scale);
_data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp;
}
}
}
void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
const Vector2f &curr_pos, const Vector2f &curr_vel)
{
obstacle_distance_s obstacle{};
_updateOffboardObstacleDistance(obstacle);
_updateDistanceSensor(obstacle);
_updateObstacleMap();
float setpoint_length = setpoint.norm();
//read parameters
float col_prev_d = _param_mpc_col_prev_d.get();
float col_prev_dly = _param_mpc_col_prev_dly.get();
float col_prev_ang_rad = math::radians(_param_mpc_col_prev_ang.get());
float xy_p = _param_mpc_xy_p.get();
float max_jerk = _param_mpc_jerk_max.get();
float max_accel = _param_mpc_acc_hor.get();
matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
float vehicle_yaw_angle_rad = Eulerf(attitude).psi();
if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) {
float setpoint_length = setpoint.norm();
if (getElapsedTime(&_obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
if (setpoint_length > 0.001f) {
Vector2f setpoint_dir = setpoint / setpoint_length;
float vel_max = setpoint_length;
int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);
float min_dist_to_keep = math::max(obstacle.min_distance / 100.0f, col_prev_d);
float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d);
for (int i = 0; i < distances_array_size; i++) {
float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
if ((float)i * obstacle.increment < 360.f) { //disregard unused bins at the end of the message
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { //disregard unused bins at the end of the message
float distance = obstacle.distances[i] / 100.0f; //convert to meters
float angle = math::radians((float)i * obstacle.increment);
//delete stale values
if (getElapsedTime(&_data_timestamps[i]) > RANGE_STREAM_TIMEOUT_US) {
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
}
if (obstacle.angle_offset > 0.f) {
angle += math::radians(obstacle.angle_offset);
float distance = _obstacle_map_body_frame.distances[i] * 0.01f; //convert to meters
float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
// convert from body to local frame in the range [0, 2*pi]
angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
//get direction of current bin
Vector2f bin_direction = {cos(angle), sin(angle)};
if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance
&& _obstacle_map_body_frame.distances[i] < UINT16_MAX) {
if (setpoint_dir.dot(bin_direction) > 0
&& setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) {
//calculate max allowed velocity with a P-controller (same gain as in the position controller)
float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
float delay_distance = curr_vel_parallel * col_prev_dly;
float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
float vel_max_posctrl = xy_p * stop_distance;
float vel_max_smooth = trajmath::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
Vector2f vel_max_vec = bin_direction * math::min(vel_max_posctrl, vel_max_smooth);
float vel_max_bin = vel_max_vec.dot(setpoint_dir);
//constrain the velocity
if (vel_max_bin >= 0) {
vel_max = math::min(vel_max, vel_max_bin);
}
}
//get direction of current bin
Vector2f bin_direction = {cos(angle), sin(angle)};
if (obstacle.distances[i] < obstacle.max_distance &&
obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
if (setpoint_dir.dot(bin_direction) > 0
&& setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) {
//calculate max allowed velocity with a P-controller (same gain as in the position controller)
float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
float delay_distance = curr_vel_parallel * col_prev_dly;
float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
float vel_max_posctrl = xy_p * stop_distance;
float vel_max_smooth = trajmath::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);
Vector2f vel_max_vec = bin_direction * math::min(vel_max_posctrl, vel_max_smooth);
float vel_max_bin = vel_max_vec.dot(setpoint_dir);
//constrain the velocity
if (vel_max_bin >= 0) {
vel_max = math::min(vel_max, vel_max_bin);
}
}
} else if (obstacle.distances[i] == UINT16_MAX) {
float sp_bin = setpoint_dir.dot(bin_direction);
float ang_half_bin = cosf(math::radians(obstacle.increment) / 2.f);
//if the setpoint lies outside the FOV set velocity to zero
if (sp_bin > ang_half_bin) {
vel_max = 0.f;
}
}
} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
vel_max = 0.f;
}
}

39
src/lib/CollisionPrevention/CollisionPrevention.hpp
View File

@ -64,7 +64,7 @@ class CollisionPrevention : public ModuleParams
public:
CollisionPrevention(ModuleParams *parent);
~CollisionPrevention();
virtual ~CollisionPrevention();
/**
* Returs true if Collision Prevention is running
*/
@ -80,6 +80,23 @@ public:
void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
protected:
obstacle_distance_s _obstacle_map_body_frame {};
uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])];
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude);
/**
* Updates obstacle distance message with measurement from offboard
* @param obstacle, obstacle_distance message to be updated
*/
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude);
virtual hrt_abstime getTime();
virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr);
private:
bool _interfering{false}; /**< states if the collision prevention interferes with the user input */
@ -136,6 +153,8 @@ private:
return offset;
}
/**
* Computes collision free setpoints
* @param setpoint, setpoint before collision prevention intervention
@ -146,16 +165,22 @@ private:
const matrix::Vector2f &curr_vel);
/**
* Updates obstacle distance message with measurement from offboard
* @param obstacle, obstacle_distance message to be updated
* Publishes collision_constraints message
* @param original_setpoint, setpoint before collision prevention intervention
* @param adapted_setpoint, collision prevention adaped setpoint
*/
void _updateOffboardObstacleDistance(obstacle_distance_s &obstacle);
void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
/**
* Updates obstacle distance message with measurement from distance sensors
* @param obstacle, obstacle_distance message to be updated
* Publishes obstacle_distance message with fused data from offboard and from distance sensors
* @param obstacle, obstacle_distance message to be publsihed
*/
void _updateDistanceSensor(obstacle_distance_s &obstacle);
void _publishObstacleDistance(obstacle_distance_s &obstacle);
/**
* Aggregates the sensor data into a internal obstacle map in body frame
*/
void _updateObstacleMap();
/**
* Publishes vehicle command.

579
src/lib/CollisionPrevention/CollisionPreventionTest.cpp
View File

@ -35,6 +35,7 @@
#include <CollisionPrevention/CollisionPrevention.hpp>
// to run: make tests TESTFILTER=CollisionPrevention
hrt_abstime mocked_time = 0;
class CollisionPreventionTest : public ::testing::Test
{
@ -50,6 +51,31 @@ class TestCollisionPrevention : public CollisionPrevention
public:
TestCollisionPrevention() : CollisionPrevention(nullptr) {}
void paramsChanged() {CollisionPrevention::updateParamsImpl();}
obstacle_distance_s &getObstacleMap() {return _obstacle_map_body_frame;}
void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude)
{
_addDistanceSensorData(distance_sensor, attitude);
}
void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &attitude)
{
_addObstacleSensorData(obstacle, attitude);
}
};
class TestTimingCollisionPrevention : public TestCollisionPrevention
{
public:
TestTimingCollisionPrevention() : TestCollisionPrevention() {}
protected:
hrt_abstime getTime() override
{
return mocked_time;
}
hrt_abstime getElapsedTime(const hrt_abstime *ptr) override
{
return mocked_time - *ptr;
}
};
TEST_F(CollisionPreventionTest, instantiation) { CollisionPrevention cp(nullptr); }
@ -58,20 +84,12 @@ TEST_F(CollisionPreventionTest, behaviorOff)
{
// GIVEN: a simple setup condition
TestCollisionPrevention cp;
matrix::Vector2f original_setpoint(10, 0);
float max_speed = 3.f;
matrix::Vector2f curr_pos(0, 0);
matrix::Vector2f curr_vel(2, 0);
// WHEN: we check if the setpoint should be modified
matrix::Vector2f modified_setpoint = original_setpoint;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
// THEN: it should be the same
EXPECT_EQ(original_setpoint, modified_setpoint);
// THEN: the collision prevention should be turned off by default
EXPECT_FALSE(cp.is_active());
}
TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing)
TEST_F(CollisionPreventionTest, noSensorData)
{
// GIVEN: a simple setup condition
TestCollisionPrevention cp;
@ -83,7 +101,6 @@ TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing)
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
// WHEN: we set the parameter check then apply the setpoint modification
float value = 10; // try to keep 10m away from obstacles
param_set(param, &value);
@ -92,18 +109,26 @@ TEST_F(CollisionPreventionTest, withoutObstacleMessageNothing)
matrix::Vector2f modified_setpoint = original_setpoint;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
// THEN: it shouldn't interfere with the setpoint, because there isn't an obstacle
EXPECT_EQ(original_setpoint, modified_setpoint);
// THEN: collision prevention should be enabled and limit the speed to zero
EXPECT_TRUE(cp.is_active());
EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm());
}
TEST_F(CollisionPreventionTest, testBehaviorOnWithAnObstacle)
TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage)
{
// GIVEN: a simple setup condition
TestCollisionPrevention cp;
matrix::Vector2f original_setpoint(10, 0);
matrix::Vector2f original_setpoint1(10, 0);
matrix::Vector2f original_setpoint2(-10, 0);
float max_speed = 3;
matrix::Vector2f curr_pos(0, 0);
matrix::Vector2f curr_vel(2, 0);
vehicle_attitude_s attitude;
attitude.timestamp = hrt_absolute_time();
attitude.q[0] = 1.0f;
attitude.q[1] = 0.0f;
attitude.q[2] = 0.0f;
attitude.q[3] = 0.0f;
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
@ -114,25 +139,180 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithAnObstacle)
// AND: an obstacle message
obstacle_distance_s message;
memset(&message, 0xDEAD, sizeof(message));
message.frame = message.MAV_FRAME_GLOBAL; //north aligned
message.min_distance = 100;
message.max_distance = 1000;
message.max_distance = 10000;
message.angle_offset = 0;
message.timestamp = hrt_absolute_time();
int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
message.increment = 360 / distances_array_size;
for (int i = 0; i < distances_array_size; i++) {
message.distances[i] = 101;
if (i < 10) {
message.distances[i] = 101;
} else {
message.distances[i] = 10001;
}
}
// WHEN: we publish the message and set the parameter and then run the setpoint modification
orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude);
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
matrix::Vector2f modified_setpoint = original_setpoint;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
matrix::Vector2f modified_setpoint1 = original_setpoint1;
matrix::Vector2f modified_setpoint2 = original_setpoint2;
cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel);
cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel);
orb_unadvertise(obstacle_distance_pub);
orb_unadvertise(vehicle_attitude_pub);
// THEN: it should be cut down to zero
EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1);
// THEN: the internal map should know the obstacle
// case 1: the velocity setpoint should be cut down to zero
// case 2: the velocity setpoint should stay the same as the input
EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100);
EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1);
EXPECT_EQ(original_setpoint2, modified_setpoint2);
}
TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
{
// GIVEN: a simple setup condition
TestCollisionPrevention cp;
matrix::Vector2f original_setpoint1(10, 0);
matrix::Vector2f original_setpoint2(-10, 0);
float max_speed = 3;
matrix::Vector2f curr_pos(0, 0);
matrix::Vector2f curr_vel(2, 0);
vehicle_attitude_s attitude;
attitude.timestamp = hrt_absolute_time();
attitude.q[0] = 1.0f;
attitude.q[1] = 0.0f;
attitude.q[2] = 0.0f;
attitude.q[3] = 0.0f;
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
// AND: an obstacle message
distance_sensor_s message;
message.timestamp = hrt_absolute_time();
message.min_distance = 1.f;
message.max_distance = 100.f;
message.current_distance = 1.1f;
message.variance = 0.1f;
message.signal_quality = 100;
message.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
message.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
message.h_fov = math::radians(50.f);
message.v_fov = math::radians(30.f);
// WHEN: we publish the message and set the parameter and then run the setpoint modification
orb_advert_t distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &message);
orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude);
orb_publish(ORB_ID(distance_sensor), distance_sensor_pub, &message);
orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
//WHEN: We run the setpoint modification
matrix::Vector2f modified_setpoint1 = original_setpoint1;
matrix::Vector2f modified_setpoint2 = original_setpoint2;
cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel);
cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel);
orb_unadvertise(distance_sensor_pub);
orb_unadvertise(vehicle_attitude_pub);
// THEN: the internal map should know the obstacle
// case 1: the velocity setpoint should be cut down to zero because there is an obstacle
// case 2: the velocity setpoint should be cut down to zero because there is no data
EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100);
EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000);
EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1);
EXPECT_FLOAT_EQ(0.f, modified_setpoint2.norm()) << modified_setpoint2(0) << "," << modified_setpoint2(1);
}
TEST_F(CollisionPreventionTest, testPurgeOldData)
{
// GIVEN: a simple setup condition
hrt_abstime start_time = hrt_absolute_time();
mocked_time = start_time;
TestTimingCollisionPrevention cp;
matrix::Vector2f original_setpoint(10, 0);
float max_speed = 3;
matrix::Vector2f curr_pos(0, 0);
matrix::Vector2f curr_vel(2, 0);
vehicle_attitude_s attitude;
attitude.timestamp = start_time;
attitude.q[0] = 1.0f;
attitude.q[1] = 0.0f;
attitude.q[2] = 0.0f;
attitude.q[3] = 0.0f;
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
// AND: an obstacle message
obstacle_distance_s message, message_empty;
memset(&message, 0xDEAD, sizeof(message));
message.frame = message.MAV_FRAME_GLOBAL; //north aligned
message.min_distance = 100;
message.max_distance = 10000;
message.angle_offset = 0;
message.timestamp = start_time;
int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
message.increment = 360 / distances_array_size;
message_empty = message;
for (int i = 0; i < distances_array_size; i++) {
if (i < 10) {
message.distances[i] = 10001;
} else {
message.distances[i] = UINT16_MAX;
}
message_empty.distances[i] = UINT16_MAX;
}
// WHEN: we publish the message and set the parameter and then run the setpoint modification
orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude);
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
for (int i = 0; i < 10; i++) {
matrix::Vector2f modified_setpoint = original_setpoint;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
mocked_time = mocked_time + 100000; //advance time by 0.1 seconds
message_empty.timestamp = mocked_time;
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message_empty);
if (i < 6) {
// THEN: If the data is new enough, the velocity setpoint should stay the same as the input
EXPECT_EQ(original_setpoint, modified_setpoint);
} else {
// THEN: If the data is expired, the velocity setpoint should be cut down to zero because there is no data
EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1);
}
}
orb_unadvertise(obstacle_distance_pub);
orb_unadvertise(vehicle_attitude_pub);
}
TEST_F(CollisionPreventionTest, noBias)
@ -156,6 +336,7 @@ TEST_F(CollisionPreventionTest, noBias)
message.min_distance = 100;
message.max_distance = 2000;
message.timestamp = hrt_absolute_time();
message.frame = message.MAV_FRAME_GLOBAL; //north aligned
int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
message.increment = 360 / distances_array_size;
@ -192,14 +373,17 @@ TEST_F(CollisionPreventionTest, outsideFOV)
// AND: an obstacle message
obstacle_distance_s message;
memset(&message, 0xDEAD, sizeof(message));
message.frame = message.MAV_FRAME_GLOBAL; //north aligned
message.min_distance = 100;
message.max_distance = 2000;
int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
message.increment = 360 / distances_array_size;
message.increment = 360.f / distances_array_size;
//fov from i = 1/4 * distances_array_size to i = 3/4 * distances_array_size
//fov from 45deg to 225deg
for (int i = 0; i < distances_array_size; i++) {
if (i > 0.25f * distances_array_size && i < 0.75f * distances_array_size) {
float angle = i * message.increment;
if (angle > 45.f && angle < 225.f) {
message.distances[i] = 700;
} else {
@ -212,14 +396,15 @@ TEST_F(CollisionPreventionTest, outsideFOV)
for (int i = 0; i < distances_array_size; i++) {
float angle = math::radians((float)i * message.increment);
matrix::Vector2f original_setpoint = {10.f *(float)cos(angle), 10.f *(float)sin(angle)};
float angle_deg = (float)i * message.increment;
float angle_rad = math::radians(angle_deg);
matrix::Vector2f original_setpoint = {10.f *(float)cos(angle_rad), 10.f *(float)sin(angle_rad)};
matrix::Vector2f modified_setpoint = original_setpoint;
message.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
if (i > 0.25f * distances_array_size && i < 0.75f * distances_array_size) {
if (angle_deg > 50.f && angle_deg < 230.f) {
// THEN: inside the FOV the setpoint should be limited
EXPECT_GT(modified_setpoint.norm(), 0.f);
EXPECT_LT(modified_setpoint.norm(), 10.f);
@ -234,7 +419,6 @@ TEST_F(CollisionPreventionTest, outsideFOV)
orb_unadvertise(obstacle_distance_pub);
}
TEST_F(CollisionPreventionTest, jerkLimit)
{
// GIVEN: a simple setup condition
@ -256,6 +440,7 @@ TEST_F(CollisionPreventionTest, jerkLimit)
message.min_distance = 100;
message.max_distance = 2000;
message.timestamp = hrt_absolute_time();
message.frame = message.MAV_FRAME_GLOBAL; //north aligned
int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
message.increment = 360 / distances_array_size;
@ -283,3 +468,339 @@ TEST_F(CollisionPreventionTest, jerkLimit)
// 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());
}
TEST_F(CollisionPreventionTest, addDistanceSensorData)
{
// GIVEN: a vehicle attitude and a distance sensor message
TestCollisionPrevention cp;
cp.getObstacleMap().increment = 10.f;
matrix::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;
//THEN: at initialization the internal obstacle map should only contain UINT16_MAX
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);
}
//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);
//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);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
}
//WHEN: we add additionally distance sensor data to the left
distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING;
distance_sensor.h_fov = math::radians(50.f);
distance_sensor.current_distance = 8.f;
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 == 8 || i == 9) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
} else if (i >= 24 && i <= 29) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
}
//WHEN: we add additionally distance sensor data to the front
distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
distance_sensor.h_fov = math::radians(10.1f);
distance_sensor.current_distance = 3.f;
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 == 8 || i == 9) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
} else if (i >= 24 && i <= 29) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800);
} else if (i == 0) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
}
}
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;
obstacle_msg.min_distance = 20;
obstacle_msg.max_distance = 2000;
obstacle_msg.angle_offset = 0.f;
matrix::Quaternion<float> vehicle_attitude1(1, 0, 0, 0); //unit transform
matrix::Euler<float> attitude2_euler(0, 0, M_PI / 2.0);
matrix::Quaternion<float> vehicle_attitude2(attitude2_euler); //90 deg yaw
matrix::Euler<float> attitude3_euler(0, 0, -M_PI / 4.0);
matrix::Quaternion<float> vehicle_attitude3(attitude3_euler); // -45 deg yaw
matrix::Euler<float> attitude4_euler(0, 0, M_PI);
matrix::Quaternion<float> vehicle_attitude4(attitude4_euler); // 180 deg yaw
//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++) {
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]);
for (int i = 0; i < distances_array_size; i++) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//WHEN: we add distance sensor data while vehicle has zero yaw
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1);
//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);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add distance sensor data while vehicle yaw 90deg to the right
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2);
//THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) {
if (i == 28 || i == 29) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add distance sensor data while vehicle yaw 45deg to the left
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3);
//THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) {
if (i == 6 || i == 7) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add distance sensor data while vehicle yaw 180deg
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4);
//THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) {
if (i == 19 || i == 20) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//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;
obstacle_msg.min_distance = 20;
obstacle_msg.max_distance = 2000;
obstacle_msg.angle_offset = 0.f;
matrix::Quaternion<float> vehicle_attitude1(1, 0, 0, 0); //unit transform
matrix::Euler<float> attitude2_euler(0, 0, M_PI / 2.0);
matrix::Quaternion<float> vehicle_attitude2(attitude2_euler); //90 deg yaw
matrix::Euler<float> attitude3_euler(0, 0, -M_PI / 4.0);
matrix::Quaternion<float> vehicle_attitude3(attitude3_euler); // -45 deg yaw
matrix::Euler<float> attitude4_euler(0, 0, M_PI);
matrix::Quaternion<float> vehicle_attitude4(attitude4_euler); // 180 deg yaw
//obstacle at 10-30 deg body frame, distance 5 meters
memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances));
for (int i = 2; i < 6 ; 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]);
for (int i = 0; i < distances_array_size; i++) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//WHEN: we add obstacle data while vehicle has zero yaw
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1);
//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);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add obstacle data while vehicle yaw 90deg to the right
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2);
//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);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add obstacle data while vehicle yaw 45deg to the left
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3);
//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);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
//WHEN: we add obstacle data while vehicle yaw 180deg
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4);
//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);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
}
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;
obstacle_msg.min_distance = 20;
obstacle_msg.max_distance = 2000;
obstacle_msg.angle_offset = 0.f;
matrix::Quaternion<float> vehicle_attitude(1, 0, 0, 0); //unit transform
//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 < 5 ; i++) {
obstacle_msg.distances[i] = 500;
}
//WHEN: we add distance sensor data
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude);
//THEN: the correct bins in the map should be filled
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);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//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;
cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude);
//THEN: the correct bins in the map should be filled
for (int i = 0; i < distances_array_size; i++) {
if (i >= 3 && i <= 5) {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500);
} else {
EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX);
}
//reset array to UINT16_MAX
cp.getObstacleMap().distances[i] = UINT16_MAX;
}
}

2
src/lib/matrix

@ -1 +1 @@
Subproject commit cc084e0791535e8426780e6a4f05794804db1b82
Subproject commit 22bf63cb714156eafd8a6d3822b903eba4980a8a

1
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1685,6 +1685,7 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
obstacle_distance.min_distance = mavlink_obstacle_distance.min_distance;
obstacle_distance.max_distance = mavlink_obstacle_distance.max_distance;
obstacle_distance.angle_offset = mavlink_obstacle_distance.angle_offset;
obstacle_distance.frame = mavlink_obstacle_distance.frame;
_obstacle_distance_pub.publish(obstacle_distance);
}