From 476cc676e68d1431d898c77281021ca7ca9c7263 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Mon, 12 Aug 2024 11:57:17 +0200 Subject: [PATCH] slight refactor --- .../CollisionPrevention.cpp | 37 +++++++++---------- .../CollisionPrevention.hpp | 22 +++++++---- 2 files changed, 31 insertions(+), 28 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 598353815f6b..b6af67fc6ac0 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -40,8 +40,6 @@ #include "CollisionPrevention.hpp" #include -using namespace matrix; -using namespace time_literals; namespace { @@ -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()); @@ -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); @@ -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()); @@ -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; @@ -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]; @@ -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) { @@ -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(); @@ -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; @@ -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++) { @@ -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) { @@ -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 diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index d46831983087..3058271f85a8 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -60,6 +60,12 @@ #include 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 { @@ -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 {}; @@ -88,13 +94,13 @@ 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 @@ -102,12 +108,12 @@ class CollisionPrevention : public ModuleParams * @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 @@ -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