From d8f9df73e681c401f393400911fcdd7699b8027c Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 9 Dec 2024 10:26:53 +0100 Subject: [PATCH 01/20] Vectorize sensor data scaling --- .../CollisionPrevention.cpp | 75 +++++++++++++++++-- .../CollisionPrevention.hpp | 25 ++++++- 2 files changed, 93 insertions(+), 7 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 0733f1b289d4..49973f619f3e 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -393,7 +393,26 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, // discard values below min range if (distance_reading > distance_sensor.min_distance) { - float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); + + Vector2f sensor_unit_vector = _getSensorUnitVector(distance_sensor); + + // Extract roll and pitch from vehicle attitude quaternion + Eulerf euler_vehicle(vehicle_attitude); + float roll = euler_vehicle.phi(); + float pitch = euler_vehicle.theta(); + + // Construct rotation matrix for roll and pitch + matrix::Dcmf rotation_matrix(matrix::Eulerf(roll, pitch, 0.0f)); + + // Apply rotation matrix to sensor unit vector + Vector3f sensor_unit_vector_3d(sensor_unit_vector(0), sensor_unit_vector(1), 0.0f); + Vector3f rotated_vector = rotation_matrix * sensor_unit_vector_3d; + Vector2f rotated_sensor_unit_vector(rotated_vector(0), rotated_vector(1)); + + // Scaling factor as dot product of vectors + float sensor_dist_scale = sensor_unit_vector.dot(rotated_sensor_unit_vector); + + float sensor_yaw_body_rad = atan2f(sensor_unit_vector(1), sensor_unit_vector(0)); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices @@ -401,11 +420,6 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - // rotate vehicle attitude into the sensor body frame - 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()); // verify - if (distance_reading < distance_sensor.max_distance) { distance_reading = distance_reading * sensor_dist_scale; } @@ -425,6 +439,55 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, } } + + +matrix::Vector2f +CollisionPrevention::_getSensorUnitVector(const distance_sensor_s &distance_sensor) const +{ + if (distance_sensor.orientation == distance_sensor_s::ROTATION_CUSTOM) { + return _getUnitVectorFromQuaternion(distance_sensor.q); + } else { + return _getUnitVectorFromOrientation(distance_sensor.orientation); + } +} + +matrix::Vector2f +CollisionPrevention::_getUnitVectorFromOrientation(uint8_t orientation) const +{ + switch (orientation) { + case distance_sensor_s::ROTATION_FORWARD_FACING: + return Vector2f(1.0f, 0.0f); + case distance_sensor_s::ROTATION_RIGHT_FACING: + return Vector2f(0.0f, 1.0f); + case distance_sensor_s::ROTATION_BACKWARD_FACING: + return Vector2f(-1.0f, 0.0f); + case distance_sensor_s::ROTATION_LEFT_FACING: + return Vector2f(0.0f, -1.0f); + case distance_sensor_s::ROTATION_YAW_45: + return Vector2f(cosf(M_PI_4), sinf(M_PI_4)); + case distance_sensor_s::ROTATION_YAW_135: + return Vector2f(-cosf(M_PI_4), sinf(M_PI_4)); + case distance_sensor_s::ROTATION_YAW_225: + return Vector2f(-cosf(M_PI_4), -sinf(M_PI_4)); + case distance_sensor_s::ROTATION_YAW_315: + return Vector2f(cosf(M_PI_4), -sinf(M_PI_4)); + default: + return Vector2f(1.0f, 0.0f); // Default to forward-facing + } +} + +matrix::Vector2f +CollisionPrevention::_getUnitVectorFromQuaternion(const float q[4]) const +{ + Quatf quaternion(q[0], q[1], q[2], q[3]); + Vector3f custom_vector(1.0f, 0.0f, 0.0f); // Default orientation + + quaternion.rotate(custom_vector); // Rotate default orientation by custom orientation + + return Vector2f(custom_vector(0), custom_vector(1)).unit_or_zero(); // Constrain to XY plane and normalize +} + + void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 8fcc7c28f3ec..2d77233f28b9 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -43,7 +43,6 @@ #pragma once #include - #include #include #include @@ -211,6 +210,30 @@ class CollisionPrevention : public ModuleParams */ float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const; + + + /** + * Gets the 2D unit vector of the sensor in the body frame + * @param distance_sensor, distance sensor message + */ + matrix::Vector2f _getSensorUnitVector(const distance_sensor_s &distance_sensor) const; + + + + /** + * Get the unit vector from a quaternion + * @param q, quaternion representing the sensor orientation + */ + matrix::Vector2f _getUnitVectorFromQuaternion(const float q[4]) const; + + + /** + * Get the unit vector from an orientation + * @param orientation, enum representing the sensor orientation + */ + matrix::Vector2f _getUnitVectorFromOrientation(uint8_t orientation) const; + + /** * Computes collision free setpoints * @param setpoint, setpoint before collision prevention intervention From 2a9b5af5b495eaaca93ac297bd45cc7a66a21bb2 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Tue, 10 Dec 2024 16:53:49 +0100 Subject: [PATCH 02/20] Add custom sensor orientations for testing --- .../common/include/px4_platform_common/i2c_spi_buses.h | 2 ++ .../lightware_laser_i2c/lightware_laser_i2c.cpp | 10 +++++++++- src/lib/collision_prevention/CollisionPrevention.cpp | 4 ++-- src/modules/simulation/gz_bridge/GZBridge.cpp | 6 ++++++ 4 files changed, 19 insertions(+), 3 deletions(-) diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index 0ca599a57ece..2c687bcc2ce3 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -188,6 +188,8 @@ class BusCLIArguments Rotation rotation{ROTATION_NONE}; ///< sensor rotation (MAV_SENSOR_ROTATION_* or distance_sensor_s::ROTATION_*) + // float q[4] {}; ///< rotation quaternion if rotation is ROTATION_CUSTOM + int custom1{0}; ///< driver-specific custom argument int custom2{0}; ///< driver-specific custom argument void *custom_data{nullptr}; ///< driver-specific custom argument diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 614d408a5e30..4f709080587a 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -570,10 +570,18 @@ extern "C" __EXPORT int lightware_laser_i2c_main(int argc, char *argv[]) int ch; using ThisDriver = LightwareLaser; BusCLIArguments cli{true, false}; - cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; + // cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_FORWARD_FACING; cli.default_i2c_frequency = 400000; cli.i2c_address = LIGHTWARE_LASER_BASEADDR; + // if (cli.rotation == distance_sensor_s::ROTATION_CUSTOM) { + // cli.q[0] = 0.9238795; // w + // cli.q[1] = 0.0; // x + // cli.q[2] = 0.0; // y + // cli.q[3] = 0.3826834; // z + // } + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 49973f619f3e..5757c40a477e 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -405,12 +405,12 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, matrix::Dcmf rotation_matrix(matrix::Eulerf(roll, pitch, 0.0f)); // Apply rotation matrix to sensor unit vector - Vector3f sensor_unit_vector_3d(sensor_unit_vector(0), sensor_unit_vector(1), 0.0f); + Vector3f sensor_unit_vector_3d(sensor_unit_vector(0), sensor_unit_vector(1), 0.0f); // add z dimension Vector3f rotated_vector = rotation_matrix * sensor_unit_vector_3d; Vector2f rotated_sensor_unit_vector(rotated_vector(0), rotated_vector(1)); // Scaling factor as dot product of vectors - float sensor_dist_scale = sensor_unit_vector.dot(rotated_sensor_unit_vector); + float sensor_dist_scale = sensor_unit_vector.dot(rotated_sensor_unit_vector); // finds projection of rotated_sensor_unit_vector onto sensor_unit_vector float sensor_yaw_body_rad = atan2f(sensor_unit_vector(1), sensor_unit_vector(0)); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 95be696835e2..e2bc71a4d6c2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -798,6 +798,8 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) pose_orientation.y(), pose_orientation.z()); + // gz::math::Quaterniond q_sensor(0.9238795, 0, 0, 0.3826834); + const gz::math::Quaterniond q_front(0.7071068, 0.7071068, 0, 0); const gz::math::Quaterniond q_down(0, 1, 0, 0); @@ -809,6 +811,10 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) } else { distance_sensor.orientation = distance_sensor_s::ROTATION_CUSTOM; + distance_sensor.q[0] = q_sensor.W(); + distance_sensor.q[1] = q_sensor.X(); + distance_sensor.q[2] = q_sensor.Y(); + distance_sensor.q[3] = q_sensor.Z(); } _distance_sensor_pub.publish(distance_sensor); From fe66a2597287c31110370c162e7fb46d7a3511e5 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Thu, 12 Dec 2024 15:14:32 +0100 Subject: [PATCH 03/20] Add rotation function for scaling --- .../CollisionPrevention.cpp | 105 ++++++++---------- .../CollisionPrevention.hpp | 28 ++--- 2 files changed, 54 insertions(+), 79 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 5757c40a477e..aff37b248cbf 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -394,31 +394,27 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, // discard values below min range if (distance_reading > distance_sensor.min_distance) { - Vector2f sensor_unit_vector = _getSensorUnitVector(distance_sensor); - - // Extract roll and pitch from vehicle attitude quaternion + // get drone attitude in rad Eulerf euler_vehicle(vehicle_attitude); float roll = euler_vehicle.phi(); float pitch = euler_vehicle.theta(); - // Construct rotation matrix for roll and pitch - matrix::Dcmf rotation_matrix(matrix::Eulerf(roll, pitch, 0.0f)); - - // Apply rotation matrix to sensor unit vector - Vector3f sensor_unit_vector_3d(sensor_unit_vector(0), sensor_unit_vector(1), 0.0f); // add z dimension - Vector3f rotated_vector = rotation_matrix * sensor_unit_vector_3d; - Vector2f rotated_sensor_unit_vector(rotated_vector(0), rotated_vector(1)); - // Scaling factor as dot product of vectors - float sensor_dist_scale = sensor_unit_vector.dot(rotated_sensor_unit_vector); // finds projection of rotated_sensor_unit_vector onto sensor_unit_vector - - float sensor_yaw_body_rad = atan2f(sensor_unit_vector(1), sensor_unit_vector(0)); + float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); + // unit vector of sensor orientation + Vector2f sensor_unit_vector = Vector2f(cosf(sensor_yaw_body_rad), sinf(sensor_yaw_body_rad)).unit_or_zero(); + + // rotate sensor unit vector into vehicle body frame + Vector2f rotated_sensor_unit_vector = _rotatePointByPitchAndRoll(sensor_unit_vector, pitch, roll); + + // dot product to find projection of rotated_sensor_unit_vector onto sensor_unit_vector + float sensor_dist_scale = rotated_sensor_unit_vector.dot(sensor_unit_vector); if (distance_reading < distance_sensor.max_distance) { distance_reading = distance_reading * sensor_dist_scale; @@ -440,51 +436,44 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, } - -matrix::Vector2f -CollisionPrevention::_getSensorUnitVector(const distance_sensor_s &distance_sensor) const -{ - if (distance_sensor.orientation == distance_sensor_s::ROTATION_CUSTOM) { - return _getUnitVectorFromQuaternion(distance_sensor.q); - } else { - return _getUnitVectorFromOrientation(distance_sensor.orientation); - } -} - -matrix::Vector2f -CollisionPrevention::_getUnitVectorFromOrientation(uint8_t orientation) const -{ - switch (orientation) { - case distance_sensor_s::ROTATION_FORWARD_FACING: - return Vector2f(1.0f, 0.0f); - case distance_sensor_s::ROTATION_RIGHT_FACING: - return Vector2f(0.0f, 1.0f); - case distance_sensor_s::ROTATION_BACKWARD_FACING: - return Vector2f(-1.0f, 0.0f); - case distance_sensor_s::ROTATION_LEFT_FACING: - return Vector2f(0.0f, -1.0f); - case distance_sensor_s::ROTATION_YAW_45: - return Vector2f(cosf(M_PI_4), sinf(M_PI_4)); - case distance_sensor_s::ROTATION_YAW_135: - return Vector2f(-cosf(M_PI_4), sinf(M_PI_4)); - case distance_sensor_s::ROTATION_YAW_225: - return Vector2f(-cosf(M_PI_4), -sinf(M_PI_4)); - case distance_sensor_s::ROTATION_YAW_315: - return Vector2f(cosf(M_PI_4), -sinf(M_PI_4)); - default: - return Vector2f(1.0f, 0.0f); // Default to forward-facing - } -} - -matrix::Vector2f -CollisionPrevention::_getUnitVectorFromQuaternion(const float q[4]) const +// Function to rotate a 2D point by pitch and roll +matrix::Vector2f CollisionPrevention::_rotatePointByPitchAndRoll(const matrix::Vector2f &point, float pitch, float roll) { - Quatf quaternion(q[0], q[1], q[2], q[3]); - Vector3f custom_vector(1.0f, 0.0f, 0.0f); // Default orientation - - quaternion.rotate(custom_vector); // Rotate default orientation by custom orientation - - return Vector2f(custom_vector(0), custom_vector(1)).unit_or_zero(); // Constrain to XY plane and normalize + // Construct the rotation matrix for pitch + matrix::Matrix3f R_pitch; + R_pitch(0, 0) = cosf(pitch); + R_pitch(0, 1) = 0.0f; + R_pitch(0, 2) = sinf(pitch); + R_pitch(1, 0) = 0.0f; + R_pitch(1, 1) = 1.0f; + R_pitch(1, 2) = 0.0f; + R_pitch(2, 0) = -sinf(pitch); + R_pitch(2, 1) = 0.0f; + R_pitch(2, 2) = cosf(pitch); + + // Construct the rotation matrix for roll + matrix::Matrix3f R_roll; + R_roll(0, 0) = 1.0f; + R_roll(0, 1) = 0.0f; + R_roll(0, 2) = 0.0f; + R_roll(1, 0) = 0.0f; + R_roll(1, 1) = cosf(roll); + R_roll(1, 2) = -sinf(roll); + R_roll(2, 0) = 0.0f; + R_roll(2, 1) = sinf(roll); + R_roll(2, 2) = cosf(roll); + + // Combine the rotation matrices + matrix::Matrix3f R = R_pitch * R_roll; + + // Convert the 2D point to a 3D point (assuming z = 0) + matrix::Vector3f point_3d(point(0), point(1), 0.0f); + + // Apply the combined rotation matrix to the 3D point + matrix::Vector3f rotated_point_3d = R * point_3d; + + // Project the rotated 3D point back to 2D + return matrix::Vector2f(rotated_point_3d(0), rotated_point_3d(1)); } diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 2d77233f28b9..3d55bbad7c73 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -211,27 +211,13 @@ class CollisionPrevention : public ModuleParams float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const; - - /** - * Gets the 2D unit vector of the sensor in the body frame - * @param distance_sensor, distance sensor message - */ - matrix::Vector2f _getSensorUnitVector(const distance_sensor_s &distance_sensor) const; - - - - /** - * Get the unit vector from a quaternion - * @param q, quaternion representing the sensor orientation - */ - matrix::Vector2f _getUnitVectorFromQuaternion(const float q[4]) const; - - - /** - * Get the unit vector from an orientation - * @param orientation, enum representing the sensor orientation - */ - matrix::Vector2f _getUnitVectorFromOrientation(uint8_t orientation) const; + // /** + // * Rotate a point by pitch and roll + // * @param point, Vector2f to be rotated + // * @param pitch, vehicle pitch in radians + // * @param roll, vehicle roll in radians + // */ + matrix::Vector2f _rotatePointByPitchAndRoll(const matrix::Vector2f &point, float pitch, float roll); /** From 5ad5f0d5601bfef69e52196c1a5af92c4afe6e2c Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Thu, 12 Dec 2024 15:18:35 +0100 Subject: [PATCH 04/20] Add custom sensor orientations for testing --- .../include/px4_platform_common/i2c_spi_buses.h | 2 -- .../lightware_laser_i2c/lightware_laser_i2c.cpp | 11 ++++------- src/modules/logger/logged_topics.cpp | 1 + src/modules/simulation/gz_bridge/GZBridge.cpp | 10 +++++++++- 4 files changed, 14 insertions(+), 10 deletions(-) diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index 2c687bcc2ce3..0ca599a57ece 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -188,8 +188,6 @@ class BusCLIArguments Rotation rotation{ROTATION_NONE}; ///< sensor rotation (MAV_SENSOR_ROTATION_* or distance_sensor_s::ROTATION_*) - // float q[4] {}; ///< rotation quaternion if rotation is ROTATION_CUSTOM - int custom1{0}; ///< driver-specific custom argument int custom2{0}; ///< driver-specific custom argument void *custom_data{nullptr}; ///< driver-specific custom argument diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 4f709080587a..5538f12f91d6 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -571,16 +571,13 @@ extern "C" __EXPORT int lightware_laser_i2c_main(int argc, char *argv[]) using ThisDriver = LightwareLaser; BusCLIArguments cli{true, false}; // cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; - cli.rotation = (Rotation)distance_sensor_s::ROTATION_FORWARD_FACING; + // cli.rotation = (Rotation)distance_sensor_s::ROTATION_YAW_45; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_RIGHT_FACING; + // cli.rotation = (Rotation)distance_sensor_s::ROTATION_FORWARD_FACING; + // cli.rotation = (Rotation)distance_sensor_s::ROTATION_CUSTOM; cli.default_i2c_frequency = 400000; cli.i2c_address = LIGHTWARE_LASER_BASEADDR; - // if (cli.rotation == distance_sensor_s::ROTATION_CUSTOM) { - // cli.q[0] = 0.9238795; // w - // cli.q[1] = 0.0; // x - // cli.q[2] = 0.0; // y - // cli.q[3] = 0.3826834; // z - // } while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3ff1c321e79b..b26f73f3c654 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -59,6 +59,7 @@ void LoggedTopics::add_default_topics() add_topic("config_overrides"); add_topic("cpuload"); add_topic("distance_sensor_mode_change_request"); + add_topic("distance_sensor"); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index e2bc71a4d6c2..c14c91b89cb2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -798,9 +798,14 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) pose_orientation.y(), pose_orientation.z()); - // gz::math::Quaterniond q_sensor(0.9238795, 0, 0, 0.3826834); + // gz::math::Quaterniond q_sensor(0.9238795, 0, 0, 0.3826834); // 45 degree + + // gz::math::Quaterniond q_sensor(0.7071068, 0, 0, -0.7071068); // -90 degree + + const gz::math::Quaterniond q_left(0.7071068, 0, 0, -0.7071068); const gz::math::Quaterniond q_front(0.7071068, 0.7071068, 0, 0); + const gz::math::Quaterniond q_down(0, 1, 0, 0); if (q_sensor.Equal(q_front, 0.03)) { @@ -809,6 +814,9 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) } else if (q_sensor.Equal(q_down, 0.03)) { distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + }else if (q_sensor.Equal(q_left, 0.03)) { + distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING; + } else { distance_sensor.orientation = distance_sensor_s::ROTATION_CUSTOM; distance_sensor.q[0] = q_sensor.W(); From ed38602cd98d909681d7ed06b418db304fe5acdf Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Thu, 12 Dec 2024 16:14:34 +0100 Subject: [PATCH 05/20] Use built-in rotation functions --- .../CollisionPrevention.cpp | 30 ++----------------- .../CollisionPrevention.hpp | 6 ---- 2 files changed, 3 insertions(+), 33 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index aff37b248cbf..5e2f49ec63dd 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -439,37 +439,13 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, // Function to rotate a 2D point by pitch and roll matrix::Vector2f CollisionPrevention::_rotatePointByPitchAndRoll(const matrix::Vector2f &point, float pitch, float roll) { - // Construct the rotation matrix for pitch - matrix::Matrix3f R_pitch; - R_pitch(0, 0) = cosf(pitch); - R_pitch(0, 1) = 0.0f; - R_pitch(0, 2) = sinf(pitch); - R_pitch(1, 0) = 0.0f; - R_pitch(1, 1) = 1.0f; - R_pitch(1, 2) = 0.0f; - R_pitch(2, 0) = -sinf(pitch); - R_pitch(2, 1) = 0.0f; - R_pitch(2, 2) = cosf(pitch); - - // Construct the rotation matrix for roll - matrix::Matrix3f R_roll; - R_roll(0, 0) = 1.0f; - R_roll(0, 1) = 0.0f; - R_roll(0, 2) = 0.0f; - R_roll(1, 0) = 0.0f; - R_roll(1, 1) = cosf(roll); - R_roll(1, 2) = -sinf(roll); - R_roll(2, 0) = 0.0f; - R_roll(2, 1) = sinf(roll); - R_roll(2, 2) = cosf(roll); - - // Combine the rotation matrices - matrix::Matrix3f R = R_pitch * R_roll; + // Construct the DCM for pitch and roll using Euler angles + matrix::Dcmf R(matrix::Eulerf(pitch, roll, 0.0f)); // Convert the 2D point to a 3D point (assuming z = 0) matrix::Vector3f point_3d(point(0), point(1), 0.0f); - // Apply the combined rotation matrix to the 3D point + // Apply the combined DCM to the 3D point matrix::Vector3f rotated_point_3d = R * point_3d; // Project the rotated 3D point back to 2D diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 3d55bbad7c73..2a4de50df797 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -211,12 +211,6 @@ class CollisionPrevention : public ModuleParams float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const; - // /** - // * Rotate a point by pitch and roll - // * @param point, Vector2f to be rotated - // * @param pitch, vehicle pitch in radians - // * @param roll, vehicle roll in radians - // */ matrix::Vector2f _rotatePointByPitchAndRoll(const matrix::Vector2f &point, float pitch, float roll); From 01e2242f93a649b69733bf5d90cae9295679077b Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 13 Dec 2024 10:32:50 +0100 Subject: [PATCH 06/20] Bug fix in rotation matrices --- src/lib/collision_prevention/CollisionPrevention.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 5e2f49ec63dd..fd16657ac49e 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -440,7 +440,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, matrix::Vector2f CollisionPrevention::_rotatePointByPitchAndRoll(const matrix::Vector2f &point, float pitch, float roll) { // Construct the DCM for pitch and roll using Euler angles - matrix::Dcmf R(matrix::Eulerf(pitch, roll, 0.0f)); + matrix::Dcmf R(matrix::Eulerf(roll, pitch, 0.0f)); // Convert the 2D point to a 3D point (assuming z = 0) matrix::Vector3f point_3d(point(0), point(1), 0.0f); From 16d0810314954005a95f15b0f1bb4b04388c0c91 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 13 Dec 2024 10:33:39 +0100 Subject: [PATCH 07/20] Set default orientation back to front facing --- .../lightware_laser_i2c/lightware_laser_i2c.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 5538f12f91d6..072289d0a9aa 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -572,8 +572,9 @@ extern "C" __EXPORT int lightware_laser_i2c_main(int argc, char *argv[]) BusCLIArguments cli{true, false}; // cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; // cli.rotation = (Rotation)distance_sensor_s::ROTATION_YAW_45; - cli.rotation = (Rotation)distance_sensor_s::ROTATION_RIGHT_FACING; - // cli.rotation = (Rotation)distance_sensor_s::ROTATION_FORWARD_FACING; + // cli.rotation = (Rotation)distance_sensor_s::ROTATION_RIGHT_FACING; + // cli.rotation = (Rotation)distance_sensor_s::ROTATION_LEFT_FACING; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_FORWARD_FACING; // cli.rotation = (Rotation)distance_sensor_s::ROTATION_CUSTOM; cli.default_i2c_frequency = 400000; cli.i2c_address = LIGHTWARE_LASER_BASEADDR; From 32904a955fcb09b0372603933a57451219caa416 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 13 Dec 2024 15:21:39 +0100 Subject: [PATCH 08/20] Formatting --- .../CollisionPrevention.cpp | 18 +++++++++--------- src/modules/simulation/gz_bridge/GZBridge.cpp | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index fd16657ac49e..8fedd7da393b 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -400,7 +400,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, float pitch = euler_vehicle.theta(); - float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); + float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices @@ -439,17 +439,17 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, // Function to rotate a 2D point by pitch and roll matrix::Vector2f CollisionPrevention::_rotatePointByPitchAndRoll(const matrix::Vector2f &point, float pitch, float roll) { - // Construct the DCM for pitch and roll using Euler angles - matrix::Dcmf R(matrix::Eulerf(roll, pitch, 0.0f)); + // Construct the DCM for pitch and roll using Euler angles + matrix::Dcmf R(matrix::Eulerf(roll, pitch, 0.0f)); - // Convert the 2D point to a 3D point (assuming z = 0) - matrix::Vector3f point_3d(point(0), point(1), 0.0f); + // Convert the 2D point to a 3D point (assuming z = 0) + matrix::Vector3f point_3d(point(0), point(1), 0.0f); - // Apply the combined DCM to the 3D point - matrix::Vector3f rotated_point_3d = R * point_3d; + // Apply the combined DCM to the 3D point + matrix::Vector3f rotated_point_3d = R * point_3d; - // Project the rotated 3D point back to 2D - return matrix::Vector2f(rotated_point_3d(0), rotated_point_3d(1)); + // Project the rotated 3D point back to 2D + return matrix::Vector2f(rotated_point_3d(0), rotated_point_3d(1)); } diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index c14c91b89cb2..b4626a6f45a3 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -814,7 +814,7 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) } else if (q_sensor.Equal(q_down, 0.03)) { distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - }else if (q_sensor.Equal(q_left, 0.03)) { + } else if (q_sensor.Equal(q_left, 0.03)) { distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING; } else { From 196424ee476ec20086736096fca772f63b12c74a Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 16 Dec 2024 15:34:39 +0100 Subject: [PATCH 09/20] Remove unnecessary comments --- src/lib/collision_prevention/CollisionPrevention.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 8fedd7da393b..9386b99c7b18 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -407,10 +407,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - // unit vector of sensor orientation Vector2f sensor_unit_vector = Vector2f(cosf(sensor_yaw_body_rad), sinf(sensor_yaw_body_rad)).unit_or_zero(); - - // rotate sensor unit vector into vehicle body frame Vector2f rotated_sensor_unit_vector = _rotatePointByPitchAndRoll(sensor_unit_vector, pitch, roll); // dot product to find projection of rotated_sensor_unit_vector onto sensor_unit_vector @@ -436,7 +433,6 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, } -// Function to rotate a 2D point by pitch and roll matrix::Vector2f CollisionPrevention::_rotatePointByPitchAndRoll(const matrix::Vector2f &point, float pitch, float roll) { // Construct the DCM for pitch and roll using Euler angles From d31fd40de2b5e9542fab81dc9ec630bbbcab438e Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 16 Dec 2024 17:26:01 +0100 Subject: [PATCH 10/20] Use quaternions for rotations --- .../CollisionPrevention.cpp | 35 ++++++------------- .../CollisionPrevention.hpp | 4 --- 2 files changed, 10 insertions(+), 29 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 9386b99c7b18..24a43c193a55 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -394,12 +394,6 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, // discard values below min range if (distance_reading > distance_sensor.min_distance) { - // get drone attitude in rad - Eulerf euler_vehicle(vehicle_attitude); - float roll = euler_vehicle.phi(); - float pitch = euler_vehicle.theta(); - - float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); @@ -407,11 +401,17 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - Vector2f sensor_unit_vector = Vector2f(cosf(sensor_yaw_body_rad), sinf(sensor_yaw_body_rad)).unit_or_zero(); - Vector2f rotated_sensor_unit_vector = _rotatePointByPitchAndRoll(sensor_unit_vector, pitch, roll); + Quatf q_vehicle_attitude = Quatf(vehicle_attitude); + Quatf q_sensor = Quatf(Eulerf(0.0f, 0.0f, sensor_yaw_body_rad)); + + matrix::Vector3f forward_vector(1.0f, 0.0f, 0.0f); - // dot product to find projection of rotated_sensor_unit_vector onto sensor_unit_vector - float sensor_dist_scale = rotated_sensor_unit_vector.dot(sensor_unit_vector); + Quatf q_sensor_rotation = q_vehicle_attitude * q_sensor; + + matrix::Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector); + matrix::Vector2f rotated_sensor_vector_xy(rotated_sensor_vector(0), rotated_sensor_vector(1)); + + float sensor_dist_scale = rotated_sensor_vector_xy.norm(); if (distance_reading < distance_sensor.max_distance) { distance_reading = distance_reading * sensor_dist_scale; @@ -433,21 +433,6 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, } -matrix::Vector2f CollisionPrevention::_rotatePointByPitchAndRoll(const matrix::Vector2f &point, float pitch, float roll) -{ - // Construct the DCM for pitch and roll using Euler angles - matrix::Dcmf R(matrix::Eulerf(roll, pitch, 0.0f)); - - // Convert the 2D point to a 3D point (assuming z = 0) - matrix::Vector3f point_3d(point(0), point(1), 0.0f); - - // Apply the combined DCM to the 3D point - matrix::Vector3f rotated_point_3d = R * point_3d; - - // Project the rotated 3D point back to 2D - return matrix::Vector2f(rotated_point_3d(0), rotated_point_3d(1)); -} - void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 2a4de50df797..e8ce28afa987 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -210,10 +210,6 @@ class CollisionPrevention : public ModuleParams */ float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const; - - matrix::Vector2f _rotatePointByPitchAndRoll(const matrix::Vector2f &point, float pitch, float roll); - - /** * Computes collision free setpoints * @param setpoint, setpoint before collision prevention intervention From 4d3a768ebc9e201f1224a79f81734f768de052dd Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Tue, 17 Dec 2024 11:07:19 +0100 Subject: [PATCH 11/20] Initialize sensor quaternion with array --- src/lib/collision_prevention/CollisionPrevention.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 24a43c193a55..e48b677c6b5a 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -402,7 +402,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); Quatf q_vehicle_attitude = Quatf(vehicle_attitude); - Quatf q_sensor = Quatf(Eulerf(0.0f, 0.0f, sensor_yaw_body_rad)); + Quatf q_sensor = Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f)); matrix::Vector3f forward_vector(1.0f, 0.0f, 0.0f); From c97aa0d8c260298953dcd0b2499ce905e664396a Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 20 Dec 2024 08:22:06 +0100 Subject: [PATCH 12/20] Remove extra sensor orientations for testing --- .../lightware_laser_i2c/lightware_laser_i2c.cpp | 6 ------ src/modules/simulation/gz_bridge/GZBridge.cpp | 4 ---- 2 files changed, 10 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 072289d0a9aa..98ce8fb4cb3a 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -570,16 +570,10 @@ extern "C" __EXPORT int lightware_laser_i2c_main(int argc, char *argv[]) int ch; using ThisDriver = LightwareLaser; BusCLIArguments cli{true, false}; - // cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; - // cli.rotation = (Rotation)distance_sensor_s::ROTATION_YAW_45; - // cli.rotation = (Rotation)distance_sensor_s::ROTATION_RIGHT_FACING; - // cli.rotation = (Rotation)distance_sensor_s::ROTATION_LEFT_FACING; cli.rotation = (Rotation)distance_sensor_s::ROTATION_FORWARD_FACING; - // cli.rotation = (Rotation)distance_sensor_s::ROTATION_CUSTOM; cli.default_i2c_frequency = 400000; cli.i2c_address = LIGHTWARE_LASER_BASEADDR; - while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index b4626a6f45a3..a225280a70e8 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -798,10 +798,6 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) pose_orientation.y(), pose_orientation.z()); - // gz::math::Quaterniond q_sensor(0.9238795, 0, 0, 0.3826834); // 45 degree - - // gz::math::Quaterniond q_sensor(0.7071068, 0, 0, -0.7071068); // -90 degree - const gz::math::Quaterniond q_left(0.7071068, 0, 0, -0.7071068); const gz::math::Quaterniond q_front(0.7071068, 0.7071068, 0, 0); From 79fe43209cae6958fb86495559dfc87ed2c1995c Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 20 Dec 2024 08:30:53 +0100 Subject: [PATCH 13/20] Remove duplicate topic --- src/modules/logger/logged_topics.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index b26f73f3c654..3ff1c321e79b 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -59,7 +59,6 @@ void LoggedTopics::add_default_topics() add_topic("config_overrides"); add_topic("cpuload"); add_topic("distance_sensor_mode_change_request"); - add_topic("distance_sensor"); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); From 5b9f1adc15348163b97008c4bc747b4c26cf8108 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 20 Dec 2024 08:36:04 +0100 Subject: [PATCH 14/20] Set sensor orientation back to default --- .../distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 98ce8fb4cb3a..614d408a5e30 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -570,7 +570,7 @@ extern "C" __EXPORT int lightware_laser_i2c_main(int argc, char *argv[]) int ch; using ThisDriver = LightwareLaser; BusCLIArguments cli{true, false}; - cli.rotation = (Rotation)distance_sensor_s::ROTATION_FORWARD_FACING; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; cli.default_i2c_frequency = 400000; cli.i2c_address = LIGHTWARE_LASER_BASEADDR; From 448d7094f5dd6623ba551db29468793facf2a0eb Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 20 Dec 2024 08:37:40 +0100 Subject: [PATCH 15/20] Cleanup --- src/lib/collision_prevention/CollisionPrevention.cpp | 6 ++---- src/lib/collision_prevention/CollisionPrevention.hpp | 1 - 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index e48b677c6b5a..f80afec9df91 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -401,8 +401,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - Quatf q_vehicle_attitude = Quatf(vehicle_attitude); - Quatf q_sensor = Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f)); + const Quatf q_vehicle_attitude(Quatf(vehicle_attitude)); + const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f))); matrix::Vector3f forward_vector(1.0f, 0.0f, 0.0f); @@ -432,8 +432,6 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, } } - - void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index e8ce28afa987..7c823c6e05ff 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -43,7 +43,6 @@ #pragma once #include -#include #include #include #include From 7cdf8eee116b5e5cdaef56b216febd2e360d5748 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 20 Dec 2024 08:46:06 +0100 Subject: [PATCH 16/20] Small bug fix vehicle quaternion initialization --- src/lib/collision_prevention/CollisionPrevention.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index f80afec9df91..bd2b0813d474 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -393,7 +393,6 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, // discard values below min range if (distance_reading > distance_sensor.min_distance) { - float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); @@ -401,7 +400,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - const Quatf q_vehicle_attitude(Quatf(vehicle_attitude)); + const Quatf q_vehicle_attitude(vehicle_attitude); const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f))); matrix::Vector3f forward_vector(1.0f, 0.0f, 0.0f); From 7fee6a21fc915c40d22e88a88bd4c504d6a453d7 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 20 Dec 2024 09:55:29 +0100 Subject: [PATCH 17/20] Add const to intermediate variables --- src/lib/collision_prevention/CollisionPrevention.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index bd2b0813d474..fb28e2c35e9d 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -403,12 +403,12 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf q_vehicle_attitude(vehicle_attitude); const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f))); - matrix::Vector3f forward_vector(1.0f, 0.0f, 0.0f); + const Vector3f forward_vector(1.0f, 0.0f, 0.0f); - Quatf q_sensor_rotation = q_vehicle_attitude * q_sensor; + const Quatf q_sensor_rotation = q_vehicle_attitude * q_sensor; - matrix::Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector); - matrix::Vector2f rotated_sensor_vector_xy(rotated_sensor_vector(0), rotated_sensor_vector(1)); + const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector); + const Vector2f rotated_sensor_vector_xy(rotated_sensor_vector(0), rotated_sensor_vector(1)); float sensor_dist_scale = rotated_sensor_vector_xy.norm(); From 5ee3dfef2c98625debeeec81a9ed0d93b86c029a Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 20 Dec 2024 09:56:48 +0100 Subject: [PATCH 18/20] Add back original dependencies --- src/lib/collision_prevention/CollisionPrevention.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 7c823c6e05ff..8fcc7c28f3ec 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -43,6 +43,8 @@ #pragma once #include + +#include #include #include #include From 1c81ce74518ee08bc7e56ffa080dddfd2f1cd989 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 20 Dec 2024 11:13:57 +0100 Subject: [PATCH 19/20] Compute distance scale directly from 3d vector --- src/lib/collision_prevention/CollisionPrevention.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index fb28e2c35e9d..5e675b694461 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -408,9 +408,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf q_sensor_rotation = q_vehicle_attitude * q_sensor; const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector); - const Vector2f rotated_sensor_vector_xy(rotated_sensor_vector(0), rotated_sensor_vector(1)); - float sensor_dist_scale = rotated_sensor_vector_xy.norm(); + const float sensor_dist_scale = rotated_sensor_vector.xy().norm(); if (distance_reading < distance_sensor.max_distance) { distance_reading = distance_reading * sensor_dist_scale; From c652932300550e9f79753e05965e53b3fff9823e Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 20 Dec 2024 14:27:39 +0100 Subject: [PATCH 20/20] Remove local copy of vehicle attitude --- src/lib/collision_prevention/CollisionPrevention.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 5e675b694461..b2c62d4906c6 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -400,12 +400,11 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - const Quatf q_vehicle_attitude(vehicle_attitude); const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f))); const Vector3f forward_vector(1.0f, 0.0f, 0.0f); - const Quatf q_sensor_rotation = q_vehicle_attitude * q_sensor; + const Quatf q_sensor_rotation = vehicle_attitude * q_sensor; const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);