diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 614d408a5e30..072289d0a9aa 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/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': diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 0733f1b289d4..24a43c193a55 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -393,6 +393,7 @@ 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)); @@ -400,11 +401,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); + 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; @@ -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) { diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 8fcc7c28f3ec..e8ce28afa987 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -43,7 +43,6 @@ #pragma once #include - #include #include #include diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3ff1c321e79b..b26f73f3c654 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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"); diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 95be696835e2..b4626a6f45a3 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/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);