Merge pull request #153 from cogip/152-detector-enable-lidar-scan-delay-compensation

Detector: enable Lidar scan delay compensation

Author: ecourtois

cogip/cpp/libraries/models/CMakeLists.txt

@@ -7,6 +7,7 @@ add_library(
     CoordsList.cpp
     Polar.cpp
     Pose.cpp
+    PoseBuffer.cpp
 )
 target_include_directories(
     models_cpp

cogip/cpp/libraries/models/PoseBuffer.cpp

@@ -0,0 +1,72 @@
+// Copyright (C) 2025 COGIP Robotics association <cogip35@gmail.com>
+// This file is subject to the terms and conditions of the GNU Lesser
+// General Public License v2.1. See the file LICENSE in the top level directory.
+
+#include "models/PoseBuffer.hpp"
+
+#include <cmath>
+#include <cstring>
+
+namespace cogip {
+
+namespace models {
+
+PoseBuffer::PoseBuffer(pose_buffer_t* data):
+    data_(data),
+    external_data_(data != nullptr)
+{
+    if (!external_data_) {
+        // Allocate memory if external pointer is not provided
+        data_ = new pose_buffer_t();
+    }
+    data_->head = 0;
+    data_->tail = 0;
+    data_->full = false;
+
+}
+
+PoseBuffer::~PoseBuffer()
+{
+    if (!external_data_) {
+        delete data_;
+    }
+};
+
+double PoseBuffer::full() const
+{
+
+};
+
+std::size_t PoseBuffer::size() const
+{
+    if (data_->full) return POSE_BUFFER_SIZE_MAX;
+    if (data_->head >= data_->tail) return data_->head - data_->tail;
+    return POSE_BUFFER_SIZE_MAX + data_->head - data_->tail;
+};
+
+void PoseBuffer::push(float x, float y, float angle)
+{
+    data_->poses[data_->head].x = x;
+    data_->poses[data_->head].y = y;
+    data_->poses[data_->head].angle = angle;
+    if (data_->full) {
+        data_->tail = (data_->tail + 1) % POSE_BUFFER_SIZE_MAX; // Advance tail if full
+    }
+    data_->head = (data_->head + 1) % POSE_BUFFER_SIZE_MAX;
+    data_->full = (data_->head == data_->tail);
+};
+
+Pose PoseBuffer::get(std::size_t n) const
+{
+    if (n >= size()) {
+        throw std::out_of_range("Requested index is out of bounds.");
+    }
+
+    // Calculate index relative to head (reverse order)
+    size_t index = (data_->head + POSE_BUFFER_SIZE_MAX - 1 - n) % POSE_BUFFER_SIZE_MAX;
+    return Pose(&data_->poses[index]);
+};
+
+} // namespace models
+
+} // namespace cogip

cogip/cpp/libraries/models/binding.cpp

@@ -4,6 +4,7 @@
 
 #include "models/CoordsList.hpp"
 #include "models/Pose.hpp"
+#include "models/PoseBuffer.hpp"
 #include "models/binding.hpp"
 
 #include <nanobind/nanobind.h>
@@ -165,6 +166,33 @@ NB_MODULE(models, m) {
             }
         )
     ;
+
+    // Bind pose_buffer_t structure
+    nb::class_<pose_buffer_t>(m, "PoseBufferT")
+        .def_rw("head", &pose_buffer_t::head, "Next write pose")
+        .def_rw("tail", &pose_buffer_t::tail, "Oldest pose")
+        .def("__repr__", [](const pose_buffer_t& buffer) {
+            std::ostringstream oss;
+            oss << buffer;
+            return oss.str();
+        })
+    ;
+
+    // Bind PoseBuffer class
+    nb::class_<PoseBuffer>(m, "PoseBuffer")
+        .def(nb::init<pose_buffer_t*>(), "Constructor", "data"_a = nullptr)
+        .def_prop_ro("head", &PoseBuffer::head, "Next write pose")
+        .def_prop_ro("tail", &PoseBuffer::tail, "Oldest pose")
+        .def("push", &PoseBuffer::push, "Get last pose pushed in the buffer", "x"_a, "y"_a, "angle"_a)
+        .def_prop_ro("last", &PoseBuffer::last, "Get last pose pushed in the buffer")
+        .def("get", &PoseBuffer::get, "Get the N-th position from head (0 is the most recent)", "n"_a)
+        .def("__repr__", [](const PoseBuffer &buffer) {
+            std::ostringstream oss;
+            oss << buffer;
+            return oss.str();
+        })
+    ;
+
 }
 
 } // namespace models

cogip/cpp/libraries/models/include/models/PoseBuffer.hpp

