Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Collision Prevention with distance sensor #12179

Merged
merged 19 commits into from
Jul 15, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
73531ea
distance_sensor: add horiontal and vertical fov, add quaternion for s…
mrivi Jun 4, 2019
4b2eb8b
obstacle_distance: add fields from mavlink extension
mrivi Jun 4, 2019
1c3f45a
simulator mavlink: add horizontal and vertical fov + quaterion orient…
mrivi Jun 4, 2019
5e45ecb
CollisionPrevention: map distance_sensor data to obstacle distance
mrivi Jun 4, 2019
77d3b4d
cm8jl65: add field of view
mrivi Jun 4, 2019
a8bfcba
cm8jl65: use paramter to set sensor orientation
mrivi Jun 4, 2019
f8ca8b4
Obstacle_distance: use only one increment in float directly
mrivi Jun 5, 2019
6b70021
CollisionPrevention: make sure that the timestamp is updated for dist…
mrivi Jun 5, 2019
bdd57a9
CollisionPrevention: refactor code to make it more readable
mrivi Jun 5, 2019
df0abc4
CollisionPrevention: make sure that vehicle tilt compensation is
mrivi Jun 13, 2019
a98b776
add uORB message obstacle_distance_fused with data from offboard
mrivi Jun 17, 2019
99ec505
stream mavlink message OBSTACLE DISTANCE
mrivi Jun 17, 2019
322572e
logger: log obstacle_distance_fused instead of obstacle_distance
mrivi Jun 25, 2019
494311f
CollisionPrevention: add failsafe for stale distance data
mrivi Jun 25, 2019
54049a0
CollisionPrevention: use FlightTasks convention for private/public me…
mrivi Jun 25, 2019
b5cffd1
mc_pos_control: increase stack size by 100 bytes
mrivi Jun 28, 2019
655d92c
CollisionPrevention: use the maximum timestamp between offboard and
mrivi Jul 1, 2019
e3eb5dc
CollisionPrevention: compute the attitude_sensor_frame outside for loop
mrivi Jul 8, 2019
f319813
CollisionPrevention: address https://github.com/PX4/Firmware/pull/12179
mrivi Jul 11, 2019
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions msg/distance_sensor.msg
Original file line number Diff line number Diff line change
Expand Up @@ -23,3 +23,8 @@ uint8 ROTATION_BACKWARD_FACING = 12 # MAV_SENSOR_ROTATION_PITCH_180
uint8 ROTATION_FORWARD_FACING = 0 # MAV_SENSOR_ROTATION_NONE
uint8 ROTATION_LEFT_FACING = 6 # MAV_SENSOR_ROTATION_YAW_270
uint8 ROTATION_RIGHT_FACING = 2 # MAV_SENSOR_ROTATION_YAW_90
uint8 ROTATION_CUSTOM =100 # MAV_SENSOR_ROTATION_CUSTOM

float32 h_fov # Sensor horizontal field of view (rad)
float32 v_fov # Sensor vertical field of view (rad)
float32[4] q # Quaterion sensor orientation with respect to the vehicle body frame to specify the orientation ROTATION_CUSTOM
6 changes: 5 additions & 1 deletion msg/obstacle_distance.msg
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,11 @@ uint8 MAV_DISTANCE_SENSOR_RADAR = 3

uint16[72] distances # Distance of obstacles around the UAV with index 0 corresponding to local North. A value of 0 means that the obstacle is right in front of the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm.

uint8 increment # Angular width in degrees of each array element.
float32 increment # Angular width in degrees of each array element.

uint16 min_distance # Minimum distance the sensor can measure in centimeters.
uint16 max_distance # Maximum distance the sensor can measure in centimeters.

float32 angle_offset # Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive values are offsets to the right.

# TOPICS obstacle_distance obstacle_distance_fused
18 changes: 14 additions & 4 deletions src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
*/

#include <poll.h>
#include <px4_cli.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
Expand Down Expand Up @@ -327,9 +328,10 @@ CM8JL65::collect()

bytes_read = OK;

distance_sensor_s report;
distance_sensor_s report = {};
report.current_distance = static_cast<float>(distance_mm) / 1000.0f;
report.id = 0; // TODO: set proper ID.
report.h_fov = 0.0488692f;
report.max_distance = _max_distance;
report.min_distance = _min_distance;
report.orientation = _rotation;
Expand Down Expand Up @@ -684,9 +686,17 @@ extern "C" __EXPORT int cm8jl65_main(int argc, char *argv[])

