Skip to content

Commit

Permalink
slight refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
@Claudio-Chies
Claudio-Chies committed Oct 1, 2024
1 parent 045c131 commit 476cc67
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 28 deletions.
37 changes: 17 additions & 20 deletions src/lib/collision_prevention/CollisionPrevention.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,6 @@
#include "CollisionPrevention.hpp"
#include <px4_platform_common/events.h>

using namespace matrix;
using namespace time_literals;

namespace
{
Expand Down Expand Up @@ -113,7 +111,7 @@ bool CollisionPrevention::is_active()
}

void
CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude)
CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude)
{
int msg_index = 0;
float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi());
Expand Down Expand Up @@ -245,7 +243,7 @@ CollisionPrevention::_updateObstacleMap()
}

void
CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude)
CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude)
{
// clamp at maximum sensor range
float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance);
Expand All @@ -268,7 +266,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
if (upper_bound < 0) { upper_bound++; }

// rotate vehicle attitude into the sensor body frame
matrix::Quatf attitude_sensor_frame = vehicle_attitude;
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());

Expand All @@ -294,7 +292,6 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
void
CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
{
const float col_prev_d = _param_cp_dist.get();
const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG);
const int sp_index_original = setpoint_index;
float best_cost = 9999.f;
Expand All @@ -310,7 +307,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
int bin = wrap_bin(j);

if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
mean_dist += col_prev_d * 100.f;
mean_dist += _param_cp_dist.get() * 100.f;

} else {
mean_dist += _obstacle_map_body_frame.distances[bin];
Expand All @@ -319,7 +316,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi

const int bin = wrap_bin(i);
mean_dist = mean_dist / (2.f * filter_size + 1.f);
const float deviation_cost = col_prev_d * 50.f * abs(i - sp_index_original);
const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original);
const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];

if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) {
Expand Down Expand Up @@ -376,19 +373,19 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist
break;

case distance_sensor_s::ROTATION_CUSTOM:
offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
offset = Eulerf(Quatf(distance_sensor.q)).psi();
break;
}

return offset;
}

void CollisionPrevention::_constrainAccelerationSetpoint(matrix::Vector2f &setpoint_accel,
const matrix::Vector2f &setpoint_vel)
void
CollisionPrevention::_constrainAccelerationSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
{
_updateObstacleMap();

const matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
const Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
const float vehicle_yaw_angle_rad = Eulerf(attitude).psi();

const float setpoint_length = setpoint_accel.norm();
Expand All @@ -397,14 +394,14 @@ void CollisionPrevention::_constrainAccelerationSetpoint(matrix::Vector2f &setpo
int num_fov_bins = 0;

float acc_vel_constraint = INFINITY;
matrix::Vector2f acc_vel_constraint_dir = {0.f, 0.f};
matrix::Vector2f acc_vel_constraint_setpoint = {0.f, 0.f};
Vector2f acc_vel_constraint_dir = {0.f, 0.f};
Vector2f acc_vel_constraint_setpoint = {0.f, 0.f};

if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {

const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get());
bool setpoint_possible = true;
matrix::Vector2f new_setpoint = {0.f, 0.f};
Vector2f new_setpoint = {0.f, 0.f};

if (setpoint_length > 0.001f) {
Vector2f setpoint_dir = setpoint_accel / setpoint_length;
Expand All @@ -418,7 +415,7 @@ void CollisionPrevention::_constrainAccelerationSetpoint(matrix::Vector2f &setpo
_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);

float closest_distance = INFINITY;
matrix::Vector2f bin_closest_dist = {0.f, 0.f};
Vector2f bin_closest_dist = {0.f, 0.f};


for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
Expand Down Expand Up @@ -521,7 +518,7 @@ void CollisionPrevention::_constrainAccelerationSetpoint(matrix::Vector2f &setpo
} else {
// If no setpoint is provided, still apply force when you are close to an obstacle
float closest_distance = INFINITY;
matrix::Vector2f bin_closest_dist = {0.f, 0.f};
Vector2f bin_closest_dist = {0.f, 0.f};

for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
if (constrain_time - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) {
Expand Down Expand Up @@ -583,11 +580,11 @@ void CollisionPrevention::_constrainAccelerationSetpoint(matrix::Vector2f &setpo

}
void
CollisionPrevention::modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel)
CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
{
//calculate movement constraints based on range data
matrix::Vector2f new_setpoint = setpoint_accel;
matrix::Vector2f original_setpoint = setpoint_accel;
Vector2f new_setpoint = setpoint_accel;
Vector2f original_setpoint = setpoint_accel;
_constrainAccelerationSetpoint(new_setpoint, setpoint_vel);

//warn user if collision prevention starts to interfere
Expand Down
22 changes: 14 additions & 8 deletions src/lib/collision_prevention/CollisionPrevention.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,12 @@
#include <uORB/topics/vehicle_command.h>

using namespace time_literals;
using matrix::Vector2f;
using matrix::Vector3f;
using matrix::Quatf;
using matrix::Eulerf;
using matrix::wrap;
using matrix::wrap_2pi;

class CollisionPrevention : public ModuleParams
{
Expand All @@ -79,7 +85,7 @@ class CollisionPrevention : public ModuleParams
* @param curr_pos, current vehicle position
* @param curr_vel, current vehicle velocity
*/
void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
void modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel);
protected:

obstacle_distance_s _obstacle_map_body_frame {};
Expand All @@ -88,26 +94,26 @@ class CollisionPrevention : public ModuleParams
uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof(
_obstacle_map_body_frame.distances[0])]; /**< in cm */

void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude);
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_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);
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const Quatf &vehicle_attitude);

/**
* Computes an adaption to the setpoint direction to guide towards free space
* @param setpoint_dir, setpoint direction before collision prevention intervention
* @param setpoint_index, index of the setpoint in the internal obstacle map
* @param vehicle_yaw_angle_rad, vehicle orientation
*/
void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad);
void _adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad);

/**
* Constrains the Acceleration setpoint cosdering the current obstacle distances, the current acceleration setpoint and velocity setpoint
*/
void _constrainAccelerationSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
void _constrainAccelerationSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel);

/**
* Determines whether a new sensor measurement is used
Expand Down Expand Up @@ -169,15 +175,15 @@ class CollisionPrevention : public ModuleParams
* @param curr_pos, current vehicle position
* @param curr_vel, current vehicle velocity
*/
void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos,
const matrix::Vector2f &curr_vel);
void _calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
const Vector2f &curr_vel);

/**
* Publishes collision_constraints message
* @param original_setpoint, setpoint before collision prevention intervention
* @param adapted_setpoint, collision prevention adaped setpoint
*/
void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
void _publishConstrainedSetpoint(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint);

/**
* Publishes obstacle_distance message with fused data from offboard and from distance sensors
Expand Down

0 comments on commit 476cc67

Please sign in to comment.