From 85d6984bba6d7f85b0c5a84d67245c42997c05d0 Mon Sep 17 00:00:00 2001 From: cyy Date: Fri, 3 Jul 2026 06:14:36 +0000 Subject: [PATCH] update --- include/car_nav_lite/grid_map.hpp | 4 +++ scripts/waypoint_nav.py | 8 +----- src/bt_executor.cpp | 41 ++++++++++++++++++++++++++++++- src/grid_map.cpp | 7 +++++- src/grid_map.hpp | 4 +++ src/local_planner.cpp | 18 +++----------- src/main.cpp | 12 +++------ 7 files changed, 62 insertions(+), 32 deletions(-) diff --git a/include/car_nav_lite/grid_map.hpp b/include/car_nav_lite/grid_map.hpp index 7323e9a..8682c82 100644 --- a/include/car_nav_lite/grid_map.hpp +++ b/include/car_nav_lite/grid_map.hpp @@ -44,6 +44,9 @@ public: // ── Morphological close (remove noise, merge nearby blobs) ── void morphologyClose(int kernel_size = 3); + // ── Reset dynamic obstacles to static PGM layer ── + void resetDynamic(); + // ── Pre-built obstacle marks ─────────────────────── void markCircle(double wx, double wy, double radius, uint8_t value = 255); void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255); @@ -55,6 +58,7 @@ public: private: OccupancyGrid cells_; + OccupancyGrid static_cells_; // snapshot after PGM load void bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val); }; diff --git a/scripts/waypoint_nav.py b/scripts/waypoint_nav.py index 6e4bf71..0348f67 100755 --- a/scripts/waypoint_nav.py +++ b/scripts/waypoint_nav.py @@ -45,7 +45,6 @@ class WaypointNav(Node): self.reached = False self.fail_gen = 0 self.skip_gen = -1 - self.skip_streak = 0 # consecutive skips self.pose_x = self.pose_y = self.pose_yaw = 0.0 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy @@ -66,11 +65,7 @@ class WaypointNav(Node): def status_cb(self, msg): if msg.data == "fail" and self.reached and self.fail_gen != self.skip_gen: - self.skip_streak += 1 - if self.skip_streak > 3: - self.get_logger().error("⏭ 3+ consecutive skips — stopping") - return - self.get_logger().warn(f"⏭ WP {self.idx+1} plan failed — skip #{self.skip_streak}") + self.get_logger().warn(f"⏭ WP {self.idx+1} plan failed — skip") self.skip_gen = self.fail_gen self.idx += 1 self.reached = False @@ -112,7 +107,6 @@ class WaypointNav(Node): f"✓ WP {self.idx+1} reached (dist={dist:.2f}m)") self.idx += 1 self.reached = False - self.skip_streak = 0 def main(): diff --git a/src/bt_executor.cpp b/src/bt_executor.cpp index 0df4d5d..29e2704 100644 --- a/src/bt_executor.cpp +++ b/src/bt_executor.cpp @@ -30,6 +30,20 @@ Twist BTExecutor::tick(const Pose2D& pose, const GridMap& map) { case NavState::PLANNING: if (goal_) { current_path_ = planner_.plan(pose, *goal_, map); + if (current_path_.empty()) { + int ggx, ggy; map.worldToGrid(goal_->x, goal_->y, ggx, ggy); + constexpr int R = 40; + for (int r = 4; r <= R && current_path_.empty(); r += 4) + for (int dx = -r; dx <= r; dx += 4) + for (int dy = -r; dy <= r; dy += 4) { + if (dx*dx+dy*dy > r*r) continue; + int nx=ggx+dx, ny=ggy+dy; + if (!map.inBounds(nx,ny)||!map.isFree(nx,ny)) continue; + double wx,wy; map.gridToWorld(nx,ny,wx,wy); + current_path_ = planner_.plan(pose, {wx,wy,goal_->yaw}, map, 0); + if (!current_path_.empty()) break; + } + } if (!current_path_.empty()) { current_path_.back().yaw = goal_->yaw; fprintf(stderr, "[BT] Plan OK, %zu waypoints\n", current_path_.size()); @@ -63,6 +77,31 @@ Twist BTExecutor::tick(const Pose2D& pose, const GridMap& map) { new_path = planner_.plan(pose, *goal_, map, inflate); if (!new_path.empty()) break; } + // If all inflations fail, search 20cm around goal + if (new_path.empty()) { + int ggx, ggy; + map.worldToGrid(goal_->x, goal_->y, ggx, ggy); + constexpr int R = 40; // 20cm at 5mm + for (int r = 4; r <= R && new_path.empty(); r += 4) { + for (int dx = -r; dx <= r; dx += 4) { + for (int dy = -r; dy <= r; dy += 4) { + if (dx*dx + dy*dy > r*r) continue; + int nx = ggx + dx, ny = ggy + dy; + if (!map.inBounds(nx, ny)) continue; + if (!map.isFree(nx, ny)) continue; + double wx, wy; + map.gridToWorld(nx, ny, wx, wy); + Pose2D alt_goal(wx, wy, goal_->yaw); + new_path = planner_.plan(pose, alt_goal, map, 0); + if (!new_path.empty()) { + fprintf(stderr, "[BT] goal shifted (%.2f,%.2f)→(%.2f,%.2f)\n", + goal_->x, goal_->y, wx, wy); + break; + } + } + } + } + } if (!new_path.empty()) current_path_ = std::move(new_path); } @@ -80,7 +119,7 @@ Twist BTExecutor::tick(const Pose2D& pose, const GridMap& map) { bool BTExecutor::atGoal(const Pose2D& pose) const { if (!goal_) return false; - if (pose.distTo(*goal_) > 0.20) return false; + if (pose.distTo(*goal_) > 0.30) return false; return true; } diff --git a/src/grid_map.cpp b/src/grid_map.cpp index 01ed8f9..71b46a7 100644 --- a/src/grid_map.cpp +++ b/src/grid_map.cpp @@ -49,9 +49,12 @@ bool GridMap::loadPGM(const std::string& path) { cells_[gy * GRID_SIZE + gx] = (pgm < 128) ? 255 : 0; } } + static_cells_ = cells_; return true; } +void GridMap::resetDynamic() { cells_ = static_cells_; } + 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); @@ -240,7 +243,9 @@ void GridMap::morphologyClose(int kernel_size) { cv::Mat img(GRID_SIZE, GRID_SIZE, CV_8UC1, cells_.data()); cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(kernel_size, kernel_size)); - cv::morphologyEx(img, img, cv::MORPH_CLOSE, kernel); + cv::Mat result; + cv::morphologyEx(img, result, cv::MORPH_CLOSE, kernel); + result.copyTo(img); // explicit write-back } void GridMap::markRect(double x1, double y1, double x2, double y2, uint8_t value) { diff --git a/src/grid_map.hpp b/src/grid_map.hpp index 7323e9a..8682c82 100644 --- a/src/grid_map.hpp +++ b/src/grid_map.hpp @@ -44,6 +44,9 @@ public: // ── Morphological close (remove noise, merge nearby blobs) ── void morphologyClose(int kernel_size = 3); + // ── Reset dynamic obstacles to static PGM layer ── + void resetDynamic(); + // ── Pre-built obstacle marks ─────────────────────── void markCircle(double wx, double wy, double radius, uint8_t value = 255); void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255); @@ -55,6 +58,7 @@ public: private: OccupancyGrid cells_; + OccupancyGrid static_cells_; // snapshot after PGM load void bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val); }; diff --git a/src/local_planner.cpp b/src/local_planner.cpp index 6657b39..9e74801 100644 --- a/src/local_planner.cpp +++ b/src/local_planner.cpp @@ -56,13 +56,7 @@ double LocalPlanner::evaluateTrajectory( sx += vx * std::cos(syaw) * DT; sy += vx * std::sin(syaw) * DT; - // ── Obstacle: check with forward margin (car moves DT*vx per step, - // at 1m/s that's 10cm — cone might be between steps) ── - // Project forward by half a step for safety - double fwd_x = sx + vx * DT * 0.5 * std::cos(syaw); - double fwd_y = sy + vx * DT * 0.5 * std::sin(syaw); - if (!map.isFreeFootprint(sx, sy, syaw) || - !map.isFreeFootprint(fwd_x, fwd_y, syaw)) { + if (!map.isFreeFootprint(sx, sy, syaw)) { cost += 500.0; return cost; } @@ -83,10 +77,6 @@ double LocalPlanner::evaluateTrajectory( double end_to_goal = std::hypot(goal.x - sx, goal.y - sy); cost += W_FOLLOW * end_to_goal; - // ── Goal: only near goal ── - if (total_path_len < 1.4) - cost += W_GOAL * end_to_goal; - // ── Reverse penalty (mild) ── if (vx < 0) cost += std::abs(vx) * 3.0; @@ -129,9 +119,8 @@ Twist LocalPlanner::compute(const Pose2D& pose, wz = ((double)(x & 0xFFFF) / 65535.0 - 0.5) * 3.0; } - // Clamp + Ackermann + // Clamp + Ackermann (no hard wz limit — Ackermann constraint is enough) vx = limit(vx, -0.5, ms_); - wz = limit(wz, -1.5, 1.5); double max_wz = std::abs(vx) / mr_; wz = limit(wz, -max_wz, max_wz); @@ -165,8 +154,7 @@ Twist LocalPlanner::compute(const Pose2D& pose, // Enforce minimum reverse speed if (cmd.vx < 0 && cmd.vx > -0.3) cmd.vx = -0.3; - double max_wz = std::abs(cmd.vx) / mr_; - cmd.wz = limit(cmd.wz, -max_wz, max_wz); + cmd.wz = limit(cmd.wz, -std::abs(cmd.vx) / mr_, std::abs(cmd.vx) / mr_); prev_vx_ = cmd.vx; prev_wz_ = cmd.wz; diff --git a/src/main.cpp b/src/main.cpp index 0328442..d18b74c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -133,7 +133,9 @@ private: latest_scan_.angle_increment=m->angle_increment; latest_scan_.range_min=m->range_min;latest_scan_.range_max=m->range_max; latest_scan_.ranges=m->ranges;has_scan_=true; - scan_pose_snapshot_ = localizer_.pose();} + scan_pose_snapshot_ = localizer_.pose(); + // Trigger correction immediately (eliminate timer delay) + localizer_.correctWithScanCV(latest_scan_, map_, &scan_pose_snapshot_);} void odom_cb(OdometryMsg::SharedPtr m){std::lock_guard lk(mtx_); // EXACT-MPPI pattern: trust odom pose directly (sim = ground truth, // real robot = wheel-encoder / EKF output). No manual integration. @@ -236,13 +238,7 @@ private: for(size_t i=0;i128)?100:-1);} pub_map_->publish(m);} - void correct_localization(){ - if(!has_scan_)return; - auto pose_before = localizer_.pose(); - localizer_.correctWithScanCV(latest_scan_, map_, &scan_pose_snapshot_); - auto pose_after = localizer_.pose(); - (void)pose_before; (void)pose_after; - } + void correct_localization(){} // now triggered from scan_cb directly void morph_close(){std::lock_guard lk(mtx_); map_.morphologyClose(3);}