diff --git a/CMakeLists.txt b/CMakeLists.txt index fe29f06..22fc126 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ add_executable(nav_lite_node src/global_planner.cpp src/local_planner.cpp src/bt_executor.cpp + src/session_mapper.cpp ) target_include_directories(nav_lite_node PRIVATE diff --git a/include/car_nav_lite/bt_executor.hpp b/include/car_nav_lite/bt_executor.hpp index 6a6519d..47c6db8 100644 --- a/include/car_nav_lite/bt_executor.hpp +++ b/include/car_nav_lite/bt_executor.hpp @@ -18,6 +18,11 @@ public: 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); } + void fillMetrics(NavMetrics& m) const { + m.mppi_alive = local_planner_.mppi_alive_; + m.mppi_total = local_planner_.mppi_total_; + m.mppi_min_cost = local_planner_.mppi_min_cost_; + } bool hasGoal() const { return goal_.has_value(); } Twist tick(const Pose2D& pose, const GridMap& map); diff --git a/include/car_nav_lite/fence_model.hpp b/include/car_nav_lite/fence_model.hpp new file mode 100644 index 0000000..a3a3a72 --- /dev/null +++ b/include/car_nav_lite/fence_model.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include + +namespace car_nav_lite { + +// ── Line in general form: n.x * x + n.y * y = d ────────── +struct Line2D { + double nx = 0.0, ny = 0.0, d = 0.0; + + void setFromTwoPoints(double x1, double y1, double x2, double y2) { + double dx = x2 - x1, dy = y2 - y1; + double len = std::sqrt(dx*dx + dy*dy); + if (len < 1e-9) { nx = 1; ny = 0; d = x1; return; } + nx = -dy / len; + ny = dx / len; + d = nx * x1 + ny * y1; + } + + double distance(double x, double y) const { + return std::abs(nx * x + ny * y - d); + } + + double signedDistance(double x, double y) const { + return nx * x + ny * y - d; + } +}; + +// ── Fence model: four walls of the arena ───────────────── +struct FenceModel { + bool valid = false; + Line2D left, right, bottom, top; + // Pre-computed wall positions for localization + double left_x = -2.5, right_x = 2.5; + double bottom_y = -2.5, top_y = 2.5; + double yaw_field = 0.0; + double width = 0.0, height = 0.0; + double cx = 0.0, cy = 0.0; +}; + +} // namespace car_nav_lite diff --git a/include/car_nav_lite/local_planner.hpp b/include/car_nav_lite/local_planner.hpp index a83d569..8b4367f 100644 --- a/include/car_nav_lite/local_planner.hpp +++ b/include/car_nav_lite/local_planner.hpp @@ -18,6 +18,10 @@ public: const std::vector& path, const GridMap& map); +public: // metrics accessible from BTExecutor + int mppi_alive_ = 0, mppi_total_ = 0; + double mppi_min_cost_ = 0; + private: // ── Params ── double ms_ = 1.0, mr_ = 0.25, sl_ = 0.6, cf_ = 6.0; diff --git a/include/car_nav_lite/localizer.hpp b/include/car_nav_lite/localizer.hpp index 1729674..278db09 100644 --- a/include/car_nav_lite/localizer.hpp +++ b/include/car_nav_lite/localizer.hpp @@ -1,6 +1,7 @@ #pragma once #include "car_nav_lite/types.hpp" +#include "car_nav_lite/fence_model.hpp" namespace car_nav_lite { @@ -13,15 +14,22 @@ public: void updateOdom(double vx, double wz, double dt); void updateIMU(double gyro_z, double dt); void setOdomPose(const Pose2D& p); + void setFenceModel(const FenceModel& f) { fence_ = f; use_fence_ = f.valid; } void correctWithScanCV(const LaserScan& scan, const GridMap& map, const Pose2D* base_override = nullptr); Pose2D pose() const; + // Metrics + int loc_lines_ = 0; + double loc_dx_ = 0, loc_dy_ = 0, loc_dyaw_ = 0; + private: Pose2D odom_pose_; Pose2D correction_; + FenceModel fence_; + bool use_fence_ = false; double yaw_imu_ = 0.0, yaw_odom_ = 0.0; }; diff --git a/include/car_nav_lite/session_mapper.hpp b/include/car_nav_lite/session_mapper.hpp new file mode 100644 index 0000000..0741995 --- /dev/null +++ b/include/car_nav_lite/session_mapper.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include "car_nav_lite/types.hpp" +#include "car_nav_lite/fence_model.hpp" + +namespace car_nav_lite { + +class SessionMapper { +public: + SessionMapper(); + + // ── Accumulate scan during calibration ─────────────── + void addScan(const LaserScan& scan, const Pose2D& start_pose, + int step = 4); + + // ── Build fence model + extract cones ──────────────── + bool build(FenceModel& fence, std::vector& cones, + double expected_size = 5.0, double size_tol = 0.35, + double ransac_thresh = 0.04); + + int scanCount() const { return scan_count_; } + const std::vector& points() const { return points_world_; } + +private: + std::vector points_world_; + int scan_count_ = 0; +}; + +} // namespace car_nav_lite diff --git a/include/car_nav_lite/types.hpp b/include/car_nav_lite/types.hpp index 37f1453..0ceb54f 100644 --- a/include/car_nav_lite/types.hpp +++ b/include/car_nav_lite/types.hpp @@ -80,4 +80,24 @@ inline double limit(double v, double lo, double hi) { return v < lo ? lo : (v > hi ? hi : v); } +// ── Navigation metrics (for /nav_metrics JSON) ──────────── +struct NavMetrics { + // localization + double loc_conf = 0; + double loc_rms = 0; + int loc_lines = 0; + double loc_dx = 0, loc_dy = 0, loc_dyaw = 0; + + // planning + int mppi_alive = 0; + int mppi_total = 0; + double mppi_min_cost = 0; + double mppi_alive_ratio = 0; + + // state + double cmd_vx = 0, cmd_wz = 0; + double pose_x = 0, pose_y = 0, pose_yaw = 0; + const char* state = "IDLE"; +}; + } // namespace car_nav_lite diff --git a/src/bt_executor.hpp b/src/bt_executor.hpp deleted file mode 100644 index 16182e2..0000000 --- a/src/bt_executor.hpp +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -#include -#include "car_nav_lite/types.hpp" -#include "car_nav_lite/grid_map.hpp" -#include "car_nav_lite/localizer.hpp" -#include "car_nav_lite/global_planner.hpp" -#include "car_nav_lite/local_planner.hpp" - -namespace car_nav_lite { - -class BTExecutor { -public: - BTExecutor(); - - 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); - - NavState state() const { return state_; } - const std::vector& currentPath() const { return current_path_; } - -private: - NavState state_ = NavState::IDLE; - std::optional goal_; - std::vector current_path_; - GlobalPlanner planner_; - LocalPlanner local_planner_; - std::function fail_callback_; - - bool atGoal(const Pose2D& pose) const; -}; - -} // namespace car_nav_lite diff --git a/src/grid_map.hpp b/src/grid_map.hpp deleted file mode 100644 index 8682c82..0000000 --- a/src/grid_map.hpp +++ /dev/null @@ -1,65 +0,0 @@ -#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); - - // ── 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); - - // ── Getters ──────────────────────────────────────── - int width() const { return GRID_SIZE; } - int height() const { return GRID_SIZE; } - const OccupancyGrid& data() const { return cells_; } - -private: - OccupancyGrid cells_; - OccupancyGrid static_cells_; // snapshot after PGM load - 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 9e74801..1edb3dc 100644 --- a/src/local_planner.cpp +++ b/src/local_planner.cpp @@ -137,6 +137,7 @@ Twist LocalPlanner::compute(const Pose2D& pose, for (int k = 0; k < K; ++k) if (best_costs[k] < 500.0) ++alive_now; alive_last = alive_now; + mppi_alive_ = alive_now; mppi_total_ = K; mppi_min_cost_ = min_cost; // ── Softmax ── double sum_weights = 0, weighted_vx = 0, weighted_wz = 0; diff --git a/src/local_planner.hpp b/src/local_planner.hpp deleted file mode 100644 index 07da861..0000000 --- a/src/local_planner.hpp +++ /dev/null @@ -1,60 +0,0 @@ -#pragma once - -#include "car_nav_lite/types.hpp" -#include "car_nav_lite/grid_map.hpp" - -namespace car_nav_lite { - -class LocalPlanner { -public: - LocalPlanner(); - - void setParams(double la, double ms, double mr, double sl, double cf); - - // ── Lightweight MPPI (EXACT-MPPI-style, Ackermann) ── - // Samples K trajectories with Gaussian noise around previous optimal, - // scores via path-align + obstacle + goal critics, softmax-weights. - Twist compute(const Pose2D& pose, - const std::vector& path, - const GridMap& map); - -private: - // ── Params ── - double ms_ = 1.5, mr_ = 0.25, sl_ = 0.6, cf_ = 6.0; - - // ── MPPI settings ── - static constexpr int K = 200; // trajectory count - static constexpr int T = 15; // timesteps - static constexpr double DT = 0.1; // step duration (1.5s horizon) - static constexpr double TEMPERATURE = 0.5; - static constexpr double VX_STD = 0.2; - static constexpr double WZ_STD = 0.8; - - // ── Cost weights (EXACT-MPPI critic ratios) ── - static constexpr double W_ALIGN = 10.0; // PathAlignCritic - static constexpr double W_FOLLOW = 5.0; // PathFollowCritic - static constexpr double W_GOAL = 5.0; // GoalCritic - static constexpr double W_OBS = 5.0; // ObstaclesCritic (proportional) - static constexpr double W_FORWARD = 3.0; // PreferForwardCritic (penalize reverse) - static constexpr double W_HEADING = 1.5; // heading-to-goal (keep low — path_align dominates) - static constexpr double W_SMOOTH = 0.5; // control smoothness - - // ── Warm-start ── - double prev_vx_ = 0.5, prev_wz_ = 0; - - // ── Simple RNG (XORshift + Box-Muller, no stdlib issues on ARM) ── - uint32_t rng_state_ = 123456789; - double randNormal(); // returns N(0,1) - - // ── Rollout one trajectory, return total cost ── - double evaluateTrajectory(const Pose2D& pose, double vx, double wz, - const std::vector& path, - const std::vector& path_dists, - double total_path_len, - const GridMap& map); - - static void computePathDistances(const std::vector& path, - std::vector& dists); -}; - -} // namespace car_nav_lite diff --git a/src/localizer.cpp b/src/localizer.cpp index c9fb6c5..799e569 100644 --- a/src/localizer.cpp +++ b/src/localizer.cpp @@ -121,16 +121,21 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map, bool is_horiz = (diff1 < diff2); if (is_horiz) { double expected = (mwy > 0) ? 2.5 : -2.5; + if (use_fence_) expected = (mwy > fence_.cy) ? fence_.top_y : fence_.bottom_y; sum_dy += (expected - mwy); } else { double expected = (mwx > 0) ? 2.5 : -2.5; + if (use_fence_) expected = (mwx > fence_.cx) ? fence_.right_x : fence_.left_x; sum_dx += (expected - mwx); } } // ── 5. Update offset ── + // Store metrics + loc_lines_ = line_count; if (line_count >= 2) { double dx = sum_dx / line_count, dy = sum_dy / line_count; + loc_dx_ = dx; loc_dy_ = dy; loc_dyaw_ = -dyaw; 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; diff --git a/src/localizer.hpp b/src/localizer.hpp deleted file mode 100644 index 1729674..0000000 --- a/src/localizer.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include "car_nav_lite/types.hpp" - -namespace car_nav_lite { - -class GridMap; - -class Localizer { -public: - Localizer(); - - void updateOdom(double vx, double wz, double dt); - void updateIMU(double gyro_z, double dt); - void setOdomPose(const Pose2D& p); - - void correctWithScanCV(const LaserScan& scan, const GridMap& map, - const Pose2D* base_override = nullptr); - - Pose2D pose() const; - -private: - Pose2D odom_pose_; - Pose2D correction_; - double yaw_imu_ = 0.0, yaw_odom_ = 0.0; -}; - -} // namespace car_nav_lite diff --git a/src/main.cpp b/src/main.cpp index d18b74c..be1bf91 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,6 +19,8 @@ #include "car_nav_lite/grid_map.hpp" #include "car_nav_lite/localizer.hpp" #include "car_nav_lite/bt_executor.hpp" +#include "car_nav_lite/fence_model.hpp" +#include "car_nav_lite/session_mapper.hpp" using namespace std::chrono_literals; using namespace car_nav_lite; @@ -71,6 +73,7 @@ public: pub_path_ = create_publisher("/global_path", 10); pub_map_ = create_publisher("/map", rclcpp::QoS(1).transient_local()); pub_status_ = create_publisher("/nav_status", 10); + pub_metrics_ = create_publisher("/nav_metrics", 10); bt_.setFailCallback([this](){ std_msgs::msg::String msg; msg.data = "fail"; pub_status_->publish(msg); @@ -84,7 +87,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)); + metrics_timer_ = create_wall_timer(200ms, std::bind(&NavLiteNode::publish_metrics, 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. " @@ -117,6 +120,12 @@ private: rclcpp::Time last_odom_t_{0,0,RCL_ROS_TIME}; rclcpp::Time last_imu_t_{0,0,RCL_ROS_TIME}; GridMap map_; Localizer localizer_; BTExecutor bt_; + SessionMapper session_mapper_; + FenceModel fence_model_; + enum SessionState { CALIBRATING, READY }; + SessionState session_state_ = CALIBRATING; + int calib_scan_count_ = 0; + static constexpr int CALIB_SCANS_NEEDED = 30; rclcpp::Subscription::SharedPtr sub_scan_; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_imu_; @@ -125,8 +134,9 @@ private: rclcpp::Publisher::SharedPtr pub_path_; rclcpp::Publisher::SharedPtr pub_map_; rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_metrics_; std::shared_ptr tf_broadcaster_; - rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_, morph_timer_; + rclcpp::TimerBase::SharedPtr timer_, map_timer_, metrics_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; @@ -134,8 +144,26 @@ private: 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(); - // Trigger correction immediately (eliminate timer delay) - localizer_.correctWithScanCV(latest_scan_, map_, &scan_pose_snapshot_);} + + if (session_state_ == CALIBRATING && calib_scan_count_ < CALIB_SCANS_NEEDED) { + session_mapper_.addScan(latest_scan_, localizer_.pose()); + if (++calib_scan_count_ >= CALIB_SCANS_NEEDED) { + std::vector cones; + if (session_mapper_.build(fence_model_, cones, 5.0, 0.35, 0.04)) { + localizer_.setFenceModel(fence_model_); + for (auto& c : cones) + map_.markCircle(c.x(), c.y(), 0.18, 255); + session_state_ = READY; + fprintf(stderr, "[Session] READY — fence %.2fx%.2f, %zu cones\n", + fence_model_.width, fence_model_.height, cones.size()); + } else { + calib_scan_count_ = 0; // retry + fprintf(stderr, "[Session] calibration failed, retrying...\n"); + } + } + } else { + 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. @@ -238,7 +266,28 @@ private: for(size_t i=0;i128)?100:-1);} pub_map_->publish(m);} - void correct_localization(){} // now triggered from scan_cb directly + void publish_metrics(){ + std::lock_guard lk(mtx_); + NavMetrics m; + auto pose = localizer_.pose(); + m.pose_x = pose.x; m.pose_y = pose.y; m.pose_yaw = pose.yaw; + m.loc_lines = localizer_.loc_lines_; + m.loc_dx = localizer_.loc_dx_; m.loc_dy = localizer_.loc_dy_; m.loc_dyaw = localizer_.loc_dyaw_; + bt_.fillMetrics(m); + static const char* states[] = {"IDLE","PLANNING","TRACKING","ARRIVED","FAILED"}; + auto s = bt_.state(); int si=(int)s; m.state = (si>=0&&si<5)?states[si]:"?"; + + char buf[512]; + snprintf(buf, sizeof(buf), + "{\"state\":\"%s\",\"x\":%.2f,\"y\":%.2f,\"yaw\":%.1f," + "\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f," + "\"mppi_alive\":%d,\"mppi_total\":%d,\"mppi_minC\":%.1f}", + m.state, m.pose_x, m.pose_y, m.pose_yaw*57.3, + m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3, + m.mppi_alive, m.mppi_total, m.mppi_min_cost); + std_msgs::msg::String msg; msg.data = buf; + pub_metrics_->publish(msg); + } void morph_close(){std::lock_guard lk(mtx_); map_.morphologyClose(3);} diff --git a/src/nav_lite.launch.py b/src/nav_lite.launch.py deleted file mode 100644 index 8561509..0000000 --- a/src/nav_lite.launch.py +++ /dev/null @@ -1,31 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument("map_file", default_value="/home/cyy/sim_map.pgm"), - DeclareLaunchArgument("lookahead", default_value="0.8"), - DeclareLaunchArgument("test_mode", default_value="false"), - - # Static TF: base_link → laser - Node( - package="tf2_ros", executable="static_transform_publisher", - name="tf_laser", - arguments=["0","0","0.12","0","0","0","base_link","laser"], - ), - - # Main navigation node - Node( - package="car_nav_lite", - executable="nav_lite_node", - name="nav_lite_node", - output="screen", - parameters=[{ - "map_file": LaunchConfiguration("map_file"), - "lookahead": LaunchConfiguration("lookahead"), - "test_mode": LaunchConfiguration("test_mode"), - }], - ), - ]) diff --git a/src/session_mapper.cpp b/src/session_mapper.cpp new file mode 100644 index 0000000..3eba6cf --- /dev/null +++ b/src/session_mapper.cpp @@ -0,0 +1,184 @@ +#include "car_nav_lite/session_mapper.hpp" +#include +#include + +namespace car_nav_lite { + +SessionMapper::SessionMapper() {} + +void SessionMapper::addScan(const LaserScan& scan, const Pose2D& pose, int step) { + for (int i = 0; i < scan.size(); i += step) { + double r = scan.ranges[i]; + if (r < 0.15 || r > 4.8) continue; + double a = scan.angle_min + i * scan.angle_increment + pose.yaw; + double x = pose.x + r * std::cos(a); + double y = pose.y + r * std::sin(a); + points_world_.emplace_back(x, y); + } + ++scan_count_; +} + +bool SessionMapper::build(FenceModel& fence, std::vector& cones, + double expected_size, double size_tol, + double ransac_thresh) { + if (points_world_.size() < 200) { + fprintf(stderr, "[SessionMapper] too few points: %zu\n", points_world_.size()); + return false; + } + + auto& pts = points_world_; + size_t n = pts.size(); + + // ── 1. Estimate yaw_field via RANSAC line fitting ── + // Try multiple random line fits, find dominant direction + double best_yaw = 0; + int best_inliers = 0; + int trials = std::min(200, (int)(n / 2)); + + for (int t = 0; t < trials; ++t) { + int i1 = rand() % n, i2 = rand() % n; + if (i1 == i2) continue; + double dx = pts[i2].x() - pts[i1].x(); + double dy = pts[i2].y() - pts[i1].y(); + double len = std::sqrt(dx*dx + dy*dy); + if (len < 0.5) continue; // need long enough line for fence + + // Line normal + double nx = -dy / len, ny = dx / len; + double d = nx * pts[i1].x() + ny * pts[i1].y(); + + // Count inliers + int inliers = 0; + for (size_t k = 0; k < n; ++k) { + double dist = std::abs(nx * pts[k].x() + ny * pts[k].y() - d); + if (dist < ransac_thresh) ++inliers; + } + + if (inliers > best_inliers) { + best_inliers = inliers; + best_yaw = std::atan2(dy, dx); // line direction angle + } + } + + if (best_inliers < 30) { + fprintf(stderr, "[SessionMapper] RANSAC failed, max inliers=%d\n", best_inliers); + return false; + } + + // Normalize yaw to [0, π/2) — fence walls are perpendicular + while (best_yaw >= M_PI/2) best_yaw -= M_PI/2; + while (best_yaw < 0) best_yaw += M_PI/2; + fence.yaw_field = best_yaw; + + // ── 2. Rotate points to field-aligned coordinates ── + double cos_y = std::cos(-best_yaw), sin_y = std::sin(-best_yaw); + std::vector xs, ys; + xs.reserve(n); ys.reserve(n); + for (auto& p : pts) { + double rx = cos_y * p.x() - sin_y * p.y(); + double ry = sin_y * p.x() + cos_y * p.y(); + xs.push_back(rx); ys.push_back(ry); + } + + // ── 3. Percentile-based boundary estimation ── + std::sort(xs.begin(), xs.end()); + std::sort(ys.begin(), ys.end()); + int lo_idx = (int)(n * 0.02); + int hi_idx = (int)(n * 0.98); + double x_min = xs[lo_idx], x_max = xs[hi_idx]; + double y_min = ys[lo_idx], y_max = ys[hi_idx]; + double w = x_max - x_min, h = y_max - y_min; + + // Validate size + if (std::abs(w - expected_size) > size_tol || + std::abs(h - expected_size) > size_tol) { + fprintf(stderr, "[SessionMapper] size mismatch: %.2fx%.2f (expected %.1f±%.1f)\n", + w, h, expected_size, size_tol); + return false; + } + + fence.width = w; fence.height = h; + fence.cx = (x_min + x_max) / 2.0; + fence.cy = (y_min + y_max) / 2.0; + + // ── 4. Build fence lines + wall positions in world coords ── + double cos_f = std::cos(best_yaw), sin_f = std::sin(best_yaw); + auto rotBack = [&](double fx, double fy) -> Eigen::Vector2d { + return {cos_f * fx - sin_f * fy, sin_f * fx + cos_f * fy}; + }; + + // Wall positions for localization + Eigen::Vector2d cp = rotBack(fence.cx, fence.cy); + Eigen::Vector2d lw = rotBack(x_min, fence.cy); + Eigen::Vector2d rw = rotBack(x_max, fence.cy); + Eigen::Vector2d bw = rotBack(fence.cx, y_min); + Eigen::Vector2d tw = rotBack(fence.cx, y_max); + fence.left_x = lw.x(); fence.right_x = rw.x(); + fence.bottom_y = bw.y(); fence.top_y = tw.y(); + + // Left wall: field-x = x_min, direction = field-y (0, 1) + Eigen::Vector2d lb = rotBack(x_min, 0), lt = rotBack(x_min, h); + fence.left.setFromTwoPoints(lb.x(), lb.y(), lt.x(), lt.y()); + + // Right wall + Eigen::Vector2d rb = rotBack(x_max, 0), rt = rotBack(x_max, h); + fence.right.setFromTwoPoints(rb.x(), rb.y(), rt.x(), rt.y()); + + // Bottom wall: field-y = y_min + Eigen::Vector2d bb = rotBack(0, y_min), br = rotBack(w, y_min); + fence.bottom.setFromTwoPoints(bb.x(), bb.y(), br.x(), br.y()); + + // Top wall + Eigen::Vector2d tb = rotBack(0, y_max), tr_ = rotBack(w, y_max); + fence.top.setFromTwoPoints(tb.x(), tb.y(), tr_.x(), tr_.y()); + + fence.valid = true; + + // ── 5. Extract cones: non-wall points → cluster ── + cones.clear(); + constexpr double WALL_DIST = 0.08; + std::vector non_wall; + for (auto& p : pts) { + double d_left = fence.left.distance(p.x(), p.y()); + double d_right = fence.right.distance(p.x(), p.y()); + double d_bottom = fence.bottom.distance(p.x(), p.y()); + double d_top = fence.top.distance(p.x(), p.y()); + double min_d = std::min({d_left, d_right, d_bottom, d_top}); + if (min_d > WALL_DIST) non_wall.push_back(p); + } + + // Simple grid clustering + constexpr double CELL = 0.10; // 10cm grid + std::vector> clusters; + std::vector used(non_wall.size(), false); + for (size_t i = 0; i < non_wall.size(); ++i) { + if (used[i]) continue; + std::vector cluster; + cluster.push_back(non_wall[i]); + used[i] = true; + // Expand cluster + for (size_t j = 0; j < cluster.size(); ++j) { + for (size_t k = i + 1; k < non_wall.size(); ++k) { + if (used[k]) continue; + double dx = non_wall[k].x() - cluster[j].x(); + double dy = non_wall[k].y() - cluster[j].y(); + if (dx*dx + dy*dy < CELL*CELL) { + cluster.push_back(non_wall[k]); + used[k] = true; + } + } + } + if (cluster.size() >= 4) { + Eigen::Vector2d center(0, 0); + for (auto& c : cluster) { center.x() += c.x(); center.y() += c.y(); } + center /= (double)cluster.size(); + cones.push_back(center); + } + } + + fprintf(stderr, "[SessionMapper] fence %.2fx%.2f yaw=%.1f°, %zu cones, %zu non-wall pts\n", + w, h, best_yaw*57.3, cones.size(), non_wall.size()); + return true; +} + +} // namespace car_nav_lite diff --git a/src/types.hpp b/src/types.hpp deleted file mode 100644 index 5b9aa0d..0000000 --- a/src/types.hpp +++ /dev/null @@ -1,83 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace car_nav_lite { - -// ── 2D Pose ───────────────────────────────────────────── -struct Pose2D { - double x = 0.0, y = 0.0, yaw = 0.0; - Pose2D() = default; - Pose2D(double x_, double y_, double yaw_) : x(x_), y(y_), yaw(yaw_) {} - double distTo(const Pose2D& o) const { - double dx = x - o.x, dy = y - o.y; - return std::sqrt(dx*dx + dy*dy); - } - double angleTo(const Pose2D& o) const { - return std::atan2(o.y - y, o.x - x); - } -}; - -// ── Twist command ─────────────────────────────────────── -struct Twist { - double vx = 0.0, wz = 0.0; -}; - -// ── LaserScan (deserialized from /scan HTTP JSON) ─────── -struct LaserScan { - double angle_min = 0.0, angle_max = 0.0, angle_increment = 0.0; - double range_min = 0.0, range_max = 0.0; - std::vector ranges; - int size() const { return (int)ranges.size(); } -}; - -// ── Grid cell index ───────────────────────────────────── -struct GridIndex { - int x = 0, y = 0; - GridIndex() = default; - GridIndex(int x_, int y_) : x(x_), y(y_) {} - bool operator==(const GridIndex& o) const { return x == o.x && y == o.y; } -}; - -// ── A* Node ───────────────────────────────────────────── -struct AStarNode { - int x, y; - int g = 0, h = 0, f = 0; - AStarNode* parent = nullptr; -}; - -// ── State enum ────────────────────────────────────────── -enum class NavState { IDLE, PLANNING, TRACKING, ARRIVED, FAILED }; - -// ── Binary occupancy grid (0-255) ─────────────────────── -using OccupancyGrid = std::vector; - -// ── Grid map parameters ───────────────────────────────── -constexpr int GRID_SIZE = 1008; // cells (5.04m @ 5mm) -constexpr double GRID_RES = 0.005; // m/cell (5mm) -constexpr double GRID_ORIGIN_X = -2.52; // world x at grid[0] (buffer for 2.5 edge) -constexpr double GRID_ORIGIN_Y = -2.52; // world y at grid[0] -constexpr int INFLATION_CELLS = 30; // 15cm (covers car half-diagonal + margin for A*) - -// ── Vehicle parameters ────────────────────────────────── -constexpr double WHEELBASE = 0.1682; // m (front-rear axle) -constexpr double MIN_TURN_R = 0.25; // m minimum turning radius -constexpr double DEFAULT_LOOKAHEAD = 1.2; // m Pure Pursuit lookahead - -// ── Car footprint (origincar: 30cm×19cm) ────────────── -constexpr double CAR_HALF_L = 0.15; // half-length -constexpr double CAR_HALF_W = 0.095; // half-width -constexpr double CAR_HALF_DIAG = 0.178; // sqrt(L²+W²) - -// ── Planner limits ────────────────────────────────────── -constexpr double MAX_SPEED = 4.0; // m/s -constexpr double MIN_SPEED = 0.5; // m/s (minimum for Ackermann turning) - -// ── Helper ────────────────────────────────────────────── -inline double limit(double v, double lo, double hi) { - return v < lo ? lo : (v > hi ? hi : v); -} - -} // namespace car_nav_lite diff --git a/src/waypoint_nav.py b/src/waypoint_nav.py deleted file mode 100644 index 34f42af..0000000 --- a/src/waypoint_nav.py +++ /dev/null @@ -1,145 +0,0 @@ -#!/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()