From 7c07eb66c8235314ea171f3634b54cc6a75c44f4 Mon Sep 17 00:00:00 2001 From: cyy Date: Fri, 3 Jul 2026 07:04:52 +0000 Subject: [PATCH] Steps 3-7: layered GridMap, confidence slowdown, frozen map, ROS2 params --- include/car_nav_lite/grid_map.hpp | 11 +++++- include/car_nav_lite/localizer.hpp | 1 + src/grid_map.cpp | 60 ++++++++++++++++++++++++++++-- src/localizer.cpp | 4 +- src/main.cpp | 48 ++++++++++++++++++------ 5 files changed, 107 insertions(+), 17 deletions(-) diff --git a/include/car_nav_lite/grid_map.hpp b/include/car_nav_lite/grid_map.hpp index 8682c82..4c91aa5 100644 --- a/include/car_nav_lite/grid_map.hpp +++ b/include/car_nav_lite/grid_map.hpp @@ -47,8 +47,13 @@ public: // ── Reset dynamic obstacles to static PGM layer ── void resetDynamic(); + // ── Layered map (fence + cone + dynamic) ── + void freezeSessionMap(); + void composeLayers(); + // ── Pre-built obstacle marks ─────────────────────── void markCircle(double wx, double wy, double radius, uint8_t value = 255); + void markCone(double wx, double wy, double radius, uint8_t value = 255); void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255); // ── Getters ──────────────────────────────────────── @@ -58,8 +63,12 @@ public: private: OccupancyGrid cells_; - OccupancyGrid static_cells_; // snapshot after PGM load + OccupancyGrid static_cells_; + OccupancyGrid cone_cells_; // fixed cones from session + OccupancyGrid dynamic_cells_; // optional real-time obstacles + bool frozen_ = false; void bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val); + void bresenhamDynamic(int gx0, int gy0, int gx1, int gy1, uint8_t v); }; } // namespace car_nav_lite diff --git a/include/car_nav_lite/localizer.hpp b/include/car_nav_lite/localizer.hpp index 278db09..40bb703 100644 --- a/include/car_nav_lite/localizer.hpp +++ b/include/car_nav_lite/localizer.hpp @@ -24,6 +24,7 @@ public: // Metrics int loc_lines_ = 0; double loc_dx_ = 0, loc_dy_ = 0, loc_dyaw_ = 0; + double loc_conf_ = 1.0; private: Pose2D odom_pose_; diff --git a/src/grid_map.cpp b/src/grid_map.cpp index 71b46a7..d2e5eec 100644 --- a/src/grid_map.cpp +++ b/src/grid_map.cpp @@ -7,7 +7,10 @@ namespace car_nav_lite { -GridMap::GridMap() : cells_(GRID_SIZE * GRID_SIZE, 0) {} +GridMap::GridMap() : cells_(GRID_SIZE * GRID_SIZE, 0), + static_cells_(GRID_SIZE * GRID_SIZE, 0), + cone_cells_(GRID_SIZE * GRID_SIZE, 0), + dynamic_cells_(GRID_SIZE * GRID_SIZE, 0) {} bool GridMap::loadPGM(const std::string& path) { std::ifstream file(path, std::ios::binary); @@ -55,6 +58,24 @@ bool GridMap::loadPGM(const std::string& path) { void GridMap::resetDynamic() { cells_ = static_cells_; } +void GridMap::composeLayers() { + for (size_t i = 0; i < cells_.size(); ++i) { + uint8_t v = static_cells_[i]; + if (cone_cells_[i] > v) v = cone_cells_[i]; + if (dynamic_cells_[i] > v) v = dynamic_cells_[i]; + cells_[i] = v; + } +} + +void GridMap::freezeSessionMap() { + frozen_ = true; + // Merge cone layer into static for persistence + for (size_t i = 0; i < cells_.size(); ++i) + if (cone_cells_[i] > static_cells_[i]) + static_cells_[i] = cone_cells_[i]; + composeLayers(); +} + bool GridMap::worldToGrid(double wx, double wy, int& gx, int& gy) const { gx = (int)((wx - GRID_ORIGIN_X) / GRID_RES); gy = (int)((wy - GRID_ORIGIN_Y) / GRID_RES); @@ -142,6 +163,7 @@ void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) { int ox, oy; if (!worldToGrid(pose.x, pose.y, ox, oy)) return; + auto& target = frozen_ ? dynamic_cells_ : cells_; for (int i = 0; i < scan.size(); ++i) { double r = scan.ranges[i]; if (r < scan.range_min || r > scan.range_max) continue; @@ -151,7 +173,8 @@ void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) { double hwx = pose.x + r * std::cos(angle); double hwy = pose.y + r * std::sin(angle); if (worldToGrid(hwx, hwy, hx, hy)) { - set(hx, hy, 255); // hit → fully occupied immediately + if (frozen_) dynamic_cells_[hy * GRID_SIZE + hx] = 255; + else set(hx, hy, 255); } // Raytrace clear @@ -160,9 +183,14 @@ void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) { double cwx = pose.x + clear_r * std::cos(angle); double cwy = pose.y + clear_r * std::sin(angle); if (worldToGrid(cwx, cwy, cx, cy)) { - bresenhamLine(ox, oy, cx, cy, 5); // clear → -occupied + if (frozen_) { + bresenhamDynamic(ox, oy, cx, cy, 5); + } else { + bresenhamLine(ox, oy, cx, cy, 5); + } } } + if (frozen_) composeLayers(); } void GridMap::bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val) { @@ -182,6 +210,22 @@ void GridMap::bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_va } } +void GridMap::bresenhamDynamic(int gx0, int gy0, int gx1, int gy1, uint8_t v) { + int dx = std::abs(gx1 - gx0), dy = -std::abs(gy1 - gy0); + int sx = gx0 < gx1 ? 1 : -1, sy = gy0 < gy1 ? 1 : -1; + int err = dx + dy, x = gx0, y = gy0; + while (true) { + if (inBounds(x, y)) { + uint8_t& c = dynamic_cells_[y * GRID_SIZE + x]; + if (c >= v) c -= v; + } + if (x == gx1 && y == gy1) break; + int e2 = 2 * err; + if (e2 >= dy) { err += dy; x += sx; } + if (e2 <= dx) { err += dx; y += sy; } + } +} + void GridMap::markCircle(double wx, double wy, double radius, uint8_t value) { int cx, cy, cr; if (!worldToGrid(wx, wy, cx, cy)) return; @@ -195,6 +239,16 @@ void GridMap::markCircle(double wx, double wy, double radius, uint8_t value) { } } +void GridMap::markCone(double wx, double wy, double radius, uint8_t value) { + int cx, cy, cr; + if (!worldToGrid(wx, wy, cx, cy)) return; + cr = (int)(radius / GRID_RES); + for (int dy = -cr; dy <= cr; ++dy) + for (int dx = -cr; dx <= cr; ++dx) + if (dx*dx + dy*dy <= cr*cr && inBounds(cx+dx, cy+dy)) + cone_cells_[(cy+dy) * GRID_SIZE + (cx+dx)] = value; +} + double GridMap::raycast(double wx, double wy, double angle, double max_range) const { // DDA (Digital Differential Analyzer) raycast on static occupancy grid. // Steps along grid cells from (wx,wy) in direction angle until hitting diff --git a/src/localizer.cpp b/src/localizer.cpp index 799e569..d800100 100644 --- a/src/localizer.cpp +++ b/src/localizer.cpp @@ -131,8 +131,10 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map, } // ── 5. Update offset ── - // Store metrics + // Store metrics + confidence loc_lines_ = line_count; + loc_conf_ = (line_count >= 3) ? 0.9 : (line_count >= 2 ? 0.6 : 0.3); + if (std::abs(dyaw) > 0.15) loc_conf_ *= 0.5; if (line_count >= 2) { double dx = sum_dx / line_count, dy = sum_dy / line_count; loc_dx_ = dx; loc_dy_ = dy; loc_dyaw_ = -dyaw; diff --git a/src/main.cpp b/src/main.cpp index be1bf91..0f9367c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -40,6 +40,12 @@ public: declare_parameter("min_turn_radius", 0.20); declare_parameter("steer_limit", 0.6); declare_parameter("speed_curv_factor", 6.0); + declare_parameter("calib_scan_count", 30); + declare_parameter("fence_expected_size", 5.0); + declare_parameter("fence_size_tolerance", 0.35); + declare_parameter("wall_ransac_thresh", 0.04); + declare_parameter("cone_mark_radius", 0.18); + declare_parameter("dynamic_obstacle_enable", false); _read_params(); param_cb_ = add_on_set_parameters_callback( @@ -101,6 +107,10 @@ private: std::string mf_; bool test_=false, tps_=false; double la_=0.5, ms_=1.0, mr_=0.20, sl_=0.6, cf_=6.0; + 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; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_; void _read_params(){ mf_=get_parameter("map_file").as_string(); @@ -110,6 +120,12 @@ private: mr_=get_parameter("min_turn_radius").as_double(); sl_=get_parameter("steer_limit").as_double(); cf_=get_parameter("speed_curv_factor").as_double(); + calib_scan_count_=get_parameter("calib_scan_count").as_int(); + fence_expected_size_=get_parameter("fence_expected_size").as_double(); + fence_size_tolerance_=get_parameter("fence_size_tolerance").as_double(); + 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(); } // ── State ── @@ -124,8 +140,7 @@ private: FenceModel fence_model_; enum SessionState { CALIBRATING, READY }; SessionState session_state_ = CALIBRATING; - int calib_scan_count_ = 0; - static constexpr int CALIB_SCANS_NEEDED = 30; + int calib_cnt_ = 0; rclcpp::Subscription::SharedPtr sub_scan_; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_imu_; @@ -145,19 +160,20 @@ private: latest_scan_.ranges=m->ranges;has_scan_=true; scan_pose_snapshot_ = localizer_.pose(); - if (session_state_ == CALIBRATING && calib_scan_count_ < CALIB_SCANS_NEEDED) { + if (session_state_ == CALIBRATING && calib_cnt_ < calib_scan_count_) { session_mapper_.addScan(latest_scan_, localizer_.pose()); - if (++calib_scan_count_ >= CALIB_SCANS_NEEDED) { + if (++calib_cnt_ >= calib_scan_count_) { std::vector cones; - if (session_mapper_.build(fence_model_, cones, 5.0, 0.35, 0.04)) { + if (session_mapper_.build(fence_model_, cones, fence_expected_size_, fence_size_tolerance_, wall_ransac_thresh_)) { localizer_.setFenceModel(fence_model_); - for (auto& c : cones) - map_.markCircle(c.x(), c.y(), 0.18, 255); + for (auto& c : cones) map_.markCone(c.x(), c.y(), cone_mark_radius_, 255); + map_.morphologyClose(3); + map_.freezeSessionMap(); session_state_ = READY; - fprintf(stderr, "[Session] READY — fence %.2fx%.2f, %zu cones\n", - fence_model_.width, fence_model_.height, cones.size()); + fprintf(stderr, "[Session] READY — fence %.2fx%.2f yaw=%.1f°, %zu cones\n", + fence_model_.width, fence_model_.height, fence_model_.yaw_field*57.3, cones.size()); } else { - calib_scan_count_ = 0; // retry + calib_cnt_ = 0; fprintf(stderr, "[Session] calibration failed, retrying...\n"); } } @@ -226,7 +242,9 @@ private: void tick(){ std::lock_guard lk(mtx_); auto pose=localizer_.pose();publish_tf(pose); - if(has_scan_)map_.updateScan(latest_scan_,pose); + // READY: only update scan if dynamic obstacles enabled + if(has_scan_ && (session_state_==CALIBRATING || dynamic_obstacle_enable_)) + map_.updateScan(latest_scan_,pose); // Test mode: auto-activate figure-8 after 3s if(test_&&!tps_){ @@ -242,6 +260,9 @@ private: bt_.setParams(la_,ms_,mr_,sl_,cf_); Twist cmd=bt_.tick(pose,map_); + // Slow down if localization confidence is low + if (localizer_.loc_conf_ < 0.3) { cmd.vx *= 0.4; cmd.wz *= 0.4; } + auto tw=geometry_msgs::msg::Twist();tw.linear.x=cmd.vx;tw.angular.z=cmd.wz; pub_cmd_->publish(tw); if(!bt_.currentPath().empty()){ @@ -289,7 +310,10 @@ private: pub_metrics_->publish(msg); } - void morph_close(){std::lock_guard lk(mtx_); map_.morphologyClose(3);} + void morph_close(){std::lock_guard lk(mtx_); + if (session_state_ == READY) return; // frozen map, no morph needed + map_.morphologyClose(3); + } void publish_tf(const Pose2D& pose){ geometry_msgs::msg::TransformStamped tf;