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

Step 2: FenceModel calibration replaces hardcoded ±2.5m

Browse Source 
- fence_model.hpp: Line2D + FenceModel with wall positions
- session_mapper: RANSAC yaw fit + percentile boundary + cone clustering
- localizer: setFenceModel(), use calibrated walls in correctWithScanCV
- main.cpp: CALIBRATING→READY state machine (30 scan frames)
- /nav_metrics publisher (5Hz JSON metrics)
- mppi_alive/min_cost exposed via LocalPlanner public members
- endpoint-only path_align (EXACT-MPPI PathFollowCritic style)
- adaptive exploration ratio based on survival rate
This commit is contained in:
cyy
2026-07-03 06:56:28 +00:00
parent 85d6984bba
commit 6085ff7383
18 changed files with 356 additions and 455 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

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

5
include/car_nav_lite/bt_executor.hpp
View File

@@ -18,6 +18,11 @@ public:
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); }
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);

43
include/car_nav_lite/fence_model.hpp Normal file
View File

@@ -0,0 +1,43 @@
#pragma once
#include <vector>
#include <cmath>
#include <Eigen/Dense>
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

4
include/car_nav_lite/local_planner.hpp
View File

@@ -18,6 +18,10 @@ public:
const std::vector<Pose2D>& 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;

8
include/car_nav_lite/localizer.hpp
View File

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

31
include/car_nav_lite/session_mapper.hpp Normal file
View File

@@ -0,0 +1,31 @@
#pragma once
#include <vector>
#include <Eigen/Dense>
#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<Eigen::Vector2d>& 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<Eigen::Vector2d>& points() const { return points_world_; }
private:
std::vector<Eigen::Vector2d> points_world_;
int scan_count_ = 0;
};
} // namespace car_nav_lite

20
include/car_nav_lite/types.hpp
View File

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

38
src/bt_executor.hpp
View File

@@ -1,38 +0,0 @@
#pragma once
#include <optional>
#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<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);
NavState state() const { return state_; }
const std::vector<Pose2D>& currentPath() const { return current_path_; }
private:
NavState state_ = NavState::IDLE;
std::optional<Pose2D> goal_;
std::vector<Pose2D> current_path_;
GlobalPlanner planner_;
LocalPlanner local_planner_;
std::function<void()> fail_callback_;
bool atGoal(const Pose2D& pose) const;
};
} // namespace car_nav_lite

65
src/grid_map.hpp
View File

@@ -1,65 +0,0 @@
#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);
// ── 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

1
src/local_planner.cpp
View File

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

60
src/local_planner.hpp
View File

@@ -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<Pose2D>& 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<Pose2D>& path,
const std::vector<double>& path_dists,
double total_path_len,
const GridMap& map);
static void computePathDistances(const std::vector<Pose2D>& path,
std::vector<double>& dists);
};
} // namespace car_nav_lite

5
src/localizer.cpp
View File

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

28
src/localizer.hpp
View File

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

59
src/main.cpp
View File

@@ -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<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);
pub_metrics_ = create_publisher<std_msgs::msg::String>("/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<LaserScanMsg>::SharedPtr sub_scan_;
rclcpp::Subscription<OdometryMsg>::SharedPtr sub_odom_;
rclcpp::Subscription<ImuMsg>::SharedPtr sub_imu_;
@@ -125,8 +134,9 @@ private:
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_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_metrics_;
std::shared_ptr<tf2_ros::TransformBroadcaster> 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<Eigen::Vector2d> 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;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(){} // 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);}

31
src/nav_lite.launch.py
View File

@@ -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"),
}],
),
])

184
src/session_mapper.cpp Normal file
View File

@@ -0,0 +1,184 @@
#include "car_nav_lite/session_mapper.hpp"
#include <algorithm>
#include <cstdio>
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<Eigen::Vector2d>& 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<double> 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<Eigen::Vector2d> 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<std::vector<Eigen::Vector2d>> clusters;
std::vector<bool> used(non_wall.size(), false);
for (size_t i = 0; i < non_wall.size(); ++i) {
if (used[i]) continue;
std::vector<Eigen::Vector2d> 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

83
src/types.hpp
View File

@@ -1,83 +0,0 @@
#pragma once
#include <cmath>
#include <vector>
#include <cstdint>
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<float> 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<uint8_t>;
// ── 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

145
src/waypoint_nav.py
View File

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