Online cone discovery: track candidates across frames, confirm after 3 hits

CMakeLists.txt

@@ -26,6 +26,7 @@ add_executable(nav_lite_node
   src/local_planner.cpp
   src/bt_executor.cpp
   src/session_mapper.cpp
+  src/cone_updater.cpp
 )
 
 target_include_directories(nav_lite_node PRIVATE

include/car_nav_lite/cone_updater.hpp

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <vector>
+#include <Eigen/Dense>
+#include "car_nav_lite/types.hpp"
+#include "car_nav_lite/fence_model.hpp"
+
+namespace car_nav_lite {
+
+class ConeUpdater {
+public:
+    ConeUpdater();
+
+    // Process one scan frame: extract cone candidates, track across frames.
+    // confirmed_cones is the current known cone list (for dedup).
+    void update(const LaserScan& scan, const Pose2D& pose,
+                const FenceModel& fence,
+                const std::vector<Eigen::Vector2d>& known_cones);
+
+    // Returns newly confirmed cones and clears them from pending.
+    std::vector<Eigen::Vector2d> popConfirmed();
+
+private:
+    struct Candidate {
+        Eigen::Vector2d center;
+        int hits = 0;
+        int age = 0;
+    };
+    std::vector<Candidate> candidates_;
+
+    static constexpr int CONFIRM_HITS = 3;
+    static constexpr double CLUSTER_DIST = 0.10;
+    static constexpr double MERGE_DIST = 0.25;
+    static constexpr double WALL_DIST = 0.15;
+    static constexpr int MIN_POINTS = 4;
+};
+
+}  // namespace car_nav_lite

src/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.8)
+project(car_nav_lite)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(OpenCV REQUIRED)
+
+set(INCLUDE_DIR include)
+
+add_executable(nav_lite_node
+  src/main.cpp
+  src/grid_map.cpp
+  src/localizer.cpp
+  src/global_planner.cpp
+  src/local_planner.cpp
+  src/bt_executor.cpp
+  src/session_mapper.cpp
+  src/cone_updater.cpp
+)
+
+target_include_directories(nav_lite_node PRIVATE
+  ${INCLUDE_DIR}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+ament_target_dependencies(nav_lite_node
+  rclcpp std_msgs geometry_msgs nav_msgs sensor_msgs tf2 tf2_ros
+)
+target_link_libraries(nav_lite_node ${OpenCV_LIBS})
+
+add_executable(calib_check src/calib_check.cpp)
+target_include_directories(calib_check PRIVATE ${INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS})
+ament_target_dependencies(calib_check rclcpp geometry_msgs nav_msgs tf2 tf2_ros)
+install(TARGETS nav_lite_node calib_check DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/maps")
+  install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
+endif()
+
+ament_package()

src/cone_updater.cpp

@@ -0,0 +1,114 @@
+#include "car_nav_lite/cone_updater.hpp"
+#include <cmath>
+#include <cstdio>
+#include <algorithm>
+
+namespace car_nav_lite {
+
+ConeUpdater::ConeUpdater() {}
+
+void ConeUpdater::update(const LaserScan& scan, const Pose2D& pose,
+                          const FenceModel& fence,
+                          const std::vector<Eigen::Vector2d>& known_cones) {
+    if (!fence.valid) return;
+
+    // ── 1. Extract non-wall, non-known-cone world points ──
+    std::vector<Eigen::Vector2d> pts;
+    double cy = std::cos(pose.yaw), sy = std::sin(pose.yaw);
+    for (int i = 0; i < scan.size(); i += 3) {
+        double r = scan.ranges[i];
+        if (r < 0.15 || r > 4.5) continue;
+        double a = scan.angle_min + i * scan.angle_increment;
+        double wx = pose.x + r * std::cos(a + pose.yaw);
+        double wy = pose.y + r * std::sin(a + pose.yaw);
+
+        // Skip wall points
+        double dw = std::min({
+            fence.left.distance(wx, wy), fence.right.distance(wx, wy),
+            fence.bottom.distance(wx, wy), fence.top.distance(wx, wy)
+        });
+        if (dw < WALL_DIST) continue;
+
+        // Skip near known cones
+        bool near_cone = false;
+        for (auto& c : known_cones) {
+            double dx = wx - c.x(), dy = wy - c.y();
+            if (dx*dx + dy*dy < MERGE_DIST * MERGE_DIST) { near_cone = true; break; }
+        }
+        if (near_cone) continue;
+
+        pts.emplace_back(wx, wy);
+    }
+    if (pts.size() < 10) return;
+
+    // ── 2. Simple grid clustering ──
+    std::vector<std::vector<Eigen::Vector2d>> clusters;
+    std::vector<bool> used(pts.size(), false);
+    for (size_t i = 0; i < pts.size(); ++i) {
+        if (used[i]) continue;
+        std::vector<Eigen::Vector2d> cl;
+        cl.push_back(pts[i]); used[i] = true;
+        for (size_t j = 0; j < cl.size(); ++j) {
+            for (size_t k = i+1; k < pts.size(); ++k) {
+                if (used[k]) continue;
+                double dx = pts[k].x() - cl[j].x();
+                double dy = pts[k].y() - cl[j].y();
+                if (dx*dx + dy*dy < CLUSTER_DIST * CLUSTER_DIST) {
+                    cl.push_back(pts[k]); used[k] = true;
+                }
+            }
+        }
+        if (cl.size() >= MIN_POINTS) clusters.push_back(cl);
+    }
+
+    // ── 3. Match clusters to candidates ──
+    for (auto& cl : clusters) {
+        Eigen::Vector2d center(0, 0);
+        for (auto& p : cl) { center.x() += p.x(); center.y() += p.y(); }
+        center /= (double)cl.size();
+
+        // Find nearest existing candidate
+        Candidate* best = nullptr;
+        double best_d2 = 1e9;
+        for (auto& c : candidates_) {
+            double dx = center.x() - c.center.x();
+            double dy = center.y() - c.center.y();
+            double d2 = dx*dx + dy*dy;
+            if (d2 < MERGE_DIST * MERGE_DIST && d2 < best_d2) {
+                best_d2 = d2; best = &c;
+            }
+        }
+
+        if (best) {
+            // Update existing candidate
+            best->center = (best->center + center) / 2.0;
+            best->hits++;
+            best->age = 0;
+        } else {
+            // New candidate
+            candidates_.push_back({center, 1, 0});
+        }
+    }
+
+    // ── 4. Age all candidates, remove stale ──
+    for (auto& c : candidates_) c.age++;
+    candidates_.erase(
+        std::remove_if(candidates_.begin(), candidates_.end(),
+                       [](const Candidate& c) { return c.age > 10 && c.hits < CONFIRM_HITS; }),
+        candidates_.end());
+}
+
+std::vector<Eigen::Vector2d> ConeUpdater::popConfirmed() {
+    std::vector<Eigen::Vector2d> confirmed;
+    for (auto it = candidates_.begin(); it != candidates_.end(); ) {
+        if (it->hits >= CONFIRM_HITS) {
+            confirmed.push_back(it->center);
+            it = candidates_.erase(it);
+        } else {
+            ++it;
+        }
+    }
+    return confirmed;
+}
+
+}  // namespace car_nav_lite

src/cone_updater.hpp

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <vector>
+#include <Eigen/Dense>
+#include "car_nav_lite/types.hpp"
+#include "car_nav_lite/fence_model.hpp"
+
+namespace car_nav_lite {
+
+class ConeUpdater {
+public:
+    ConeUpdater();
+
+    // Process one scan frame: extract cone candidates, track across frames.
+    // confirmed_cones is the current known cone list (for dedup).
+    void update(const LaserScan& scan, const Pose2D& pose,
+                const FenceModel& fence,
+                const std::vector<Eigen::Vector2d>& known_cones);
+
+    // Returns newly confirmed cones and clears them from pending.
+    std::vector<Eigen::Vector2d> popConfirmed();
+
+private:
+    struct Candidate {
+        Eigen::Vector2d center;
+        int hits = 0;
+        int age = 0;
+    };
+    std::vector<Candidate> candidates_;
+
+    static constexpr int CONFIRM_HITS = 3;
+    static constexpr double CLUSTER_DIST = 0.10;
+    static constexpr double MERGE_DIST = 0.25;
+    static constexpr double WALL_DIST = 0.15;
+    static constexpr int MIN_POINTS = 4;
+};
+
+}  // namespace car_nav_lite

src/main.cpp

@@ -21,6 +21,7 @@
 #include "car_nav_lite/bt_executor.hpp"
 #include "car_nav_lite/fence_model.hpp"
 #include "car_nav_lite/session_mapper.hpp"
+#include "car_nav_lite/cone_updater.hpp"
 
 using namespace std::chrono_literals;
 using namespace car_nav_lite;
@@ -46,6 +47,7 @@ public:
         declare_parameter("wall_ransac_thresh", 0.04);
         declare_parameter("cone_mark_radius", 0.18);
         declare_parameter("dynamic_obstacle_enable", false);
+        declare_parameter("online_cone_enable", true);
         _read_params();
 
         param_cb_ = add_on_set_parameters_callback(
@@ -64,6 +66,7 @@ public:
                     else if (n=="wall_ransac_thresh") wall_ransac_thresh_=p.as_double();
                     else if (n=="cone_mark_radius") cone_mark_radius_=p.as_double();
                     else if (n=="dynamic_obstacle_enable") dynamic_obstacle_enable_=p.as_bool();
+                    else if (n=="online_cone_enable") online_cone_enable_=p.as_bool();
                 }
                 RCLCPP_INFO(get_logger(),"params: la=%.2f ms=%.1f mr=%.2f sl=%.2f cf=%.1f test=%d",
                             la_,ms_,mr_,sl_,cf_,test_);
@@ -116,7 +119,7 @@ private:
     int calib_scan_count_=30;
     double fence_expected_size_=5.0, fence_size_tolerance_=0.35;
     double wall_ransac_thresh_=0.04, cone_mark_radius_=0.18;
-    bool dynamic_obstacle_enable_=false;
+    bool dynamic_obstacle_enable_=false, online_cone_enable_=true;
     rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;
     void _read_params(){
         mf_=get_parameter("map_file").as_string();
@@ -132,6 +135,7 @@ private:
         wall_ransac_thresh_=get_parameter("wall_ransac_thresh").as_double();
         cone_mark_radius_=get_parameter("cone_mark_radius").as_double();
         dynamic_obstacle_enable_=get_parameter("dynamic_obstacle_enable").as_bool();
+        online_cone_enable_=get_parameter("online_cone_enable").as_bool();
     }
 
     // ── State ──
@@ -144,6 +148,8 @@ private:
     GridMap map_; Localizer localizer_; BTExecutor bt_;
     SessionMapper session_mapper_;
     FenceModel fence_model_;
+    ConeUpdater cone_updater_;
+    std::vector<Eigen::Vector2d> known_cones_;
     enum SessionState { CALIBRATING, READY };
     SessionState session_state_ = CALIBRATING;
     int calib_cnt_ = 0;
@@ -172,6 +178,7 @@ private:
                 std::vector<Eigen::Vector2d> cones;
                 if (session_mapper_.build(fence_model_, cones, fence_expected_size_, fence_size_tolerance_, wall_ransac_thresh_)) {
                     localizer_.setFenceModel(fence_model_);
+                    known_cones_ = cones;
                     for (auto& c : cones) map_.markCone(c.x(), c.y(), cone_mark_radius_, 255);
                     map_.markFenceModel(fence_model_);
                     map_.morphologyClose(3);
@@ -254,9 +261,22 @@ private:
             pub_cmd_->publish(geometry_msgs::msg::Twist());
             return;
         }
-        // READY: only update scan if dynamic obstacles enabled
-        if(has_scan_ && dynamic_obstacle_enable_)
-            map_.updateScan(latest_scan_,pose);
+        // READY: online cone discovery + optional dynamic obstacles
+        if (has_scan_) {
+            if (online_cone_enable_) {
+                cone_updater_.update(latest_scan_, pose, fence_model_, known_cones_);
+                auto new_cones = cone_updater_.popConfirmed();
+                for (auto& c : new_cones) {
+                    known_cones_.push_back(c);
+                    map_.markCone(c.x(), c.y(), cone_mark_radius_, 255);
+                    fprintf(stderr, "[Session] new cone: (%.2f,%.2f) total=%zu\n",
+                            c.x(), c.y(), known_cones_.size());
+                }
+                if (!new_cones.empty()) map_.composeLayers();
+            }
+            if (dynamic_obstacle_enable_)
+                map_.updateScan(latest_scan_, pose);
+        }
 
         // Test mode: auto-activate figure-8 after 3s
         if(test_&&!tps_){