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

MPPI + fence-line loc + waypoint nav with fail skip

Browse Source
This commit is contained in:
cyy
2026-07-03 00:17:08 +00:00
parent 5156a8d766
commit 50ff61a16b
14 changed files with 555 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
include/car_nav_lite/bt_executor.hpp
View File

@@ -1,5 +1,6 @@
#pragma once
#include <functional>
#include <optional>
#include "car_nav_lite/types.hpp"
#include "car_nav_lite/grid_map.hpp"
@@ -16,6 +17,7 @@ public:
void setGoal(const Pose2D& goal);
void setTestPath(const std::vector<Pose2D>& path);
void setParams(double la, double ms, double mr, double sl, double cf);
void setFailCallback(std::function<void()> cb) { fail_callback_ = std::move(cb); }
bool hasGoal() const { return goal_.has_value(); }
Twist tick(const Pose2D& pose, const GridMap& map);
@@ -29,6 +31,7 @@ private:
std::vector<Pose2D> current_path_;
GlobalPlanner planner_;
LocalPlanner local_planner_;
std::function<void()> fail_callback_;
bool atGoal(const Pose2D& pose) const;
};

3
include/car_nav_lite/grid_map.hpp
View File

@@ -41,6 +41,9 @@ public:
// ── Obstacle update from scan ──────────────────────
void updateScan(const LaserScan& scan, const Pose2D& pose);
// ── Morphological close (remove noise, merge nearby blobs) ──
void morphologyClose(int kernel_size = 3);
// ── 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);

10
include/car_nav_lite/localizer.hpp
View File