while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
break;
case 'R': {
int rot = -1;

if (px4_get_parameter_value(myoptarg, rot) != 0) {
PX4_ERR("rotation parsing failed");
return -1;
}

rotation = (uint8_t)rot;
break;
}

case 'd':
device_path = myoptarg;
Expand Down
22 changes: 21 additions & 1 deletion src/drivers/distance_sensor/cm8jl65/module.yaml
Original file line number Diff line number Diff line change
@@ -1,6 +1,26 @@
module_name: Lanbao PSK-CM8JL65-CC5
serial_config:
- command: cm8jl65 start -d ${SERIAL_DEV}
- command: cm8jl65 start -d ${SERIAL_DEV} -R p:SENS_CM8JL65_R_0
port_config_param:
name: SENS_CM8JL65_CFG
group: Sensors

parameters:
- group: Sensors
definitions:
SENS_CM8JL65_R_0:
description:
short: Distance Sensor Rotation
long: |
Distance Sensor Rotation as MAV_SENSOR_ORIENTATION enum

type: enum
values:
25: ROTATION_DOWNWARD_FACING
24: ROTATION_UPWARD_FACING
12: ROTATION_BACKWARD_FACING
0: ROTATION_FORWARD_FACING
6: ROTATION_LEFT_FACING
2: ROTATION_RIGHT_FACING
reboot_required: true
default: 25
169 changes: 153 additions & 16 deletions src/lib/CollisionPrevention/CollisionPrevention.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,6 @@ using namespace time_literals;
CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
ModuleParams(parent)
{

}

CollisionPrevention::~CollisionPrevention()
Expand All @@ -56,7 +55,7 @@ CollisionPrevention::~CollisionPrevention()
}
}

void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_setpoint,
void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint,
const Vector2f &adapted_setpoint)
{
collision_constraints_s constraints{}; /**< collision constraints message */
Expand All @@ -78,12 +77,121 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se
}
}

void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint,
const Vector2f &curr_pos, const Vector2f &curr_vel)
void CollisionPrevention::_publishObstacleDistance(obstacle_distance_s &obstacle)
{
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
if (_obstacle_distance_pub != nullptr) {
orb_publish(ORB_ID(obstacle_distance_fused), _obstacle_distance_pub, &obstacle);

} else {
_obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance_fused), &obstacle);
}
}

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++) {
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;
}

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

_publishObstacleDistance(obstacle);
}

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();
Expand All @@ -92,18 +200,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 < 360.f) {
float distance = obstacle.distances[i] / 100.0f; //convert to meters
float angle = math::radians((float)i * obstacle.increment);

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)};
Expand Down Expand Up @@ -153,9 +264,10 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint,
}
}

} else if (_last_message + MESSAGE_THROTTLE_US < hrt_absolute_time()) {
mavlink_log_critical(&_mavlink_log_pub, "No range data received");
_last_message = hrt_absolute_time();
} else {
// if distance data are stale, switch to Loiter
_publishVehicleCmdDoLoiter();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure if this is the best way to handle this.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@mhkabir what do you suggest?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

IMO this is the best way. If your sensors fail you don't want the drone to continue flying as per normal. Ideally I think this should happen only once per arm, but if that will complicate the architecture a lot this is fine, it just means that you need to disable collision prevention in flight if the sensors fail.

mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
}
}

Expand All @@ -164,7 +276,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
{
//calculate movement constraints based on range data
Vector2f new_setpoint = original_setpoint;
calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);

//warn user if collision prevention starts to interfere
bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
Expand All @@ -177,6 +289,31 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
}

_interfering = currently_interfering;
publishConstrainedSetpoint(original_setpoint, new_setpoint);
_publishConstrainedSetpoint(original_setpoint, new_setpoint);
original_setpoint = new_setpoint;
}

void CollisionPrevention::_publishVehicleCmdDoLoiter()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = (float)1; // base mode
command.param3 = (float)0; // sub mode
command.target_system = 1;
command.target_component = 1;
command.source_system = 1;
command.source_component = 1;
command.confirmation = false;
command.from_external = false;
command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;

// publish the vehicle command
if (_pub_vehicle_command == nullptr) {
_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
vehicle_command_s::ORB_QUEUE_LENGTH);

} else {
orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
}
}
Loading