Loop Closure

src/launcher/launch/se_planning_control.launch.py

@@ -8,7 +8,7 @@ def generate_launch_description():
     se_launch_description = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare("ekf_state_est"), "launch", "state_estimation.launch.py"]
+                [FindPackageShare("slam"), "launch", "slam.launch.py"]
             )
         ),
     )

src/perception_sensor_lib/CMakeLists.txt

@@ -35,6 +35,7 @@ set(IMPLEMENTATION_FILES
   src/data_association/nearest_neighbour.cpp
   src/data_association/nearest_neighbor.cpp
   src/observation_model/base_observation_model.cpp
+  src/loop_closure/lap_counter.cpp
 )
 
 add_library(${PROJECT_NAME} SHARED ${IMPLEMENTATION_FILES})

src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/lap_counter.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+#include "loop_closure.hpp"
+
+/**
+ * @brief Implementation of LoopClosure that detects when the robot returns near 
+ *        the origin and re‑observes any of the first X cones from the map.
+ */
+class LapCounter : public LoopClosure {
+public:
+  /**
+   * @param threshold_dist   distance (m) around origin to trigger closure
+   * @param first_x_cones    consider a loop if you see any of these first X cones
+   * @param border_width     distance to givve to start searching for loop closure again
+   * @param minimum_confidence minimum number of observations to confirm loop closure
+   */
+  LapCounter(double threshold_dist, int first_x_cones, int border_width, int minimum_confidence);
+
+  /**
+   * @brief Detects loop closure when returning to origin and seeing initial cones
+   * @param current_pose     your latest pose in world frame
+   * @param map_cones        full map of cones (in insertion order)
+   * @param associations     one entry per observation:
+   *                         >=3 → matched map_cones[(j-3)/2]
+   *                         -1 → new landmark, -2 → no match
+   * @param observations     raw observations (unused in this implementation)
+   * @return                 {true,0.0} once you re‑see any of map_cones[0..X-1]
+   */
+  Result detect(
+    const Eigen::Vector3d& current_pose,
+    const Eigen::VectorXi& map_cones,
+    const Eigen::VectorXi& associations,
+    const Eigen::VectorXd& observations) const override;
+
+private:
+  double threshold_dist_;
+  int first_x_cones_;
+  int border_width_;
+  int minimum_confidence_;
+  mutable int confidence_ {0};
+  mutable bool searching_{false};
+};

src/perception_sensor_lib/include/perception_sensor_lib/loop_closure/loop_closure.hpp

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <common_lib/structures/cone.hpp>
+#include <vector>
+#include <utility>
+
+/**
+ * @brief Interface for detecting loop closures
+ */
+class LoopClosure {
+public:
+
+  /**
+   * @brief Result of loop closure detection
+   * @param detected    true if a loop closure was detected
+   * @param offset      offset in meters of the map deviation
+   */
+  struct Result{
+    bool detected;
+    double offset;
+  };
+
+  virtual ~LoopClosure() = default;
+
+  /**
+   * @brief Call every time you have new observations.
+   * @param current_pose     your latest pose in world frame
+   * @param map_cones        full map of cones (in insertion order)
+   * @param associations     one entry per observation:
+   *                         >=3 → matched map_cones[(j-3)/2]
+   *                         -1 → new landmark, -2 → no match
+   * @param observations     raw observations
+   * @return                 result indicating if loop closure was detected
+   */
+  virtual Result detect(
+    const Eigen::Vector3d& current_pose,
+    const Eigen::VectorXi& map_cones,
+    const Eigen::VectorXi& associations,
+    const Eigen::VectorXd& observations) const = 0;
+};

src/perception_sensor_lib/src/loop_closure/lap_counter.cpp

