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

update

Browse Source
This commit is contained in:
cyy
2026-07-03 06:14:36 +00:00
parent 50ff61a16b
commit 85d6984bba
7 changed files with 62 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/car_nav_lite/grid_map.hpp
View File

@@ -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);
};

8
scripts/waypoint_nav.py
View File

@@ -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():

41
src/bt_executor.cpp
View File

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

7
src/grid_map.cpp
View File

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

4
src/grid_map.hpp
View File

@@ -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);
};

18
src/local_planner.cpp
View File

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

12
src/main.cpp
View File

@@ -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;i<map_.data().size();++i){uint8_t v=map_.data()[i];m.data[i]=(v==0)?0:((v>128)?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);}