Step 2: FenceModel calibration replaces hardcoded ±2.5m
- 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
@@ -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
|
||||
@@ -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
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
@@ -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;
|
||||
|
||||
@@ -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
59
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<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);}
|
||||
|
||||
|
||||
@@ -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
184
src/session_mapper.cpp
Normal 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
|
||||
@@ -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
|
||||
@@ -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()
|
||||
Reference in New Issue
Block a user