@@ -0,0 +1,61 @@
+#include "perception_sensor_lib/loop_closure/lap_counter.hpp"
+#include <cmath>
+
+LapCounter::LapCounter(double threshold_dist, int first_x_cones, int border_width, int minimum_confidence)
+  : threshold_dist_(threshold_dist),
+    first_x_cones_(first_x_cones),
+    border_width_(border_width),
+    minimum_confidence_(minimum_confidence)
+{}
+
+LoopClosure::Result LapCounter::detect(
+  const Eigen::Vector3d& current_pose,
+  const Eigen::VectorXi& map_cones,
+  const Eigen::VectorXi& associations,
+  const Eigen::VectorXd& observations) const
+{
+  double dx = current_pose.x();
+  double dy = current_pose.y();
+  double dist = std::sqrt(dx * dx + dy * dy);
+
+  // add a border to give some space until we start searching again
+  if (!searching_) {
+    if (dist > threshold_dist_ + border_width_) {
+      searching_ = true;
+    } else {
+      return {false, 0.0};
+    }
+  }
+
+  // If we're still far from the origin, no closure
+  if (dist > threshold_dist_) {
+    return {false, 0.0};
+  }
+
+  bool match_found = false;
+  // Look for match with any of the first X cones
+  for (int i = 0; i < associations.size(); ++i) {
+    int j = associations[i];
+    if (j >= 3) {
+      int map_idx = (j - 3) / 2;  // Index into map_cones
+      if (map_idx < first_x_cones_) {
+        match_found = true;  // Update the outer match_found variable
+        break;  // We found a match, no need to continue searching
+      }
+    }
+  }
+
+  if (match_found) {
+    confidence_++;
+    if (confidence_ >= minimum_confidence_) {
+      // We found loop closure, reset confidence and return
+      confidence_ = 0;
+      searching_ = false;  // Reset search flag
+      return {true, 0.0};
+    }
+  } else {
+    confidence_ = 0;
+  }
+
+  return {false, 0.0};
+}

src/slam/include/ros_node/slam_node.hpp