@@ -0,0 +1,76 @@
+// Copyright (C) 2025 COGIP Robotics association <cogip35@gmail.com>
+// This file is subject to the terms and conditions of the GNU Lesser
+// General Public License v2.1. See the file LICENSE in the top level directory.
+
+/// @ingroup     lib_models
+/// @{
+/// @file
+/// @brief       PoseBuffer declaration
+/// @author      Eric Courtois <eric.courtois@gmail.com>
+
+#pragma once
+
+#include "models/pose_buffer.hpp"
+#include "models/Pose.hpp"
+
+#include <ostream>
+
+namespace cogip {
+
+namespace models {
+
+class PoseBuffer {
+public:
+    /// Constructor.
+    PoseBuffer(
+        pose_buffer_t* data      ///< [in] Pointer to an existing data structure
+                                 ///<      If nullptr, will allocate one internally
+    );
+
+    /// Destructor
+    ~PoseBuffer();
+
+    /// Return the pointer to the underlying data structure.
+    pose_buffer_t* data() { return data_; };
+
+    /// Return head.
+    double head() const { return data_->head; };
+
+    /// Return tail.
+    double tail() const { return data_->tail; };
+
+    /// Return true if the buffer is full, false otherwise.
+    double full() const;
+
+    /// Get the number of stored positions
+    std::size_t size() const;
+
+    /// Add a new pose to the buffer.
+    void push(float x, float y, float angle);
+
+    /// Get last pose pushed in the buffer.
+    Pose last() const { return get(0); };
+
+    /// Get the N-th position from head (0 is the most recent).
+    Pose get(std::size_t n) const;
+
+protected:
+    pose_buffer_t* data_;   ///< pointer to internal data structure
+    bool external_data_;    ///< Flag to indicate if memory is externally managed
+};
+
+/// Overloads the stream insertion operator for `PoseBuffer`.
+/// Prints the PoseBuffer in a human-readable format.
+/// @param os The output stream.
+/// @param buffer The PoseBuffer to print.
+/// @return A reference to the output stream.
+inline std::ostream& operator<<(std::ostream& os, const PoseBuffer& buffer) {
+    os << "PoseBuffer(size=" << buffer.size() << "/" << POSE_BUFFER_SIZE_MAX << ", head=" << buffer.head() << ", tail=" << buffer.tail() << ")";
+    return os;
+}
+
+} // namespace models
+
+} // namespace cogip
+
+/// @}

cogip/cpp/libraries/models/include/models/pose_buffer.hpp

@@ -0,0 +1,53 @@
+// Copyright (C) 2025 COGIP Robotics association <cogip35@gmail.com>
+// This file is subject to the terms and conditions of the GNU Lesser
+// General Public License v2.1. See the file LICENSE in the top level directory.
+
+/// @ingroup     lib_models
+/// @{
+/// @file
+/// @brief       pose_buffer_t class declaration
+/// @author      Eric Courtois <eric.courtois@gmail.com>
+
+#pragma once
+
+#include "models/pose.hpp"
+
+#include <ostream>
+
+namespace cogip {
+
+namespace models {
+
+constexpr std::size_t POSE_BUFFER_SIZE_MAX = 256;
+
+/// A circular buffer to store pose_t.
+typedef struct {
+    pose_t poses[POSE_BUFFER_SIZE_MAX];  ///< Poses list
+    size_t head;                         ///< Next write pose
+    size_t tail;                         ///< Oldest pose
+    bool full;                           ///< Indicates if the buffer is full
+} pose_buffer_t;
+
+/// Overloads the stream insertion operator for `pose_buffer_t`.
+/// Prints the pose in a human-readable format.
+/// @param os The output stream.
+/// @param buffer The buffer to print.
+/// @return A reference to the output stream.
+inline std::ostream& operator<<(std::ostream& os, const pose_buffer_t& buffer) {
+        size_t size = 0;
+        if (buffer.head >= buffer.tail){
+            size = buffer.head - buffer.tail;
+        }
+        else {
+            size = POSE_BUFFER_SIZE_MAX + buffer.head - buffer.tail;
+        }
+
+    os << "pose_buffer_t(size=" << size << "/" << POSE_BUFFER_SIZE_MAX << ", head=" << buffer.head << ", tail=" << buffer.tail << ")";
+    return os;
+}
+
+} // namespace cogip_defs
+
+} // namespace cogip
+
+/// @}

cogip/cpp/libraries/shared_memory/SharedMemory.cpp

@@ -56,6 +56,7 @@ SharedMemory::SharedMemory(const std::string& name, bool owner):
     for (const auto& [lock, name] : lock2str) {
         locks_.emplace(lock, std::make_unique<WritePriorityLock>(name_ + "_" + name, owner_));
     }
+    pose_current_buffer_ = new models::PoseBuffer(&data_->pose_current_buffer);
     pose_current_ = new models::Pose(&data_->pose_current);
     pose_order_ = new models::Pose(&data_->pose_order);
     detector_obstacles_ = new models::CoordsList(&data_->detector_obstacles);