@@ -10,19 +10,13 @@ class Localizer {
public:
Localizer();
// ── Odometry update (called at odom rate) ──────────
void updateOdom(double vx, double wz, double dt);
void updateIMU(double gyro_z, double dt);
// Set raw odom pose. pose() returns odom + correction offset.
void setOdomPose(const Pose2D& p);
// ── Scan-to-map correction (updates persistent offset) ──
// Correction = matched_pose - odom_pose, stored as offset.
// Only updates if confidence is high; otherwise offset decays to zero.
void correctWithScanCV(const LaserScan& scan, const GridMap& map);
void correctWithScanCV(const LaserScan& scan, const GridMap& map,
const Pose2D* base_override = nullptr);
// ── Final pose = odom + correction offset ──────────
Pose2D pose() const;
private:

148
scripts/waypoint_nav.py Executable file
View File

@@ -0,0 +1,148 @@
#!/usr/bin/env python3
"""Sequential waypoint navigation for car_nav_lite.
Publishes /goal_pose waypoints one by one. Waits for the car to reach
each waypoint before publishing the next.
Usage:
python3 waypoint_nav.py # default waypoints
python3 waypoint_nav.py --waypoints waypoints.json # from JSON file
python3 waypoint_nav.py --loop # loop back to start
JSON format:
[{"x": 1.0, "y": -1.5, "yaw": 0.0}, ...]
"""
import argparse
import json
import math
import time
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
from std_msgs.msg import String
# Default waypoints for 5m×5m arena (car spawns near -2.0, -2.3)
DEFAULT_WAYPOINTS = [
{"x": -0.5, "y": -1.5, "yaw": 0.0, "desc": "mid-left"},
{"x": 1.0, "y": -1.5, "yaw": 0.0, "desc": "right"},
{"x": 1.5, "y": 0.0, "yaw": 0.0, "desc": "top-right"},
{"x": 0.0, "y": 1.5, "yaw": 0.0, "desc": "top-center"},
{"x": -1.5, "y": 1.0, "yaw": 0.0, "desc": "top-left"},
{"x": -1.5, "y": -0.5, "yaw": 0.0, "desc": "left"},
]
class WaypointNav(Node):
def __init__(self, waypoints, loop=False, threshold=0.25):
super().__init__("waypoint_nav")
self.waypoints = waypoints
self.loop = loop
self.threshold = threshold
self.idx = 0
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
qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
self.pub_goal = self.create_publisher(PoseStamped, "/goal_pose", qos)
self.sub_odom = self.create_subscription(
Odometry, "/odom", self.odom_cb, 10)
self.sub_status = self.create_subscription(
String, "/nav_status", self.status_cb, 10)
self.timer = self.create_timer(0.2, self.check) # 5Hz
def odom_cb(self, msg):
self.pose_x = msg.pose.pose.position.x
self.pose_y = msg.pose.pose.position.y
q = msg.pose.pose.orientation
self.pose_yaw = math.atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z))
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.skip_gen = self.fail_gen
self.idx += 1
self.reached = False
def send_goal(self, wp):
msg = PoseStamped()
msg.header.frame_id = "map"
msg.header.stamp = self.get_clock().now().to_msg()
msg.pose.position.x = float(wp["x"])
msg.pose.position.y = float(wp["y"])
yaw_deg = float(wp.get("yaw", 0.0))
yaw = math.radians(yaw_deg) # JSON stores degrees
msg.pose.orientation.z = math.sin(yaw / 2.0)
msg.pose.orientation.w = math.cos(yaw / 2.0)
self.pub_goal.publish(msg)
desc = wp.get("desc", "")
self.get_logger().info(
f"→ WP {self.idx+1}/{len(self.waypoints)}: "
f"({wp['x']:.2f},{wp['y']:.2f}) {desc}"
)
def check(self):
if self.idx >= len(self.waypoints):
if self.loop:
self.idx = 0
self.reached = False
else:
return
wp = self.waypoints[self.idx]
if not self.reached:
self.fail_gen += 1 # new generation for this wp
self.send_goal(wp)
self.reached = True
dist = math.hypot(self.pose_x - wp["x"], self.pose_y - wp["y"])
if dist < self.threshold:
self.get_logger().info(
f"✓ WP {self.idx+1} reached (dist={dist:.2f}m)")
self.idx += 1
self.reached = False
self.skip_streak = 0
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--waypoints", type=str, default=None,
help="JSON file with waypoints")
parser.add_argument("--loop", action="store_true",
help="Loop back to first waypoint after last")
parser.add_argument("--threshold", type=float, default=0.3,
help="Arrival distance threshold (m)")
args = parser.parse_args()
if args.waypoints:
with open(args.waypoints) as f:
waypoints = json.load(f)
print(f"Loaded {len(waypoints)} waypoints from {args.waypoints}")
else:
waypoints = DEFAULT_WAYPOINTS
print(f"Using {len(waypoints)} default waypoints")
rclpy.init()
node = WaypointNav(waypoints, args.loop, args.threshold)
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

110
scripts/waypoints.json Normal file
View File

@@ -0,0 +1,110 @@
[
{
"x": 2.044,
"y": -1.656,
"yaw": 71,
"desc": "wp0"
},
{
"x": 1.256,
"y": -0.756,
"yaw": 166,
"desc": "wp1"
},
{
"x": 0.056,
"y": -0.45,
"yaw": 90,
"desc": "wp2"
},
{
"x": 0.031,
"y": 0.25,
"yaw": 64,
"desc": "wp3"
},
{
"x": 0.312,
"y": 0.694,
"yaw": 22,
"desc": "wp4"
},
{
"x": 1.475,
"y": 0.694,
"yaw": 14,
"desc": "wp5"
},
{
"x": 1.75,
"y": 1.469,
"yaw": 109,
"desc": "wp6"
},
{
"x": 1.387,
"y": 1.8,
"yaw": 163,
"desc": "wp7"
},
{
"x": 0.369,
"y": 1.988,
"yaw": 179,
"desc": "wp8"
},
{
"x": -0.381,
"y": 1.988,
"yaw": -180,
"desc": "wp9"
},
{
"x": -1.487,
"y": 1.919,
"yaw": -157,
"desc": "wp10"
},
{
"x": -1.825,
"y": 1.244,
"yaw": -90,
"desc": "wp11"
},
{
"x": -1.756,
"y": 0.794,
"yaw": -49,
"desc": "wp12"
},
{
"x": -0.844,
"y": 0.544,
"yaw": -7,
"desc": "wp13"
},
{
"x": -0.388,
"y": 0.388,
"yaw": -64,
"desc": "wp14"
},
{
"x": -0.169,
"y": -0.106,
"yaw": -96,
"desc": "wp15"
},
{
"x": -0.169,
"y": -0.506,
"yaw": -127,
"desc": "wp16"
},
{
"x": -1.956,
"y": -2.175,
"yaw": -90,
"desc": "wp17"
}
]

