From defccfa99b01b74f106b84f7b778d46fc992a469 Mon Sep 17 00:00:00 2001
From: Mahima Yoga <61349849+mahimayoga@users.noreply.github.com>
Date: Fri, 20 Dec 2024 14:47:22 +0100
Subject: [PATCH] Collision Prevention: Scale obstacle distance with vehicle
 attitude for varying sensor orientations  (#24107)

---
 src/lib/collision_prevention/CollisionPrevention.cpp | 12 ++++++++----
 src/modules/simulation/gz_bridge/GZBridge.cpp        | 10 ++++++++++
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp
index 0733f1b289d4..b2c62d4906c6 100644
--- a/src/lib/collision_prevention/CollisionPrevention.cpp
+++ b/src/lib/collision_prevention/CollisionPrevention.cpp
@@ -400,11 +400,15 @@ 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_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f)));
 
-		// 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
+		const Vector3f forward_vector(1.0f, 0.0f, 0.0f);
+
+		const Quatf q_sensor_rotation = vehicle_attitude * q_sensor;
+
+		const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);
+
+		const float sensor_dist_scale = rotated_sensor_vector.xy().norm();
 
 		if (distance_reading < distance_sensor.max_distance) {
 			distance_reading = distance_reading * sensor_dist_scale;
diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
index f69be21062db..2dc1ef13e4f4 100644
--- a/src/modules/simulation/gz_bridge/GZBridge.cpp
+++ b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -800,7 +800,10 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan)
 			pose_orientation.y(),
 			pose_orientation.z());
 
+	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,8 +812,15 @@ 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();
+		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);