Scale obstacle distance with vehicle attitude for varying sensor orientations (Collision Prevention)

src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp

@@ -570,10 +570,16 @@ 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_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':

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -393,18 +393,25 @@ 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));
 
 		// 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);
 
+		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));
+
+		matrix::Vector3f forward_vector(1.0f, 0.0f, 0.0f);
+
+		Quatf q_sensor_rotation = q_vehicle_attitude * q_sensor;
 
-		// 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
+		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;
@@ -425,6 +432,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 	}
 }
 
+
+
 void
 CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
 {

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -43,7 +43,6 @@
 #pragma once
 
 #include <float.h>
-
 #include <commander/px4_custom_mode.h>
 #include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>

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

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -798,7 +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); // 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)) {
@@ -807,8 +814,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);