14
src/bt_executor.cpp
View File

@@ -35,9 +35,11 @@ Twist BTExecutor::tick(const Pose2D& pose, const GridMap& map) {
fprintf(stderr, "[BT] Plan OK, %zu waypoints\n", current_path_.size());
state_ = NavState::TRACKING;
} else {
fprintf(stderr, "[BT] Plan FAILED to (%.2f,%.2f)\n",
goal_->x, goal_->y);
static int fail_cnt = 0;
fprintf(stderr, "[BT] Plan FAILED #%d to (%.2f,%.2f)\n",
++fail_cnt, goal_->x, goal_->y);
state_ = NavState::FAILED;
fail_callback_();
}
}
break;
@@ -54,7 +56,7 @@ Twist BTExecutor::tick(const Pose2D& pose, const GridMap& map) {
if (++tick_cnt % 5 == 0 && goal_) {
static const int inflations[] = {
INFLATION_CELLS, INFLATION_CELLS*4/5,
(int)(CAR_HALF_W / GRID_RES)
(int)(CAR_HALF_W / GRID_RES), 0 // last resort: no inflation
};
std::vector<Pose2D> new_path;
for (int inflate : inflations) {
@@ -79,12 +81,6 @@ 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 (std::abs(goal_->yaw) > 0.01) {
double h_err = goal_->yaw - pose.yaw;
while (h_err > M_PI) h_err -= 2*M_PI;
while (h_err < -M_PI) h_err += 2*M_PI;
return std::abs(h_err) < 0.15;
}
return true;
}

11
src/bt_executor.hpp
View File

@@ -13,19 +13,16 @@ class BTExecutor {
public:
BTExecutor();
// ── Set navigation goal ────────────────────────────
void setGoal(const Pose2D& goal);
void setTestPath(const std::vector<Pose2D>& path);
void setParams(double la, double ms, double mr, double sl, double cf);
void setFailCallback(std::function<void()> cb) { fail_callback_ = std::move(cb); }
bool hasGoal() const { return goal_.has_value(); }
// ── Tick the state machine (call at 10Hz) ──────────
Twist tick(const Pose2D& pose, const GridMap& map);
// ── Status query ───────────────────────────────────
NavState state() const { return state_; }
const std::vector<Pose2D>& currentPath() const { return current_path_; }
bool hasGoal() const { return goal_.has_value(); }
private:
NavState state_ = NavState::IDLE;
@@ -33,13 +30,9 @@ private:
std::vector<Pose2D> current_path_;
GlobalPlanner planner_;
LocalPlanner local_planner_;
int replan_count_ = 0;
int stuck_count_ = 0;
int recovery_attempts_ = 0;
Pose2D recovery_target_;
std::function<void()> fail_callback_;
bool atGoal(const Pose2D& pose) const;
bool tryRecover(const Pose2D& pose, const GridMap& map);
};
} // namespace car_nav_lite

8
src/grid_map.cpp
View File

@@ -3,6 +3,7 @@
#include <sstream>
#include <cmath>
#include <algorithm>
#include <opencv2/imgproc.hpp>
namespace car_nav_lite {
@@ -235,6 +236,13 @@ double GridMap::raycast(double wx, double wy, double angle, double max_range) co
return max_range; // no hit
}
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);
}
void GridMap::markRect(double x1, double y1, double x2, double y2, uint8_t value) {
int gx1, gy1, gx2, gy2;
worldToGrid(x1, y1, gx1, gy1);

61
src/grid_map.hpp Normal file
View File

@@ -0,0 +1,61 @@
#pragma once
#include "car_nav_lite/types.hpp"
#include <string>
#include <array>
namespace car_nav_lite {
// ── 8-connected neighbor deltas ──────────────────────────
constexpr std::array<std::pair<int,int>, 8> NEIGHBORS = {{
{1,0},{-1,0},{0,1},{0,-1},{1,1},{-1,-1},{1,-1},{-1,1}
}};
class GridMap {
public:
GridMap();
// ── Load PGM P5 from file ──────────────────────────
bool loadPGM(const std::string& path);
// ── Coordinate transforms ──────────────────────────
bool worldToGrid(double wx, double wy, int& gx, int& gy) const;
void gridToWorld(int gx, int gy, double& wx, double& wy) const;
// ── Access ──────────────────────────────────────────
uint8_t at(int gx, int gy) const;
void set(int gx, int gy, uint8_t v);
bool inBounds(int gx, int gy) const;
bool isFree(int gx, int gy) const;
bool isFree(int gx, int gy, int inflate_cells) const; // configurable inflation
// ── Raycast on static obstacles (for scan-to-map matching) ──
// Returns range to first obstacle >128 along (wx,wy) → angle.
double raycast(double wx, double wy, double angle, double max_range = 5.0) const;
// ── Oriented footprint collision check (EXACT-MPPI style) ──
// Checks raw occupancy against car's oriented rectangle at (wx,wy,yaw).
// Handles car width/length correctly regardless of orientation.
bool isFreeFootprint(double wx, double wy, double yaw) const;
// ── Obstacle update from scan ──────────────────────
void updateScan(const LaserScan& scan, const Pose2D& pose);
// ── Morphological close (remove noise, merge nearby blobs) ──
void morphologyClose(int kernel_size = 3);
// ── 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);
// ── Getters ────────────────────────────────────────
int width() const { return GRID_SIZE; }
int height() const { return GRID_SIZE; }
const OccupancyGrid& data() const { return cells_; }
private:
OccupancyGrid cells_;
void bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val);
};
} // namespace car_nav_lite

