diff --git a/include/car_nav_lite/bt_executor.hpp b/include/car_nav_lite/bt_executor.hpp index 423461f..6a6519d 100644 --- a/include/car_nav_lite/bt_executor.hpp +++ b/include/car_nav_lite/bt_executor.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #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& path); void setParams(double la, double ms, double mr, double sl, double cf); + void setFailCallback(std::function 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 current_path_; GlobalPlanner planner_; LocalPlanner local_planner_; + std::function fail_callback_; bool atGoal(const Pose2D& pose) const; }; diff --git a/include/car_nav_lite/grid_map.hpp b/include/car_nav_lite/grid_map.hpp index 7802556..7323e9a 100644 --- a/include/car_nav_lite/grid_map.hpp +++ b/include/car_nav_lite/grid_map.hpp @@ -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); diff --git a/include/car_nav_lite/localizer.hpp b/include/car_nav_lite/localizer.hpp index 18d8ed9..1729674 100644 --- a/include/car_nav_lite/localizer.hpp +++ b/include/car_nav_lite/localizer.hpp @@ -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: diff --git a/scripts/waypoint_nav.py b/scripts/waypoint_nav.py new file mode 100755 index 0000000..6e4bf71 --- /dev/null +++ b/scripts/waypoint_nav.py @@ -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() diff --git a/scripts/waypoints.json b/scripts/waypoints.json new file mode 100644 index 0000000..13541d8 --- /dev/null +++ b/scripts/waypoints.json @@ -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" + } +] \ No newline at end of file diff --git a/src/bt_executor.cpp b/src/bt_executor.cpp index 6839855..0df4d5d 100644 --- a/src/bt_executor.cpp +++ b/src/bt_executor.cpp @@ -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 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; } diff --git a/src/bt_executor.hpp b/src/bt_executor.hpp index 57dc7af..16182e2 100644 --- a/src/bt_executor.hpp +++ b/src/bt_executor.hpp @@ -13,19 +13,16 @@ class BTExecutor { public: BTExecutor(); - // ── Set navigation goal ──────────────────────────── void setGoal(const Pose2D& goal); void setTestPath(const std::vector& path); void setParams(double la, double ms, double mr, double sl, double cf); + void setFailCallback(std::function 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& currentPath() const { return current_path_; } - bool hasGoal() const { return goal_.has_value(); } private: NavState state_ = NavState::IDLE; @@ -33,13 +30,9 @@ private: std::vector current_path_; GlobalPlanner planner_; LocalPlanner local_planner_; - int replan_count_ = 0; - int stuck_count_ = 0; - int recovery_attempts_ = 0; - Pose2D recovery_target_; + std::function fail_callback_; bool atGoal(const Pose2D& pose) const; - bool tryRecover(const Pose2D& pose, const GridMap& map); }; } // namespace car_nav_lite diff --git a/src/grid_map.cpp b/src/grid_map.cpp index 43cc0b1..01ed8f9 100644 --- a/src/grid_map.cpp +++ b/src/grid_map.cpp @@ -3,6 +3,7 @@ #include #include #include +#include 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); diff --git a/src/grid_map.hpp b/src/grid_map.hpp new file mode 100644 index 0000000..7323e9a --- /dev/null +++ b/src/grid_map.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include "car_nav_lite/types.hpp" +#include +#include + +namespace car_nav_lite { + +// ── 8-connected neighbor deltas ────────────────────────── +constexpr std::array, 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 diff --git a/src/local_planner.cpp b/src/local_planner.cpp index caeb86a..6657b39 100644 --- a/src/local_planner.cpp +++ b/src/local_planner.cpp @@ -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); diff --git a/src/localizer.cpp b/src/localizer.cpp index c1f3b4e..c9fb6c5 100644 --- a/src/localizer.cpp +++ b/src/localizer.cpp @@ -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(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 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; - 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; + // ── 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); + 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; } diff --git a/src/localizer.hpp b/src/localizer.hpp index 18d8ed9..1729674 100644 --- a/src/localizer.hpp +++ b/src/localizer.hpp @@ -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: diff --git a/src/main.cpp b/src/main.cpp index f96a167..0328442 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -69,6 +70,11 @@ public: pub_cmd_ = create_publisher("/cmd_vel", 10); pub_path_ = create_publisher("/global_path", 10); pub_map_ = create_publisher("/map", rclcpp::QoS(1).transient_local()); + pub_status_ = create_publisher("/nav_status", 10); + bt_.setFailCallback([this](){ + std_msgs::msg::String msg; msg.data = "fail"; + pub_status_->publish(msg); + }); tf_broadcaster_ = std::make_shared(*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 "); @@ -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::SharedPtr pub_cmd_; rclcpp::Publisher::SharedPtr pub_path_; rclcpp::Publisher::SharedPtr pub_map_; + rclcpp::Publisher::SharedPtr pub_status_; std::shared_ptr 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"; diff --git a/src/waypoint_nav.py b/src/waypoint_nav.py new file mode 100644 index 0000000..34f42af --- /dev/null +++ b/src/waypoint_nav.py @@ -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()