@@ -73,6 +74,7 @@ SharedMemory::~SharedMemory() {
     delete detector_obstacles_;
     delete pose_order_;
     delete pose_current_;
+    delete pose_current_buffer_;
 
     if (data_ != nullptr) {
         munmap(data_, sizeof(shared_data_t));

cogip/cpp/libraries/shared_memory/binding.cpp

@@ -25,6 +25,7 @@ NB_MODULE(shared_memory, m) {
 
     nb::class_<shared_data_t>(m, "SharedData")
         .def(nb::init<>())
+        .def_rw("pose_current_buffer", &shared_data_t::pose_current_buffer)
         .def_rw("pose_current", &shared_data_t::pose_current)
         .def_rw("pose_order", &shared_data_t::pose_order)
         .def("__repr__", [](const shared_data_t &s) {
@@ -68,6 +69,8 @@ NB_MODULE(shared_memory, m) {
              "Get a lock for a specific part of the shared memory.")
         .def("get_data", &SharedMemory::getData, nb::rv_policy::reference,
              "Get the shared data.")
+        .def("get_pose_current_buffer", &SharedMemory::getPoseCurrentBuffer, nb::rv_policy::reference_internal,
+             "Get PoseBuffer object wrapping the shared memory pose_current_buffer structure.")
         .def("get_pose_current", &SharedMemory::getPoseCurrent, nb::rv_policy::reference_internal,
              "Get Pose object wrapping the shared memory pose_current structure.")
         .def("get_pose_order", &SharedMemory::getPoseOrder, nb::rv_policy::reference_internal,

cogip/cpp/libraries/shared_memory/include/shared_memory/SharedMemory.hpp

@@ -5,6 +5,7 @@
 #include "shared_memory/shared_data.hpp"
 #include "shared_memory/WritePriorityLock.hpp"
 
+#include "models/PoseBuffer.hpp"
 #include "obstacles/ObstacleCircleList.hpp"
 #include "obstacles/ObstacleRectangleList.hpp"
 
@@ -46,6 +47,9 @@ class SharedMemory {
     /// Retrieves a pointer to the shared memory data structure.
     shared_data_t* getData() { return data_; }
 
+    /// Retrieves a pointer to the PoseBuffer object wrapping the shared memory pose_current_buffer structure.
+    models::PoseBuffer* getPoseCurrentBuffer() { return pose_current_buffer_; }
+
     /// Retrieves a pointer to the Pose object wrapping the shared memory pose_current structure.
     models::Pose* getPoseCurrent() { return pose_current_; }
 
@@ -73,6 +77,7 @@ class SharedMemory {
     int shm_fd_;           ///< File descriptor for the shared memory.
     shared_data_t* data_;  ///< Pointer to the shared memory data structure.
     std::map<LockName, std::shared_ptr<WritePriorityLock>> locks_;  ///< Map of locks for synchronization.
+    models::PoseBuffer* pose_current_buffer_;  ///< Pointer to the PoseBuffer object wrapping the shared memory pose_current_buffer structure.
     models::Pose* pose_current_;  ///< Pointer to the Pose object wrapping the shared memory pose_current structure.
     models::Pose* pose_order_;    ///< Pointer to the Pose object wrapping the shared memory pose_order structure.
     models::CoordsList* detector_obstacles_;  ///< Pointer to the CoordsList object wrapping the shared memory detector_obstacles structure.

cogip/cpp/libraries/shared_memory/include/shared_memory/shared_data.hpp

@@ -6,6 +6,7 @@
 
 #include "models/coords_list.hpp"
 #include "models/pose.hpp"
+#include "models/pose_buffer.hpp"
 #include "obstacles/obstacle_circle_list.hpp"
 #include "obstacles/obstacle_polygon_list.hpp"
 
@@ -21,6 +22,7 @@ constexpr std::size_t NUM_ANGLES = 360;
 
 /// Represents shared data in shared memory.
 typedef struct {
+    models::pose_buffer_t pose_current_buffer;  ///< The last current poses.
     models::pose_t pose_current;  ///< The current pose.
     models::pose_t pose_order;    ///< The target pose.
     uint16_t lidar_data[NUM_ANGLES][2];  ///< The Lidar data.

cogip/entities/robot.py

@@ -78,6 +78,11 @@ def __init__(self, robot_id: int, win: MainWindow, parent: Qt3DCore.QEntity | No
         self.update_pose_current_timer = QtCore.QTimer()
         self.update_pose_current_timer.timeout.connect(self.update_pose_current)
 
+        # Create a timer for consistent hit updates
+        self.update_hit_timer = QtCore.QTimer()
+        self.update_hit_timer.setTimerType(QtCore.Qt.TimerType.PreciseTimer)
+        self.update_hit_timer.start(RobotEntity.update_pose_current_interval)
+
     def setEnabled(self, isEnabled):
         if isEnabled:
             self.shared_memory = SharedMemory(f"cogip_{self.robot_id}")
@@ -148,6 +153,7 @@ def add_lidar_sensors(self):
             )
             sensor.shared_sensor_data = self.shared_lidar_data
             self.sensors.append(sensor)
+            self.update_hit_timer.timeout.connect(sensor.ray_caster.trigger)
 
     def add_tof_sensor(self):
         """