diff --git a/CMakeLists.txt b/CMakeLists.txt index 93c9b89..144ae3d 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/car_nav_lite/cone_updater.hpp b/include/car_nav_lite/cone_updater.hpp new file mode 100644 index 0000000..c414e85 --- /dev/null +++ b/include/car_nav_lite/cone_updater.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#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& known_cones); + + // Returns newly confirmed cones and clears them from pending. + std::vector popConfirmed(); + +private: + struct Candidate { + Eigen::Vector2d center; + int hits = 0; + int age = 0; + }; + std::vector 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 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..144ae3d --- /dev/null +++ b/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() diff --git a/src/cone_updater.cpp b/src/cone_updater.cpp new file mode 100644 index 0000000..18ac840 --- /dev/null +++ b/src/cone_updater.cpp @@ -0,0 +1,114 @@ +#include "car_nav_lite/cone_updater.hpp" +#include +#include +#include + +namespace car_nav_lite { + +ConeUpdater::ConeUpdater() {} + +void ConeUpdater::update(const LaserScan& scan, const Pose2D& pose, + const FenceModel& fence, + const std::vector& known_cones) { + if (!fence.valid) return; + + // ── 1. Extract non-wall, non-known-cone world points ── + std::vector 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> clusters; + std::vector used(pts.size(), false); + for (size_t i = 0; i < pts.size(); ++i) { + if (used[i]) continue; + std::vector 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 ConeUpdater::popConfirmed() { + std::vector 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 diff --git a/src/cone_updater.hpp b/src/cone_updater.hpp new file mode 100644 index 0000000..c414e85 --- /dev/null +++ b/src/cone_updater.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#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& known_cones); + + // Returns newly confirmed cones and clears them from pending. + std::vector popConfirmed(); + +private: + struct Candidate { + Eigen::Vector2d center; + int hits = 0; + int age = 0; + }; + std::vector 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 diff --git a/src/main.cpp b/src/main.cpp index 47c1067..60008cc 100644 --- a/src/main.cpp +++ b/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 known_cones_; enum SessionState { CALIBRATING, READY }; SessionState session_state_ = CALIBRATING; int calib_cnt_ = 0; @@ -172,6 +178,7 @@ private: std::vector 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_){