Merge pull request #157 from cogip/134-pami-use-new-lidar-instead-of-tof-to-detect-obstacles

PAMI: use new Lidar instead of ToF to detect obstacles

Author: ecourtois

.github/workflows/build-wheel.yml

@@ -21,34 +21,10 @@ jobs:
         - name: Set up Docker Buildx
           uses: docker/setup-buildx-action@v3
 
-        - name: Cache Docker layers
-          uses: actions/cache@v4
-          with:
-            path: /tmp/.buildx-cache
-            key: ${{ runner.os }}-buildx-${{ github.sha }}
-            restore-keys: |
-              ${{ runner.os }}-buildx-
-
-        - name: Build docker image with layer caching
-          uses: docker/build-push-action@v4
-          with:
-            context: .
-            target: uv_base
-            platforms: linux/arm64
-            load: true
-            cache-from: type=local,src=/tmp/.buildx-cache
-            cache-to: type=local,dest=/tmp/.buildx-cache-new,mode=max
-
-        - # Temp fix
-          # https://github.com/docker/build-push-action/issues/252
-          # https://github.com/moby/buildkit/issues/1896
-          name: Move cache
-          run: |
-            rm -rf /tmp/.buildx-cache
-            mv /tmp/.buildx-cache-new /tmp/.buildx-cache
-
         - name: Build wheel
-          run: docker compose up build_wheel
+          run: |
+            docker compose pull build_wheel
+            docker compose up build_wheel
 
         - name: Check if wheel exists
           run: |

cogip/cpp/CMakeLists.txt

@@ -1,3 +1,10 @@
+list(
+    APPEND
+    CMAKE_INSTALL_RPATH
+    "$ORIGIN/../libraries"
+    "$ORIGIN/../drivers"
+)
+
 add_subdirectory(examples)
 add_subdirectory(drivers)
 add_subdirectory(libraries)

cogip/cpp/drivers/lidar_ld19/CMakeLists.txt

@@ -17,7 +17,7 @@ target_include_directories(
     PUBLIC
     ${CMAKE_CURRENT_SOURCE_DIR}/include
 )
-target_link_libraries(lidar_ld19 PRIVATE ${LibSerial_LIBRARIES})
+target_link_libraries(lidar_ld19 PRIVATE ${LibSerial_LIBRARIES} models shared_memory)
 set_target_properties(lidar_ld19 PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/..)
 
 # Install the library.
@@ -35,6 +35,7 @@ nanobind_add_stub(
     PYTHON_PATH ${CMAKE_BINARY_DIR}
     DEPENDS lidar_ld19
     VERBOSE
+    INSTALL_TIME
 )
 
 # Copy stub files into the source directory.

cogip/cpp/drivers/lidar_ld19/binding.cpp

@@ -8,10 +8,14 @@
 #include "lidar_ld19/ldlidar_driver.h"
 
 namespace nb = nanobind;
