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,28 @@ 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));
 
 		// 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 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
+		// 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;
@@ -425,6 +435,24 @@ 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));
+
+	// 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)
 {

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>
@@ -211,6 +210,10 @@ 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

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