Steps 3-7: layered GridMap, confidence slowdown, frozen map, ROS2 params

2026-07-03 07:04:52 +00:00
parent 6085ff7383
commit 7c07eb66c8
5 changed files with 107 additions and 17 deletions
--- 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
--- 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_;
--- 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,10 +183,15 @@ 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) {
    int dx = std::abs(gx1 - gx0), dy = -std::abs(gy1 - gy0);
@@ -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
--- 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;
--- 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<LaserScanMsg>::SharedPtr sub_scan_;
    rclcpp::Subscription<OdometryMsg>::SharedPtr  sub_odom_;
    rclcpp::Subscription<ImuMsg>::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<Eigen::Vector2d> 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;