Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 20 commits into from
Dec 20, 2024
Merged

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

Show file tree
Hide file tree
Changes from 18 commits
Commits
Show all changes
20 commits
Select commit Hold shift + click to select a range
d8f9df7
Vectorize sensor data scaling
mahimayoga Dec 9, 2024
2a9b5af
Add custom sensor orientations for testing
mahimayoga Dec 10, 2024
fe66a25
Add rotation function for scaling
mahimayoga Dec 12, 2024
5ad5f0d
Add custom sensor orientations for testing
mahimayoga Dec 12, 2024
ed38602
Use built-in rotation functions
mahimayoga Dec 12, 2024
01e2242
Bug fix in rotation matrices
mahimayoga Dec 13, 2024
16d0810
Set default orientation back to front facing
mahimayoga Dec 13, 2024
32904a9
Formatting
mahimayoga Dec 13, 2024
196424e
Remove unnecessary comments
mahimayoga Dec 16, 2024
d31fd40
Use quaternions for rotations
mahimayoga Dec 16, 2024
4d3a768
Initialize sensor quaternion with array
mahimayoga Dec 17, 2024
c97aa0d
Remove extra sensor orientations for testing
mahimayoga Dec 20, 2024
79fe432
Remove duplicate topic
mahimayoga Dec 20, 2024
5b9f1ad
Set sensor orientation back to default
mahimayoga Dec 20, 2024
448d709
Cleanup
mahimayoga Dec 20, 2024
7cdf8ee
Small bug fix vehicle quaternion initialization
mahimayoga Dec 20, 2024
7fee6a2
Add const to intermediate variables
mahimayoga Dec 20, 2024
5ee3dfe
Add back original dependencies
mahimayoga Dec 20, 2024
1c81ce7
Compute distance scale directly from 3d vector
mahimayoga Dec 20, 2024
c652932
Remove local copy of vehicle attitude
mahimayoga Dec 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 10 additions & 4 deletions src/lib/collision_prevention/CollisionPrevention.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -400,11 +400,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);

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

// 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 = 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();

if (distance_reading < distance_sensor.max_distance) {
distance_reading = distance_reading * sensor_dist_scale;
Expand Down
10 changes: 10 additions & 0 deletions src/modules/simulation/gz_bridge/GZBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -798,7 +798,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)) {
Expand All @@ -807,8 +810,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);
Expand Down
Loading