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

CollisionPrevention: map distance_sensor data to obstacle distance

This commit is contained in:
Martina Rivizzigno 2019-06-04 21:36:35 +02:00 committed by Beat Küng
parent aff131774e
commit 5b24f28ac7
2 changed files with 136 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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)};

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

@ -44,8 +44,10 @@
#include <px4_module_params.h>
#include <float.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/collision_constraints.h>
#include <uORB/topics/vehicle_attitude.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/mavlink_log.h>
@ -72,6 +74,8 @@ private:
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
uORB::SubscriptionData<obstacle_distance_s> _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<vehicle_attitude_s> _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);
};