@@ -40,6 +40,8 @@ class SLAMNode : public rclcpp::Node {
   rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr _position_publisher_;
   rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr _execution_time_publisher_;
   rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr _covariance_publisher_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr _lap_counter_publisher_;
+
   std::shared_ptr<tf2_ros::TransformBroadcaster> _tf_broadcaster_;
   rclcpp::TimerBase::SharedPtr _timer_;      /**< timer */
   std::shared_ptr<SLAMSolver> _slam_solver_; /**< SLAM solver object */
@@ -89,6 +91,12 @@ class SLAMNode : public rclcpp::Node {
    */
   void _publish_covariance();
 
+  /**
+   * @brief publishes the lap counter
+   *
+   */
+  void _publish_lap_counter();
+
 public:
   // /**
   //  * @brief Constructor of the main node, most things are received by launch parameter

src/slam/include/slam_solver/ekf_slam_solver.hpp

@@ -8,6 +8,7 @@
 #include "perception_sensor_lib/observation_model/base_observation_model.hpp"
 #include "slam_solver/slam_solver.hpp"
 
+
 class EKFSLAMSolver : public SLAMSolver {
   SLAMParameters slam_parameters_;
   std::shared_ptr<ObservationModel> observation_model_;
@@ -103,7 +104,8 @@ class EKFSLAMSolver : public SLAMSolver {
   EKFSLAMSolver(const SLAMParameters& params,
                 std::shared_ptr<DataAssociationModel> data_association,
                 std::shared_ptr<V2PMotionModel> motion_model,
-                std::shared_ptr<std::vector<double>> execution_times);
+                std::shared_ptr<std::vector<double>> execution_times,
+                std::shared_ptr<LoopClosure> loop_closure);
 
   /**
    * @brief Initialize the EKF SLAM solver
@@ -133,4 +135,11 @@ class EKFSLAMSolver : public SLAMSolver {
    * @return Eigen::MatrixXd covariance matrix
    */
   Eigen::MatrixXd get_covariance() override { return covariance_; }
+
+  /**
+   * @brief Get the lap counter
+   *
+   * @return int lap counter
+   */
+  int get_lap_counter() override { return lap_counter_; }
 };

src/slam/include/slam_solver/graph_slam_solver/graph_slam_solver.hpp

@@ -12,6 +12,8 @@
 #include "slam_solver/graph_slam_solver/optimizer/base_optimizer.hpp"
 #include "slam_solver/graph_slam_solver/pose_updater.hpp"
 #include "slam_solver/slam_solver.hpp"
+#include <perception_sensor_lib/loop_closure/lap_counter.hpp>
+
 
 /**
  * @brief Graph SLAM solver class
@@ -57,7 +59,8 @@ class GraphSLAMSolver : public SLAMSolver {
   GraphSLAMSolver(const SLAMParameters& params,
                   std::shared_ptr<DataAssociationModel> data_association,
                   std::shared_ptr<V2PMotionModel> motion_model,
-                  std::shared_ptr<std::vector<double>> execution_times);
+                  std::shared_ptr<std::vector<double>> execution_times,
+                  std::shared_ptr<LoopClosure> loop_closure);
 
   ~GraphSLAMSolver() = default;
 
@@ -103,6 +106,13 @@ class GraphSLAMSolver : public SLAMSolver {
    */
   Eigen::MatrixXd get_covariance() override;
 
+  /**
+   * @brief Get the lap counter
+   *
+   * @return int lap counter
+   */
+  int get_lap_counter() override { return lap_counter_; }
+
   /**
    * Timekeeping array
    * - 0: total motion time (prediction)

src/slam/include/slam_solver/map.hpp

@@ -14,21 +14,21 @@
 const std::map<std::string,
                std::function<std::shared_ptr<SLAMSolver>(
                    const SLAMParameters&, std::shared_ptr<DataAssociationModel>,
-                   std::shared_ptr<V2PMotionModel>, std::shared_ptr<std::vector<double>>)>,
+                   std::shared_ptr<V2PMotionModel>, std::shared_ptr<std::vector<double>>, std::shared_ptr<LoopClosure>)>,
                std::less<>>
     slam_solver_constructors_map = {
         {"graph_slam",
          [](const SLAMParameters& params, std::shared_ptr<DataAssociationModel> data_association,
             std::shared_ptr<V2PMotionModel> motion_model,
-            std::shared_ptr<std::vector<double>> execution_times) -> std::shared_ptr<SLAMSolver> {
+            std::shared_ptr<std::vector<double>> execution_times, std::shared_ptr<LoopClosure> loop_closure) -> std::shared_ptr<SLAMSolver> {
            return std::make_shared<GraphSLAMSolver>(params, data_association, motion_model,
-                                                    execution_times);
+                                                    execution_times, loop_closure);
          }},
         {"ekf_slam",
          [](const SLAMParameters& params, std::shared_ptr<DataAssociationModel> data_association,
             std::shared_ptr<V2PMotionModel> motion_model,
-            std::shared_ptr<std::vector<double>> execution_times) -> std::shared_ptr<SLAMSolver> {
+            std::shared_ptr<std::vector<double>> execution_times, std::shared_ptr<LoopClosure> loop_closure) -> std::shared_ptr<SLAMSolver> {
            return std::make_shared<EKFSLAMSolver>(params, data_association, motion_model,
-                                                  execution_times);
+                                                  execution_times, loop_closure);
          }},
 };

src/slam/include/slam_solver/slam_solver.hpp

@@ -7,6 +7,7 @@
 #include "common_lib/structures/velocities.hpp"
 #include "motion_lib/v2p_models/base_v2p_motion_model.hpp"
 #include "perception_sensor_lib/data_association/base_data_association.hpp"
+#include "perception_sensor_lib/loop_closure/loop_closure.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "slam_config/general_config.hpp"
 
@@ -23,12 +24,16 @@ class SLAMSolver {
   std::shared_ptr<std::vector<double>>
       _execution_times_;  //< Execution times: 0 -> total motion; 1 -> total
                           // observation; the rest are solver specific
+  std::shared_ptr<LoopClosure> _loop_closure_;  //< Loop closure object pointer
 
   rclcpp::Time _last_pose_update_ = rclcpp::Time(0);
   rclcpp::Time _last_observation_update_ = rclcpp::Time(0);
 
   bool _received_first_velocities_ =
       false;  //< Flag to check if the first velocities have been received
+
+
+  int lap_counter_ = 0;  //< Lap counter for the graph SLAM solver
 
 public:
   /**
@@ -38,10 +43,12 @@ class SLAMSolver {
    * @param data_association Data association module
    * @param motion_model Motion model
    * @param execution_times Timekeeping array
+   * @param loop_closure Loop closure model
    */
   SLAMSolver(const SLAMParameters& params, std::shared_ptr<DataAssociationModel> data_association,
              std::shared_ptr<V2PMotionModel> motion_model,
-             std::shared_ptr<std::vector<double>> execution_times);
+             std::shared_ptr<std::vector<double>> execution_times, 
+             std::shared_ptr<LoopClosure> loop_closure);
 
   virtual ~SLAMSolver() = default;
 
@@ -88,4 +95,11 @@ class SLAMSolver {
    * @return Eigen::MatrixXd
    */
   virtual Eigen::MatrixXd get_covariance() = 0;
+
+  /**
+   * @brief Get the lap counter
+   *
+   * @return int lap counter
+   */
+  virtual int get_lap_counter() = 0;
 };

src/slam/src/ros_node/slam_node.cpp

@@ -12,6 +12,7 @@
 #include "common_lib/structures/position.hpp"
 #include "motion_lib/v2p_models/map.hpp"
 #include "perception_sensor_lib/data_association/map.hpp"
+#include "perception_sensor_lib/loop_closure/lap_counter.hpp"
 #include "slam_solver/map.hpp"
 
 /*---------------------- Constructor --------------------*/
@@ -28,10 +29,11 @@ SLAMNode::SLAMNode(const SLAMParameters &params) : Node("slam") {
           params.observation_y_noise_));
 
   this->_execution_times_ = std::make_shared<std::vector<double>>(20, 0.0);
