diff --git a/cmake/px4_add_common_flags.cmake b/cmake/px4_add_common_flags.cmake index a13f6ce47a..4f5507bf5a 100644 --- a/cmake/px4_add_common_flags.cmake +++ b/cmake/px4_add_common_flags.cmake @@ -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 diff --git a/msg/obstacle_distance.msg b/msg/obstacle_distance.msg index 73e9d3bc22..e3c4963ab2 100644 --- a/msg/obstacle_distance.msg +++ b/msg/obstacle_distance.msg @@ -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 diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 6bc246e038..21ee50c0cc 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -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::_updateDistanceSensor(obstacle_distance_s &obstacle) +void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, + const matrix::Quatf &vehicle_attitude) { - 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 (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) - - } 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; + 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; } - 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)); + } 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; + } - // 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]); + } else { + mavlink_log_critical(&_mavlink_log_pub, "Obstacle message received in unsupported frame %.0f\n", + (double)obstacle.frame); + } +} - 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; - } +void CollisionPrevention::_updateObstacleMap() +{ + _sub_vehicle_attitude.update(); - // 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()); + // add distance sensor data + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - for (int bin = lower_bound; bin <= upper_bound; ++bin) { - int wrap_bin = bin; + //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); - if (wrap_bin < 0) { - // wrap bin index around the array - wrap_bin = (int)floor(360.f / obstacle.increment) + bin; - } + // 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 (wrap_bin >= distances_array_size) { - // wrap bin index around the array - wrap_bin = bin - distances_array_size; - } + //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); - // 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); - //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); - } - } + // convert from body to local frame in the range [0, 2*pi] + angle = wrap_2pi(vehicle_yaw_angle_rad + angle); - } 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); + //get direction of current bin + Vector2f bin_direction = {cos(angle), sin(angle)}; - //if the setpoint lies outside the FOV set velocity to zero - if (sp_bin > ang_half_bin) { - vel_max = 0.f; - } + 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); + } } + + } else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) { + vel_max = 0.f; } } diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 648cd2b038..31a0efaa48 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -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 _publishObstacleDistance(obstacle_distance_s &obstacle); + + /** + * Aggregates the sensor data into a internal obstacle map in body frame */ - void _updateDistanceSensor(obstacle_distance_s &obstacle); + void _updateObstacleMap(); /** * Publishes vehicle command. diff --git a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp index 95886907c2..31a95824e5 100644 --- a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp +++ b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp @@ -35,6 +35,7 @@ #include // 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: 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); - // THEN: it should be cut down to zero - EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1); + } 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 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 vehicle_attitude1(1, 0, 0, 0); //unit transform + matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); + matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw + matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); + matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw + matrix::Euler attitude4_euler(0, 0, M_PI); + matrix::Quaternion 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 vehicle_attitude1(1, 0, 0, 0); //unit transform + matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); + matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw + matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); + matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw + matrix::Euler attitude4_euler(0, 0, M_PI); + matrix::Quaternion 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 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; + } +} diff --git a/src/lib/matrix b/src/lib/matrix index cc084e0791..22bf63cb71 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit cc084e0791535e8426780e6a4f05794804db1b82 +Subproject commit 22bf63cb714156eafd8a6d3822b903eba4980a8a diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d6ca4fa728..7a8b46823c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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); }