cyy/car_nav_lite
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
cyy
2026-07-03 08:07:17 +00:00
parent 4ad21bba6a
commit 14809af5f6
4 changed files with 177 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -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

38
include/car_nav_lite/cone_updater.hpp Normal file
View File

@@ -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

114
src/cone_updater.cpp Normal file
View File

@@ -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

28
src/main.cpp
View File

@@ -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_){