+using namespace nb::literals;
 
 namespace ldlidar {
 
 NB_MODULE(lidar_ld19, m) {
+    auto models_module = nb::module_::import_("cogip.cpp.libraries.models");
+    auto shared_memory_module = nb::module_::import_("cogip.cpp.libraries.shared_memory");
+
     nb::enum_<LibSerial::BaudRate>(m, "BaudRate")
         .value("BAUD_230400", LibSerial::BaudRate::BAUD_230400);
 
@@ -24,10 +28,9 @@ NB_MODULE(lidar_ld19, m) {
 
     nb::class_<LDLidarDriver>(m, "LDLidarDriver")
         .def(nb::init<>(), "Constructor that internally manages memory")
-        .def(
-            nb::init<nb::ndarray<uint16_t, nb::numpy, nb::shape<ldlidar::NUM_ANGLES, 2>>>(),
-            "Constructor with external NumPy array",
-            nb::arg("external_lidar_points")
+        .def(nb::init<nb::ndarray<float, nb::numpy, nb::shape<MAX_DATA_COUNT, 3>>>(),
+             "Constructor accepting nanobind::ndarray",
+             "external_lidar_data"_a
         )
         .def("connect", &LDLidarDriver::connect)
         .def("disconnect", &LDLidarDriver::disconnect)
@@ -45,12 +48,18 @@ NB_MODULE(lidar_ld19, m) {
         )
         .def(
             "get_lidar_data",
-            [](const LDLidarDriver &self) -> nb::ndarray<uint16_t, nb::numpy, nb::shape<NUM_ANGLES, 2>> {
+            [](const LDLidarDriver &self) -> nb::ndarray<float, nb::numpy, nb::shape<MAX_DATA_COUNT, 3>> {
                 const auto &points = self.getLidarData();
-                return nb::ndarray<uint16_t, nb::numpy, nb::shape<NUM_ANGLES, 2>>((void *)points);
+                return nb::ndarray<float, nb::numpy, nb::shape<MAX_DATA_COUNT, 3>>((void *)points);
             },
             nb::rv_policy::reference_internal
-        );
+        )
+        .def("set_data_write_lock", &LDLidarDriver::setDataWriteLock, "Set the data write lock", "lock"_a)
+        .def("set_min_intensity", &LDLidarDriver::setMinIntensity, "Set the minimum intensity value to validate data", "min_intensity"_a)
+        .def("set_min_distance", &LDLidarDriver::setMinDistance, "Set the minimum distance to validate data", "min_distance"_a)
+        .def("set_max_distance", &LDLidarDriver::setMaxDistance, "Set the maximum distance to validate data", "max_distance"_a)
+        .def("set_invalid_angle_range", &LDLidarDriver::setInvalidAngleRange, "Set the invalid angle range", "min_angle"_a, "max_angle"_a)
+    ;
 }
 
 } // namespace ldlidar

cogip/cpp/drivers/lidar_ld19/include/lidar_ld19/ldlidar_driver.h

@@ -2,6 +2,7 @@
 
 #include "lidar_ld19/ldlidar_datatype.h"
 #include "lidar_ld19/ldlidar_protocol.h"
+#include "shared_memory/WritePriorityLock.hpp"
 
 #include <libserial/SerialPort.h>
 
@@ -19,17 +20,17 @@ namespace nb = nanobind;
 namespace ldlidar {
 
 constexpr size_t MAX_ACK_BUF_LEN = 512;
-constexpr size_t NUM_ANGLES = 360;
+constexpr std::size_t MAX_DATA_COUNT = 1024;
 
 uint64_t getSystemTimeStamp();
 
 class LDLidarDriver {
 public:
     /// Constructor with optional external memory pointer
-    explicit LDLidarDriver(uint16_t (*external_lidar_data)[2] = nullptr);
+    explicit LDLidarDriver(float (*external_lidar_data)[3] = nullptr);
 
     /// Constructor accepting a nanobind::ndarray
-    explicit LDLidarDriver(nb::ndarray<uint16_t, nb::numpy, nb::shape<NUM_ANGLES, 2>> external_lidar_data);
+    explicit LDLidarDriver(nb::ndarray<float, nb::numpy, nb::shape<MAX_DATA_COUNT, 3>> external_lidar_data);
 
     ~LDLidarDriver();
 
@@ -82,8 +83,17 @@ class LDLidarDriver {
     /// Function executed after reading data on serial port.
     void commReadCallback(const char *byte, size_t len);
 
+    /// Set the minimum intensity value to validate data.
+    void setMinIntensity(uint8_t min_intensity) { min_intensity_ = min_intensity; }
+
+    /// Set the minimum distance to validate data.
+    void setMinDistance(uint16_t min_distance) { min_distance_ = min_distance; }
+
+    /// Set the maximum distance to validate data.
+    void setMaxDistance(uint16_t max_distance) { max_distance_ = max_distance; }
+
     /// Get pointer to internal lidar points
-    const uint16_t (&getLidarData() const)[2] { return *lidar_data_; }
+    const float (&getLidarData() const)[3] { return *lidar_data_; }
 
     /// Get Lidar spin speed (Hz)
     double getSpeed() const { return (speed_ / 360.0); };
@@ -103,6 +113,17 @@ class LDLidarDriver {
         tmp_lidar_scan_data_vec_.clear();
     }
 
+    /// Set the data write lock.
+    void setDataWriteLock(cogip::shared_memory::WritePriorityLock &lock) {
+        data_write_lock_ = &lock;
+    }
+
+    /// Set invalid angle range
+    void setInvalidAngleRange(uint16_t min_angle, uint16_t max_angle) {
+        min_angle_ = min_angle;
+        max_angle_ = max_angle;
+    }
+
 protected:
     bool is_start_flag_;
     bool is_connect_flag_;
@@ -117,15 +138,20 @@ class LDLidarDriver {
     LidarStatus lidar_status_;
     uint8_t lidar_error_code_;
     bool is_frame_ready_;
-    bool is_noise_filter_;
+    bool external_data_;      ///< Flag to indicate if memory is externally managed
+    float (*lidar_data_)[3];  ///< Pointer to lidar data memory
+    cogip::shared_memory::WritePriorityLock *data_write_lock_;
+    uint8_t min_intensity_;
     uint16_t timestamp_;
+    uint16_t min_distance_;
+    uint16_t max_distance_;
+    uint16_t min_angle_;
+    uint16_t max_angle_;
     double speed_;
     bool is_poweron_comm_normal_;
     uint64_t last_pkg_timestamp_;
     LdLidarProtocol *protocol_handle_;
     Points2D tmp_lidar_scan_data_vec_;
-    uint16_t (*lidar_data_)[2];  ///< Pointer to lidar data memory
-    bool external_data_;           ///< Flag to indicate if memory is externally managed
     std::mutex mutex_lock1_;
     std::mutex mutex_lock2_;
 

cogip/cpp/drivers/lidar_ld19/ldlidar_driver.cpp

@@ -11,8 +11,7 @@
 
 namespace ldlidar {
 
-constexpr uint16_t max_distance = 3000;
-constexpr uint8_t min_intensity = 150;
+constexpr size_t FILTERED_DATA_COUNT = 360;
 
 uint64_t getSystemTimeStamp() {
     std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> tp =
@@ -23,20 +22,20 @@ uint64_t getSystemTimeStamp() {
 
 bool LDLidarDriver::is_ok_ = false;
 
-LDLidarDriver::LDLidarDriver(uint16_t (*external_lidar_data)[2]):
+LDLidarDriver::LDLidarDriver(float (*external_lidar_data)[3]):
     lidar_data_(external_lidar_data),
     external_data_(external_lidar_data != nullptr)
 {
     if (!external_data_) {
         // Allocate memory if external pointer is not provided
-        lidar_data_ = new uint16_t[NUM_ANGLES][2]();
+        lidar_data_ = new float[MAX_DATA_COUNT][3]();
     }
 
     commonInit();
 }
 
-LDLidarDriver::LDLidarDriver(nb::ndarray<uint16_t, nb::numpy, nb::shape<NUM_ANGLES, 2>> external_lidar_points):
-    lidar_data_(reinterpret_cast<uint16_t(*)[2]>(external_lidar_points.data())),
+LDLidarDriver::LDLidarDriver(nb::ndarray<float, nb::numpy, nb::shape<MAX_DATA_COUNT, 3>> external_lidar_raw_points):
+    lidar_data_(reinterpret_cast<float(*)[3]>(external_lidar_raw_points.data())),
     external_data_(true)
 {
     if (!lidar_data_) {
@@ -53,6 +52,12 @@ void LDLidarDriver::commonInit() {
     lidar_status_ = LidarStatus::NORMAL;
     lidar_error_code_ = LIDAR_NO_ERROR;
     is_frame_ready_ = false;
+    data_write_lock_ = nullptr;
+    min_intensity_ = 0;
+    min_distance_ = 0;
+    max_distance_ = std::numeric_limits<uint16_t>::max();
+    min_angle_ = 0;
+    max_angle_ = 360;
     timestamp_ = 0;
     speed_ = 0;
     is_poweron_comm_normal_ = false;
@@ -340,34 +345,49 @@ void LDLidarDriver::setFrameReady() {
 
 void LDLidarDriver::setLaserScanData(Points2D &src) {
     std::lock_guard<std::mutex> lg(mutex_lock2_);
-    std::array<uint16_t, NUM_ANGLES> result_distances;
-    std::array<uint8_t, NUM_ANGLES> result_intensities;
-    std::array<std::vector<uint16_t>, NUM_ANGLES> tmp_distances;
-    std::array<std::vector<uint8_t>, NUM_ANGLES> tmp_intensities;
+    std::array<std::vector<uint16_t>, FILTERED_DATA_COUNT> tmp_distances;
+    std::array<std::vector<uint8_t>, FILTERED_DATA_COUNT> tmp_intensities;
 
     // Build a list of points for each integer degree
+    std::size_t count = 0;
+
+    // Compute mean of points list for each degree.
+    if (data_write_lock_ != nullptr) {
+        data_write_lock_->startWriting();
+    }
+
     for (const auto &point: src) {
-        if (point.intensity < min_intensity) {
+        if (point.intensity < min_intensity_) {
             continue;
         }
-        size_t angle = (static_cast<size_t>(point.angle) + NUM_ANGLES) % NUM_ANGLES;
-        tmp_distances[angle].push_back(point.distance);
-        tmp_intensities[angle].push_back(point.intensity);
-    }
-
-    // Compute mean of points list for each degree.
-    for (size_t angle = 0; angle < NUM_ANGLES; angle++) {
-        const auto &distances = tmp_distances[angle];
-        const auto &intensities = tmp_intensities[angle];
-        size_t size = distances.size();
-        if (size > 0) {
-            lidar_data_[angle][0] = std::accumulate(distances.begin(), distances.end(), 0) / size;
-            lidar_data_[angle][1] = std::accumulate(intensities.begin(), intensities.end(), 0) / size;
+        if (point.distance < min_distance_) {
+            continue;
+        }
+        if (point.distance > max_distance_) {
+            continue;
         }
-        else {
-            lidar_data_[angle][0] = max_distance;  // Mark as invalid
-            lidar_data_[angle][1] = 0;  // Mark as invalid
+
+        double angle = 360 - point.angle; // Lidar angle is inverted
+
+        // Skip excluded angles
+        if (angle > min_angle_ && angle < max_angle_) {
+            continue;
         }
+
+        lidar_data_[count][0] = angle;
+        lidar_data_[count][1] = point.distance;
+        lidar_data_[count][2] = point.intensity;
+        count++;
+    }
+
+    // Mark as end of data
+    lidar_data_[count][0] = -1.0;
+    lidar_data_[count][1] = -1.0;
+    lidar_data_[count][2] = -1.0;
+
+    if (data_write_lock_ != nullptr) {
+        data_write_lock_->finishWriting();
+        data_write_lock_->postUpdate();
     }
 }
 

cogip/cpp/examples/nanobind_example/CMakeLists.txt

@@ -27,6 +27,7 @@ nanobind_add_stub(
     PYTHON_PATH ${CMAKE_BINARY_DIR}
     DEPENDS nanobind_example
     VERBOSE
+    INSTALL_TIME
 )
 
 # Copy stub files into the source directory.

cogip/cpp/libraries/CMakeLists.txt

@@ -2,3 +2,4 @@ add_subdirectory(models)
 add_subdirectory(obstacles)
 add_subdirectory(shared_memory)
 add_subdirectory(avoidance)
+add_subdirectory(utils)

cogip/cpp/libraries/avoidance/CMakeLists.txt

@@ -46,6 +46,7 @@ nanobind_add_stub(
     PYTHON_PATH ${CMAKE_BINARY_DIR}
     DEPENDS avoidance models_stub obstacles_stub
     VERBOSE
+    INSTALL_TIME
 )
 
 # Copy stub files into the source directory.

cogip/cpp/libraries/models/CMakeLists.txt

@@ -3,6 +3,8 @@
 add_library(
     models_cpp
     SHARED
+    Circle.cpp
+    CircleList.cpp
     Coords.cpp
     CoordsList.cpp
     Polar.cpp
@@ -41,6 +43,7 @@ nanobind_add_stub(
     PYTHON_PATH ${CMAKE_BINARY_DIR}
     DEPENDS models
     VERBOSE
+    INSTALL_TIME
 )
 
 # Copy stub files into the source directory.