3
src/local_planner.cpp
View File

@@ -162,6 +162,9 @@ Twist LocalPlanner::compute(const Pose2D& pose,
cmd.vx = weighted_vx / sum_weights;
cmd.wz = weighted_wz / sum_weights;
// 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);

124
src/localizer.cpp
View File

@@ -32,27 +32,21 @@ Pose2D Localizer::pose() const {
odom_pose_.yaw + correction_.yaw};
}
// ── Fence-line-based scan correction ───────────────────────
//
// The 5m×5m arena has a rectangular fence. We detect Hough lines
// in the scan, keep near-H/V ones (= fence walls), and compare
// their world positions against the known fence at x=±2.5, y=±2.5.
// This gives dx,dy correction. Yaw comes from the line angle bias.
//
void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map) {
// ── Scan-to-map correction (fence-line based, offset mode) ──
void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
const Pose2D* base_override) {
if (scan.size() < 30) return;
const int N = scan.size();
const Pose2D& base = odom_pose_;
Pose2D base = base_override ? *base_override : odom_pose_;
// ── 1. Render scan to local image (world coords, ±1.5m around car) ──
constexpr int IMG_SZ = 600; // 3m × 3m at 5mm
constexpr int HALF = IMG_SZ / 2;
// ── 1. Render scan to local image ──
constexpr int IMG_SZ = 600, HALF = IMG_SZ / 2;
int cx_g, cy_g;
map.worldToGrid(base.x, base.y, cx_g, cy_g);
cv::Mat scan_img(IMG_SZ, IMG_SZ, CV_8UC1, cv::Scalar(0));
int hit_count = 0;
int point_count = 0;
for (int i = 0; i < N; i += 2) {
double r = scan.ranges[i];
if (r < 0.1 || r > 4.5) continue;
@@ -65,47 +59,41 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map) {
int py = (gy - cy_g) + HALF;
if (px >= 0 && px < IMG_SZ && py >= 0 && py < IMG_SZ) {
scan_img.at<uint8_t>(py, px) = 255;
++hit_count;
++point_count;
}
}
if (hit_count < 30) return;
if (point_count < 30) return;
// ── 2. Hough line detection ────────────────────────────
// ── 2. Hough lines ──
std::vector<cv::Vec4i> lines;
cv::HoughLinesP(scan_img, lines,
1, CV_PI / 180.0, // rho=1px, theta=1°
20, // vote threshold
40, // min line length (40px = 20cm)
10); // max gap
cv::HoughLinesP(scan_img, lines, 1, CV_PI/180.0, 20, 40, 10);
// ── 3. Fence line analysis (no angle filter — car yaw can be anything) ──
// Use double-angle voting: 0° and 90° fence lines both map to 0° after
// doubling. Deviation of the dominant direction from 0 = yaw error.
double sum_cos2 = 0, sum_sin2 = 0;
// ── 3. Yaw from line deviations ──
double sum_dyaw = 0;
int yaw_count = 0;
for (auto& l : lines) {
double ldx = l[2] - l[0], ldy = l[3] - l[1];
if (ldx == 0 && ldy == 0) continue;
double ang = std::atan2(ldy, ldx); // [-π, π]
sum_cos2 += std::cos(2.0 * ang);
sum_sin2 += std::sin(2.0 * ang);
double ang = std::atan2(ldy, ldx);
double a0 = ang;
while (a0 > M_PI/2) a0 -= M_PI/2;
while (a0 < -M_PI/2) a0 += M_PI/2;
sum_dyaw += (std::abs(a0) < std::abs(a0 - M_PI/2)) ? a0 : (a0 - M_PI/2);
++yaw_count;
}
if (yaw_count < 3) {
correction_.x *= 0.9; correction_.y *= 0.9;
return;
// ── Debug ──
static int yaw_dbg = 0;
if (yaw_count >= 2 && ++yaw_dbg % 5 == 0) {
double dyaw_tmp = yaw_count > 0 ? sum_dyaw / yaw_count : 0;
fprintf(stderr, "[YAW] odom=%.1f° scan=%.1f° dyaw=%.2f° lines=%d\n",
base.yaw * 57.3, (base.yaw - dyaw_tmp) * 57.3, dyaw_tmp * 57.3, yaw_count);
}
// Dominant fence orientation → yaw error
double dyaw = std::atan2(sum_sin2, sum_cos2) / 2.0;
while (dyaw > M_PI/4) dyaw -= M_PI/2;
while (dyaw < -M_PI/4) dyaw += M_PI/2;
if (yaw_count < 2) { correction_.x *= 0.9; correction_.y *= 0.9; return; }
double dyaw = sum_dyaw / yaw_count;
// ── 4. Re-classify lines AFTER yaw correction ──────────
// Now we know the fence direction. Lines at ang≈dyaw or ang≈dyaw+π/2
// are fence walls.
// ── 4. Classify lines & compute translation ──
double sum_dx = 0, sum_dy = 0;
int line_count = 0;
constexpr double TOL = 15.0 * M_PI / 180.0;
@@ -114,31 +102,23 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map) {
double ldx = l[2] - l[0], ldy = l[3] - l[1];
if (ldx == 0 && ldy == 0) continue;
double ang = std::atan2(ldy, ldx);
if (ang < 0) ang += M_PI; // [0, π)
if (ang < 0) ang += M_PI;
// Check if this line aligns with the detected fence direction
double diff1 = std::abs(ang - dyaw);
while (diff1 > M_PI) diff1 -= M_PI;
if (diff1 > M_PI/2) diff1 = M_PI - diff1;
double diff2 = std::abs(ang - (dyaw + M_PI/2));
while (diff2 > M_PI) diff2 -= M_PI;
if (diff2 > M_PI/2) diff2 = M_PI - diff2;
double diff = std::min(diff1, diff2);
if (diff > TOL) continue; // not aligned with fence
if (std::min(diff1, diff2) > TOL) continue;
++line_count;
// Line midpoint → world coords → compare with known fence (±2.5m)
double mx = (l[0] + l[2]) / 2.0;
double my = (l[1] + l[3]) / 2.0;
double mwx, mwy;
map.gridToWorld(cx_g - HALF + (int)mx, cy_g - HALF + (int)my, mwx, mwy);
// Which fence wall? Use the corrected yaw to determine H vs V
bool is_horiz = (diff1 < diff2); // closer to dyaw = horizontal in fence frame
bool is_horiz = (diff1 < diff2);
if (is_horiz) {
double expected = (mwy > 0) ? 2.5 : -2.5;
sum_dy += (expected - mwy);
@@ -148,35 +128,25 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map) {
}
}
if (line_count < 2) {
correction_.yaw = -dyaw; // at least update yaw
correction_.x *= 0.9; correction_.y *= 0.9;
return;
}
double dx = sum_dx / line_count;
double dy = sum_dy / line_count;
// ── 5. Update offset ──
if (line_count >= 2) {
double dx = sum_dx / line_count, dy = sum_dy / line_count;
double corr_dist = std::sqrt(dx*dx + dy*dy);
// ── 5. Safety checks before updating offset ──
bool trust_translation = (corr_dist < 0.2 && line_count >= 2);
bool trust_yaw = (std::abs(dyaw) < 0.15 && yaw_count >= 2);
// Don't let correction push pose outside arena
double new_x = odom_pose_.x + dx;
double new_y = odom_pose_.y + dy;
if (std::abs(new_x) > 2.3 || std::abs(new_y) > 2.3)
trust_translation = false;
if (trust_translation && trust_yaw) {
correction_.x = dx;
correction_.y = dy;
if (corr_dist < 0.3 && std::abs(dyaw) < 0.2) {
correction_.x = dx; correction_.y = dy; correction_.yaw = -dyaw;
fprintf(stderr, "[Localizer] fence: d(%.3f,%.3f,%.2f°) lines=%d\n",
dx, dy, -dyaw*57.3, line_count);
}
} else if (line_count == 1) {
bool is_horiz = (std::abs(dyaw) < std::abs(dyaw - M_PI/2));
if (is_horiz) {
correction_.y = sum_dy; correction_.x *= 0.9;
} else {
correction_.x = sum_dx; correction_.y *= 0.9;
}
correction_.yaw = -dyaw;
fprintf(stderr, "[Localizer] fence: d(%.3f,%.3f,%.2f°) lines=%d/%d\n",
dx, dy, -dyaw*57.3, line_count, yaw_count);
} else if (trust_yaw) {
correction_.yaw = -dyaw;
correction_.x *= 0.9; correction_.y *= 0.9;
fprintf(stderr, "[Localizer] 1-line: d(%.3f,%.3f,%.2f°)\n",
sum_dx, sum_dy, -dyaw*57.3);
} else {
correction_.x *= 0.9; correction_.y *= 0.9;
}

10
src/localizer.hpp
View File

@@ -10,19 +10,13 @@ class Localizer {
public:
Localizer();
// ── Odometry update (called at odom rate) ──────────
void updateOdom(double vx, double wz, double dt);
void updateIMU(double gyro_z, double dt);
// Set raw odom pose. pose() returns odom + correction offset.
void setOdomPose(const Pose2D& p);
// ── Scan-to-map correction (updates persistent offset) ──
// Correction = matched_pose - odom_pose, stored as offset.
// Only updates if confidence is high; otherwise offset decays to zero.
void correctWithScanCV(const LaserScan& scan, const GridMap& map);
void correctWithScanCV(const LaserScan& scan, const GridMap& map,
const Pose2D* base_override = nullptr);
// ── Final pose = odom + correction offset ──────────
Pose2D pose() const;
private:

18
src/main.cpp
View File

@@ -6,6 +6,7 @@
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_msgs/msg/string.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
@@ -69,6 +70,11 @@ public:
pub_cmd_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
pub_path_ = create_publisher<nav_msgs::msg::Path>("/global_path", 10);
pub_map_ = create_publisher<nav_msgs::msg::OccupancyGrid>("/map", rclcpp::QoS(1).transient_local());
pub_status_ = create_publisher<std_msgs::msg::String>("/nav_status", 10);
bt_.setFailCallback([this](){
std_msgs::msg::String msg; msg.data = "fail";
pub_status_->publish(msg);
});
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
if (!mf_.empty()) {
@@ -79,6 +85,7 @@ public:
timer_ = create_wall_timer(50ms, std::bind(&NavLiteNode::tick, this));
map_timer_ = create_wall_timer(1s, std::bind(&NavLiteNode::publish_map, this));
correct_timer_ = create_wall_timer(200ms, std::bind(&NavLiteNode::correct_localization, this));
morph_timer_ = create_wall_timer(100ms, std::bind(&NavLiteNode::morph_close, this));
RCLCPP_INFO(get_logger(),"car_nav_lite ready. test_mode:=true for auto path. "
"ros2 param set /nav_lite_node <name> <value>");
@@ -105,6 +112,7 @@ private:
// ── State ──
std::mutex mtx_;
LaserScan latest_scan_; bool has_scan_=false;
Pose2D scan_pose_snapshot_;
double latest_vx_=0, latest_wz_=0, latest_gyro_z_=0;
rclcpp::Time last_odom_t_{0,0,RCL_ROS_TIME};
rclcpp::Time last_imu_t_{0,0,RCL_ROS_TIME};
@@ -116,14 +124,16 @@ private:
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_cmd_;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_path_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_map_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_status_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_;
rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_, morph_timer_;
void scan_cb(LaserScanMsg::SharedPtr m){std::lock_guard lk(mtx_);
latest_scan_.angle_min=m->angle_min;latest_scan_.angle_max=m->angle_max;
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;}
latest_scan_.ranges=m->ranges;has_scan_=true;
scan_pose_snapshot_ = localizer_.pose();}
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.
@@ -229,11 +239,13 @@ private:
void correct_localization(){
if(!has_scan_)return;
auto pose_before = localizer_.pose();
localizer_.correctWithScanCV(latest_scan_, map_);
localizer_.correctWithScanCV(latest_scan_, map_, &scan_pose_snapshot_);
auto pose_after = localizer_.pose();
(void)pose_before; (void)pose_after;
}
void morph_close(){std::lock_guard lk(mtx_); map_.morphologyClose(3);}
void publish_tf(const Pose2D& pose){
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp=now();tf.header.frame_id="map";tf.child_frame_id="base_link";

145
src/waypoint_nav.py Normal file
View File

@@ -0,0 +1,145 @@
#!/usr/bin/env python3
"""Sequential waypoint navigation for car_nav_lite.
Publishes /goal_pose waypoints one by one. Waits for the car to reach
each waypoint before publishing the next.
Usage:
python3 waypoint_nav.py # default waypoints
python3 waypoint_nav.py --waypoints waypoints.json # from JSON file
python3 waypoint_nav.py --loop # loop back to start
JSON format:
[{"x": 1.0, "y": -1.5, "yaw": 0.0}, ...]
"""
import argparse
import json
import math
import time
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
# Default waypoints for 5m×5m arena (car spawns near -2.0, -2.3)
DEFAULT_WAYPOINTS = [
{"x": -0.5, "y": -1.5, "yaw": 0.0, "desc": "mid-left"},
{"x": 1.0, "y": -1.5, "yaw": 0.0, "desc": "right"},
{"x": 1.5, "y": 0.0, "yaw": 0.0, "desc": "top-right"},
{"x": 0.0, "y": 1.5, "yaw": 0.0, "desc": "top-center"},
{"x": -1.5, "y": 1.0, "yaw": 0.0, "desc": "top-left"},
{"x": -1.5, "y": -0.5, "yaw": 0.0, "desc": "left"},
]
class WaypointNav(Node):
def __init__(self, waypoints, loop=False, threshold=0.25):
super().__init__("waypoint_nav")
self.waypoints = waypoints
self.loop = loop
self.threshold = threshold
self.idx = 0
self.reached = False
self.last_dist = 1e9
self.stuck_count = 0
self.pose_x = self.pose_y = self.pose_yaw = 0.0
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
self.pub_goal = self.create_publisher(PoseStamped, "/goal_pose", qos)
self.sub_odom = self.create_subscription(
Odometry, "/odom", self.odom_cb, 10)
self.timer = self.create_timer(0.2, self.check) # 5Hz
def odom_cb(self, msg):
self.pose_x = msg.pose.pose.position.x
self.pose_y = msg.pose.pose.position.y
q = msg.pose.pose.orientation
self.pose_yaw = math.atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z))
def send_goal(self, wp):
msg = PoseStamped()
msg.header.frame_id = "map"
msg.header.stamp = self.get_clock().now().to_msg()
msg.pose.position.x = float(wp["x"])
msg.pose.position.y = float(wp["y"])
yaw_deg = float(wp.get("yaw", 0.0))
yaw = math.radians(yaw_deg) # JSON stores degrees
msg.pose.orientation.z = math.sin(yaw / 2.0)
msg.pose.orientation.w = math.cos(yaw / 2.0)
self.pub_goal.publish(msg)
desc = wp.get("desc", "")
self.get_logger().info(
f"→ WP {self.idx+1}/{len(self.waypoints)}: "
f"({wp['x']:.2f},{wp['y']:.2f}) {desc}"
)
def check(self):
if self.idx >= len(self.waypoints):
if self.loop:
self.idx = 0
self.reached = False
else:
return
wp = self.waypoints[self.idx]
if not self.reached:
self.send_goal(wp)
self.reached = True
self.last_dist = 1e9
self.stuck_count = 0
dist = math.hypot(self.pose_x - wp["x"], self.pose_y - wp["y"])
if dist < self.last_dist - 0.05:
self.stuck_count = 0
else:
self.stuck_count += 1
self.last_dist = min(self.last_dist, dist)
if self.stuck_count > 15:
self.get_logger().warn(f"⏭ WP {self.idx+1} stuck — skip")
self.idx += 1
self.reached = False
return
if dist < self.threshold:
self.get_logger().info(
f"✓ WP {self.idx+1} reached (dist={dist:.2f}m)")
self.idx += 1
self.reached = False
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--waypoints", type=str, default=None,
help="JSON file with waypoints")
parser.add_argument("--loop", action="store_true",
help="Loop back to first waypoint after last")
parser.add_argument("--threshold", type=float, default=0.3,
help="Arrival distance threshold (m)")
args = parser.parse_args()
if args.waypoints:
with open(args.waypoints) as f:
waypoints = json.load(f)
print(f"Loaded {len(waypoints)} waypoints from {args.waypoints}")
else:
waypoints = DEFAULT_WAYPOINTS
print(f"Using {len(waypoints)} default waypoints")
rclpy.init()
node = WaypointNav(waypoints, args.loop, args.threshold)
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()