fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Mahima Yoga 2024-12-20 14:47:22 +01:00 committed by GitHub
parent b765769f50
commit defccfa99b
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/lib/collision_prevention/CollisionPrevention.cpp
View File

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

10
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

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