From 77aea028d54fa0887155b36e7e3f0cf384761930 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 8 Jan 2025 16:10:16 +0100 Subject: [PATCH] CollisionPrevention/SF45: move common projection function to library --- boards/px4/fmu-v5x/default.px4board | 1 + .../lightware_sf45_serial/CMakeLists.txt | 1 + .../lightware_sf45_serial.cpp | 21 ++------ .../lightware_sf45_serial.hpp | 1 - src/lib/collision_prevention/CMakeLists.txt | 7 ++- .../CollisionPrevention.cpp | 15 ++---- .../CollisionPrevention.hpp | 2 +- src/lib/collision_prevention/ObstacleMath.cpp | 54 +++++++++++++++++++ src/lib/collision_prevention/ObstacleMath.hpp | 47 ++++++++++++++++ 9 files changed, 118 insertions(+), 31 deletions(-) create mode 100644 src/lib/collision_prevention/ObstacleMath.cpp create mode 100644 src/lib/collision_prevention/ObstacleMath.hpp diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 8ba41726c086..dd3869afa962 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -109,3 +109,4 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_SF45_SERIAL=y diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt index 561b7755c76d..6d561f72db08 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt +++ b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( DEPENDS drivers_rangefinder px4_work_queue + CollisionPrevention MODULE_CONFIG module.yaml ) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index 55fb9ee8fecc..cb8a8753629e 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -33,14 +33,15 @@ #include "lightware_sf45_serial.hpp" -#include #include +#include +#include #include + #include #include #include - -#include +#include using namespace time_literals; using matrix::Quatf; @@ -660,7 +661,7 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) } } - _project_distance_on_horizontal_plane(*distance_m, scaled_yaw, _vehicle_attitude); + ObstacleMath::project_distance_on_horizontal_plane(*distance_m, scaled_yaw, _vehicle_attitude); if (_current_bin_dist > _obstacle_distance.max_distance) { _current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition @@ -741,18 +742,6 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_ } } } -void SF45LaserSerial::_project_distance_on_horizontal_plane(float &distance, const int16_t &yaw, - const matrix::Quatf &q_world_vehicle) -{ - const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f))); - const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor; - const Vector3f forward(1.f, 0.f, 0.f); - const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward); - - float horizontal_projection_scale = sensor_direction_in_world.xy().norm(); - horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f); - distance *= horizontal_projection_scale; -} uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) { diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index d234bc38f9ed..3903f707c2b4 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -114,7 +114,6 @@ class SF45LaserSerial : public px4::ScheduledWorkItem bool _crc_valid{false}; void _handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now); - void _project_distance_on_horizontal_plane(float &distance, const int16_t &yaw, const matrix::Quatf &q_world_vehicle); void _publish_obstacle_msg(hrt_abstime now); uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uint64_t _data_timestamps[BIN_COUNT]; diff --git a/src/lib/collision_prevention/CMakeLists.txt b/src/lib/collision_prevention/CMakeLists.txt index 663c9e9fee2f..1b62a10f63a9 100644 --- a/src/lib/collision_prevention/CMakeLists.txt +++ b/src/lib/collision_prevention/CMakeLists.txt @@ -31,7 +31,12 @@ # ############################################################################ -px4_add_library(CollisionPrevention CollisionPrevention.cpp) +px4_add_library(CollisionPrevention + CollisionPrevention.cpp + ObstacleMath.cpp +) target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable +target_include_directories(CollisionPrevention PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(CollisionPrevention PRIVATE mathlib) px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index b2c62d4906c6..3f1344ffdd1b 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +38,7 @@ */ #include "CollisionPrevention.hpp" +#include "ObstacleMath.hpp" #include using namespace matrix; @@ -400,18 +401,8 @@ 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))); - - 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; + ObstacleMath::project_distance_on_horizontal_plane(distance_reading, sensor_yaw_body_rad, vehicle_attitude); } uint16_t sensor_range = static_cast(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 8fcc7c28f3ec..7d9d16cd7fca 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/lib/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp new file mode 100644 index 000000000000..3610b513d0ff --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMath.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ObstacleMath.hpp" +#include + +using namespace matrix; + +namespace ObstacleMath +{ + +void project_distance_on_horizontal_plane(float &distance, const int16_t yaw, const matrix::Quatf &q_world_vehicle) +{ + const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f))); + const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor; + const Vector3f forward(1.f, 0.f, 0.f); + const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward); + + float horizontal_projection_scale = sensor_direction_in_world.xy().norm(); + horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f); + distance *= horizontal_projection_scale; +} + +} // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMath.hpp b/src/lib/collision_prevention/ObstacleMath.hpp new file mode 100644 index 000000000000..ae18b7da402d --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMath.hpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +namespace ObstacleMath +{ + +/** + * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane + * @param distance measurement which is scaled down + * @param yaw orientation of the measurement on the body horizontal plane + * @param q_world_vehicle vehicle attitude quaternion + */ +void project_distance_on_horizontal_plane(float &distance, const int16_t yaw, const matrix::Quatf &q_world_vehicle); + +} // ObstacleMath