Skip to content
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.

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

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

Open
wants to merge 10 commits into
base: main
Choose a base branch
from
Original file line number Diff line number Diff line change
Expand Up @@ -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':
Expand Down
17 changes: 13 additions & 4 deletions src/lib/collision_prevention/CollisionPrevention.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(Eulerf(0.0f, 0.0f, sensor_yaw_body_rad));

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;
Expand All @@ -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)
{
Expand Down
1 change: 0 additions & 1 deletion src/lib/collision_prevention/CollisionPrevention.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,6 @@
#pragma once

#include <float.h>

#include <commander/px4_custom_mode.h>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
Expand Down
1 change: 1 addition & 0 deletions src/modules/logger/logged_topics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
14 changes: 14 additions & 0 deletions src/modules/simulation/gz_bridge/GZBridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Expand All @@ -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);
Expand Down
Loading