+  std::shared_ptr<LoopClosure> loop_closure = std::make_shared<LapCounter>(4,10,5,3);
 
   // Initialize SLAM solver object
   this->_slam_solver_ = slam_solver_constructors_map.at(params.slam_solver_name_)(
-      params, data_association, motion_model, this->_execution_times_);
+      params, data_association, motion_model, this->_execution_times_, loop_closure);
 
   _perception_map_ = std::vector<common_lib::structures::Cone>();
   _vehicle_state_velocities_ = common_lib::structures::Velocities();
@@ -67,6 +69,9 @@ SLAMNode::SLAMNode(const SLAMParameters &params) : Node("slam") {
       "/state_estimation/slam_execution_time", 10);
   this->_covariance_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
       "/state_estimation/slam_covariance", 10);
+  this->_lap_counter_publisher_ = this->create_publisher<std_msgs::msg::Float64>(
+      "/state_estimation/lap_counter", 10);
+
   this->_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 
   RCLCPP_INFO(this->get_logger(), "SLAM Node has been initialized");
@@ -106,6 +111,7 @@ void SLAMNode::_perception_subscription_callback(const custom_interfaces::msg::C
 
   this->_publish_vehicle_pose();
   this->_publish_map();
+  this->_publish_lap_counter();
 
   // Timekeeping
   rclcpp::Time end_time = this->get_clock()->now();
@@ -213,4 +219,10 @@ void SLAMNode::_publish_covariance() {
     }
   }
   this->_covariance_publisher_->publish(covariance_msg);
+}
+
+void SLAMNode::_publish_lap_counter() {
+  std_msgs::msg::Float64 lap_counter_msg;
+  lap_counter_msg.data = this->_slam_solver_->get_lap_counter();
+  this->_lap_counter_publisher_->publish(lap_counter_msg);
 }