From 5b24f28ac759bcb3d43dc69902639f1426acee25 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 4 Jun 2019 21:36:35 +0200 Subject: [PATCH] CollisionPrevention: map distance_sensor data to obstacle distance --- .../CollisionPrevention.cpp | 139 ++++++++++++++++-- .../CollisionPrevention.hpp | 7 + 2 files changed, 136 insertions(+), 10 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index fb304d2360..60fd7f85c7 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -45,7 +45,6 @@ using namespace time_literals; CollisionPrevention::CollisionPrevention(ModuleParams *parent) : ModuleParams(parent) { - } CollisionPrevention::~CollisionPrevention() @@ -78,12 +77,129 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se } } -void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, - const Vector2f &curr_pos, const Vector2f &curr_vel) +void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &obstacle) { _sub_obstacle_distance.update(); const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); + // Update with offboard data if the data is not stale + if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { + obstacle = obstacle_distance; + } +} + +void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) +{ + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + // _sub_distance_sensor[i].update(); + // const distance_sensor_s &distance_sensor = _sub_distance_sensor[i].get(); + 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) && + (distance_sensor.current_distance > distance_sensor.min_distance) && + (distance_sensor.current_distance < distance_sensor.max_distance)) { + + + if (obstacle.increment_f > 0.f || obstacle.increment > 0) { + // data from companion + obstacle.timestamp = math::min(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.increment_f = math::max(obstacle.increment_f, (float)obstacle.increment); + 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], UINT16_MAX, sizeof(obstacle.distances)); + obstacle.increment_f = math::degrees(distance_sensor.h_fov); + obstacle.angle_offset = 0.f; + } + + + // init offset for sensor orientation distance_sensor_s::ROTATION_FORWARD_FACING or with offset coming from the companion + float offset = obstacle.angle_offset > 0.f ? math::radians(obstacle.angle_offset) : 0.0f; + + switch (distance_sensor.orientation) { + case distance_sensor_s::ROTATION_RIGHT_FACING: + offset = M_PI_F / 2.0f; + break; + + case distance_sensor_s::ROTATION_LEFT_FACING: + offset = -M_PI_F / 2.0f; + break; + + case distance_sensor_s::ROTATION_BACKWARD_FACING: + offset = M_PI_F; + break; + + case distance_sensor_s::ROTATION_CUSTOM: + offset = Eulerf(Quatf(distance_sensor.q)).psi(); + break; + } + + matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); + // convert the sensor orientation from body to local frame + float sensor_orientation = math::degrees(wrap_pi(Eulerf(attitude).psi() + offset)); + + // convert orientation from range [-180, 180] to [0, 360] + if ((sensor_orientation <= FLT_EPSILON) || (sensor_orientation >= 180.0f)) { + sensor_orientation += 360.0f; + } + + // calculate the field of view boundary bin indices + int lower_bound = (int)floor((sensor_orientation - math::degrees(distance_sensor.h_fov / 2.0f)) / + obstacle.increment_f); + int upper_bound = (int)floor((sensor_orientation + math::degrees(distance_sensor.h_fov / 2.0f)) / + obstacle.increment_f); + + // if increment_f 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_f < 5.f) { + obstacle.angle_offset = sensor_orientation; + upper_bound = abs(upper_bound - lower_bound); + lower_bound = 0; + } + + 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_f) + 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 * cosf(Eulerf(attitude).theta()))); + } + } + } +} + +void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, + const Vector2f &curr_pos, const Vector2f &curr_vel) +{ + obstacle_distance_s obstacle = {}; + updateOffboardObstacleDistance(obstacle); + updateDistanceSensor(obstacle); + //The maximum velocity formula contains a square root, therefore the whole calculation is done with squared norms. //that way the root does not have to be calculated for every range bin but once at the end. float setpoint_length = setpoint.norm(); @@ -92,18 +208,21 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, //Limit the deviation of the adapted setpoint from the originally given joystick input (slightly less than 90 degrees) float max_slide_angle_rad = 0.5f; - if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { + if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) { if (setpoint_length > 0.001f) { - int distances_array_size = sizeof(obstacle_distance.distances) / sizeof(obstacle_distance.distances[0]); + int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]); for (int i = 0; i < distances_array_size; i++) { - //determine if distance bin is valid and contains a valid distance measurement - if (obstacle_distance.distances[i] < obstacle_distance.max_distance && - obstacle_distance.distances[i] > obstacle_distance.min_distance && i * obstacle_distance.increment < 360) { - float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters - float angle = math::radians((float)i * obstacle_distance.increment); + if (obstacle.distances[i] < obstacle.max_distance && + obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment_f < 360.f) { + float distance = obstacle.distances[i] / 100.0f; //convert to meters + float angle = math::radians((float)i * obstacle.increment_f); + + if (obstacle.angle_offset > 0.f) { + angle += math::radians(obstacle.angle_offset); + } //split current setpoint into parallel and orthogonal components with respect to the current bin direction Vector2f bin_direction = {cos(angle), sin(angle)}; diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index f75fc8e5c0..7bf703fbf4 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -44,8 +44,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -72,6 +74,8 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ + uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */ + uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500000}; static constexpr uint64_t MESSAGE_THROTTLE_US{5000000}; @@ -91,4 +95,7 @@ private: void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + void updateOffboardObstacleDistance(obstacle_distance_s &obstacle); + void updateDistanceSensor(obstacle_distance_s &obstacle); + };