MPPI planner + fence-line localization + scan obstacles
- Lightweight MPPI (200 trajectories, 15 steps, endpoint path_align) - adaptive exploration (more random when stuck) - fence-line Hough detection + geometric correction - scan hit points marked at 255 immediately - forward half-step pre-check for obstacle avoidance - Ackermann constraint |wz| <= |vx|/min_turn_r - XORshift RNG (stdlib broken on ARM)
This commit is contained in:
cyy
91
src/bt_executor.cpp
Normal file
91
src/bt_executor.cpp
Normal file
@@ -0,0 +1,91 @@
|
||||
#include "car_nav_lite/bt_executor.hpp"
|
||||
|
||||
namespace car_nav_lite {
|
||||
|
||||
BTExecutor::BTExecutor() {}
|
||||
|
||||
void BTExecutor::setParams(double la, double ms, double mr, double sl, double cf) {
|
||||
local_planner_.setParams(la, ms, mr, sl, cf);
|
||||
}
|
||||
|
||||
void BTExecutor::setTestPath(const std::vector<Pose2D>& path) {
|
||||
current_path_ = path;
|
||||
if (!path.empty()) goal_ = path.back();
|
||||
state_ = NavState::TRACKING;
|
||||
}
|
||||
|
||||
void BTExecutor::setGoal(const Pose2D& goal) {
|
||||
if (goal_ && goal_->distTo(goal) < 0.05) return;
|
||||
goal_ = goal;
|
||||
state_ = NavState::PLANNING;
|
||||
}
|
||||
|
||||
Twist BTExecutor::tick(const Pose2D& pose, const GridMap& map) {
|
||||
Twist cmd{};
|
||||
|
||||
switch (state_) {
|
||||
case NavState::IDLE:
|
||||
break;
|
||||
|
||||
case NavState::PLANNING:
|
||||
if (goal_) {
|
||||
current_path_ = planner_.plan(pose, *goal_, map);
|
||||
if (!current_path_.empty()) {
|
||||
current_path_.back().yaw = goal_->yaw;
|
||||
fprintf(stderr, "[BT] Plan OK, %zu waypoints\n", current_path_.size());
|
||||
state_ = NavState::TRACKING;
|
||||
} else {
|
||||
fprintf(stderr, "[BT] Plan FAILED to (%.2f,%.2f)\n",
|
||||
goal_->x, goal_->y);
|
||||
state_ = NavState::FAILED;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case NavState::TRACKING: {
|
||||
if (atGoal(pose)) {
|
||||
fprintf(stderr, "[BT] Goal reached!\n");
|
||||
state_ = NavState::ARRIVED;
|
||||
break;
|
||||
}
|
||||
|
||||
// Periodic replan (4Hz) with progressive inflation reduction
|
||||
static int tick_cnt = 0;
|
||||
if (++tick_cnt % 5 == 0 && goal_) {
|
||||
static const int inflations[] = {
|
||||
INFLATION_CELLS, INFLATION_CELLS*4/5,
|
||||
(int)(CAR_HALF_W / GRID_RES)
|
||||
};
|
||||
std::vector<Pose2D> new_path;
|
||||
for (int inflate : inflations) {
|
||||
new_path = planner_.plan(pose, *goal_, map, inflate);
|
||||
if (!new_path.empty()) break;
|
||||
}
|
||||
if (!new_path.empty()) current_path_ = std::move(new_path);
|
||||
}
|
||||
|
||||
cmd = local_planner_.compute(pose, current_path_, map);
|
||||
break;
|
||||
}
|
||||
|
||||
case NavState::ARRIVED:
|
||||
case NavState::FAILED:
|
||||
break;
|
||||
}
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
bool BTExecutor::atGoal(const Pose2D& pose) const {
|
||||
if (!goal_) return false;
|
||||
if (pose.distTo(*goal_) > 0.20) return false;
|
||||
if (std::abs(goal_->yaw) > 0.01) {
|
||||
double h_err = goal_->yaw - pose.yaw;
|
||||
while (h_err > M_PI) h_err -= 2*M_PI;
|
||||
while (h_err < -M_PI) h_err += 2*M_PI;
|
||||
return std::abs(h_err) < 0.15;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace car_nav_lite
|
||||
45
src/bt_executor.hpp
Normal file
45
src/bt_executor.hpp
Normal file
@@ -0,0 +1,45 @@
|
||||
#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();
|
||||
|
||||
// ── Set navigation goal ────────────────────────────
|
||||
void setGoal(const Pose2D& goal);
|
||||
void setTestPath(const std::vector<Pose2D>& path);
|
||||
void setParams(double la, double ms, double mr, double sl, double cf);
|
||||
bool hasGoal() const { return goal_.has_value(); }
|
||||
|
||||
// ── Tick the state machine (call at 10Hz) ──────────
|
||||
Twist tick(const Pose2D& pose, const GridMap& map);
|
||||
|
||||
// ── Status query ───────────────────────────────────
|
||||
NavState state() const { return state_; }
|
||||
const std::vector<Pose2D>& currentPath() const { return current_path_; }
|
||||
bool hasGoal() const { return goal_.has_value(); }
|
||||
|
||||
private:
|
||||
NavState state_ = NavState::IDLE;
|
||||
std::optional<Pose2D> goal_;
|
||||
std::vector<Pose2D> current_path_;
|
||||
GlobalPlanner planner_;
|
||||
LocalPlanner local_planner_;
|
||||
int replan_count_ = 0;
|
||||
int stuck_count_ = 0;
|
||||
int recovery_attempts_ = 0;
|
||||
Pose2D recovery_target_;
|
||||
|
||||
bool atGoal(const Pose2D& pose) const;
|
||||
bool tryRecover(const Pose2D& pose, const GridMap& map);
|
||||
};
|
||||
|
||||
} // namespace car_nav_lite
|
||||
51
src/calib_check.cpp
Normal file
51
src/calib_check.cpp
Normal file
@@ -0,0 +1,51 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <memory>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using Odometry = nav_msgs::msg::Odometry;
|
||||
|
||||
class CalibCheck : public rclcpp::Node {
|
||||
public:
|
||||
CalibCheck() : Node("calib_check"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) {
|
||||
sub_odom_ = create_subscription<Odometry>("/odom", 10,
|
||||
std::bind(&CalibCheck::odom_cb, this, std::placeholders::_1));
|
||||
timer_ = create_wall_timer(500ms, std::bind(&CalibCheck::print, this));
|
||||
RCLCPP_INFO(get_logger(), "calib_check: /odom vs map->base_link vs odom_combined->base_link");
|
||||
printf("time | odom_x odom_y odom_yaw | map_x map_y map_yaw | e_x e_y\n");
|
||||
printf("-------+-------------------------+------------------------+---------\n");
|
||||
}
|
||||
private:
|
||||
void odom_cb(Odometry::SharedPtr m) {
|
||||
odom_x_ = m->pose.pose.position.x;
|
||||
odom_y_ = m->pose.pose.position.y;
|
||||
double qw=m->pose.pose.orientation.w, qz=m->pose.pose.orientation.z;
|
||||
odom_yaw_ = std::atan2(2.0*qw*qz, qw*qw - qz*qz);
|
||||
}
|
||||
void print() {
|
||||
// TF lookups
|
||||
double mx=0,my=0,myaw=0;
|
||||
bool has_m=false;
|
||||
try {
|
||||
auto t = tf_buffer_.lookupTransform("map","base_link", tf2::TimePointZero);
|
||||
mx=t.transform.translation.x; my=t.transform.translation.y;
|
||||
double qz=t.transform.rotation.z, qw=t.transform.rotation.w;
|
||||
myaw=std::atan2(2.0*qw*qz, qw*qw-qz*qz); has_m=true;
|
||||
} catch(...) {}
|
||||
if (!has_m) { printf("no map tf\n"); return; }
|
||||
double ex=mx-odom_x_, ey=my-odom_y_;
|
||||
printf("%6.1f | %7.2f %7.2f %7.1f | %6.2f %6.2f %7.1f | %+.2f %+.2f\n",
|
||||
now().seconds(), odom_x_,odom_y_,odom_yaw_*57.3, mx,my,myaw*57.3, ex,ey);
|
||||
}
|
||||
rclcpp::Subscription<Odometry>::SharedPtr sub_odom_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
double odom_x_=0,odom_y_=0,odom_yaw_=0;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[]) { rclcpp::init(argc,argv); rclcpp::spin(std::make_shared<CalibCheck>()); rclcpp::shutdown(); return 0; }
|
||||
126
src/global_planner.cpp
Normal file
126
src/global_planner.cpp
Normal file
@@ -0,0 +1,126 @@
|
||||
#include "car_nav_lite/global_planner.hpp"
|
||||
#include <memory>
|
||||
#include <functional>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
|
||||
namespace car_nav_lite {
|
||||
|
||||
GlobalPlanner::GlobalPlanner() {}
|
||||
|
||||
std::vector<Pose2D> GlobalPlanner::plan(const Pose2D& start, const Pose2D& goal,
|
||||
const GridMap& map, int inflate) {
|
||||
int sx, sy, gx, gy;
|
||||
int inflate_cells = (inflate < 0) ? INFLATION_CELLS : inflate;
|
||||
// Clamp to valid grid range
|
||||
auto clampGrid = [&](double wx, double wy, int& gx, int& gy) {
|
||||
gx = std::clamp((int)((wx - GRID_ORIGIN_X) / GRID_RES), 0, GRID_SIZE - 1);
|
||||
gy = std::clamp((int)((wy - GRID_ORIGIN_Y) / GRID_RES), 0, GRID_SIZE - 1);
|
||||
};
|
||||
clampGrid(start.x, start.y, sx, sy);
|
||||
clampGrid(goal.x, goal.y, gx, gy);
|
||||
|
||||
static constexpr int SZ = GRID_SIZE;
|
||||
auto closed = std::unique_ptr<bool[]>(new bool[SZ * SZ]());
|
||||
|
||||
using NodePtr = AStarNode*;
|
||||
auto cmp = [](NodePtr a, NodePtr b) { return a->f > b->f; };
|
||||
std::priority_queue<NodePtr, std::vector<NodePtr>, decltype(cmp)> open(cmp);
|
||||
|
||||
AStarNode start_node{sx, sy, 0, euclidean(sx, sy, gx, gy), 0, nullptr};
|
||||
start_node.f = start_node.g + start_node.h;
|
||||
AStarNode* start_ptr = new AStarNode(start_node);
|
||||
open.push(start_ptr);
|
||||
|
||||
NodePtr goal_node = nullptr;
|
||||
|
||||
while (!open.empty()) {
|
||||
NodePtr cur = open.top(); open.pop();
|
||||
int idx = cur->y * SZ + cur->x;
|
||||
|
||||
if (closed[idx]) { delete cur; continue; }
|
||||
closed[idx] = true;
|
||||
|
||||
if (cur->x == gx && cur->y == gy) { goal_node = cur; break; }
|
||||
|
||||
for (auto [dx, dy] : NEIGHBORS) {
|
||||
int nx = cur->x + dx, ny = cur->y + dy;
|
||||
if (!map.inBounds(nx, ny)) continue;
|
||||
int nidx = ny * SZ + nx;
|
||||
if (closed[nidx]) continue;
|
||||
if (!map.isFree(nx, ny, inflate_cells)) continue;
|
||||
|
||||
int ng = cur->g + moveCost(cur->x, cur->y, nx, ny);
|
||||
int nh = euclidean(nx, ny, gx, gy);
|
||||
auto* nb = new AStarNode{nx, ny, ng, nh, ng + nh, cur};
|
||||
open.push(nb);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<Pose2D> path;
|
||||
if (!goal_node) {
|
||||
fprintf(stderr, "[A*] No path from (%d,%d) to (%d,%d)\n", sx, sy, gx, gy);
|
||||
while (!open.empty()) { delete open.top(); open.pop(); }
|
||||
return path;
|
||||
}
|
||||
|
||||
std::vector<GridIndex> gpath;
|
||||
for (NodePtr n = goal_node; n; n = n->parent) gpath.emplace_back(n->x, n->y);
|
||||
std::reverse(gpath.begin(), gpath.end());
|
||||
|
||||
for (auto& gi : gpath) {
|
||||
double wx, wy; map.gridToWorld(gi.x, gi.y, wx, wy);
|
||||
path.emplace_back(wx, wy, 0.0);
|
||||
}
|
||||
|
||||
for (NodePtr n = goal_node; n;) { NodePtr p = n->parent; delete n; n = p; }
|
||||
while (!open.empty()) { delete open.top(); open.pop(); }
|
||||
|
||||
// Interpolate sparse path (add points every 0.3m)
|
||||
std::vector<Pose2D> dense;
|
||||
for (size_t i = 0; i + 1 < path.size(); ++i) {
|
||||
dense.push_back(path[i]);
|
||||
double d = path[i].distTo(path[i+1]);
|
||||
int steps = (int)(d / 0.3);
|
||||
for (int s = 1; s < steps; ++s) {
|
||||
double t = (double)s / steps;
|
||||
dense.emplace_back(
|
||||
path[i].x + t*(path[i+1].x - path[i].x),
|
||||
path[i].y + t*(path[i+1].y - path[i].y),
|
||||
0.0);
|
||||
}
|
||||
}
|
||||
dense.push_back(path.back());
|
||||
// Don't re-smooth; interpolated path is already good
|
||||
for (size_t i = 0; i + 1 < dense.size(); ++i)
|
||||
dense[i].yaw = dense[i].angleTo(dense[i+1]);
|
||||
if (!dense.empty()) dense.back().yaw = dense[dense.size()-2].yaw;
|
||||
return dense;
|
||||
}
|
||||
|
||||
std::vector<Pose2D> GlobalPlanner::smooth(const std::vector<Pose2D>& raw,
|
||||
const GridMap& map) {
|
||||
if (raw.size() <= 2) return raw;
|
||||
std::vector<Pose2D> out; out.push_back(raw[0]);
|
||||
size_t anchor = 0;
|
||||
for (size_t i = 2; i < raw.size(); ++i) {
|
||||
int ax, ay, ix, iy;
|
||||
map.worldToGrid(raw[anchor].x, raw[anchor].y, ax, ay);
|
||||
map.worldToGrid(raw[i].x, raw[i].y, ix, iy);
|
||||
int dx = std::abs(ix-ax), dy = -std::abs(iy-ay);
|
||||
int sx_i = ax < ix ? 1 : -1, sy_i = ay < iy ? 1 : -1;
|
||||
int err = dx+dy, x=ax, y=ay;
|
||||
bool blocked = false;
|
||||
while (x!=ix || y!=iy) {
|
||||
if (!map.isFree(x,y)) { blocked=true; break; }
|
||||
int e2=2*err; if(e2>=dy){err+=dy;x+=sx_i;} if(e2<=dx){err+=dx;y+=sy_i;}
|
||||
}
|
||||
if (blocked) { out.push_back(raw[i-1]); anchor=i-1; }
|
||||
}
|
||||
out.push_back(raw.back());
|
||||
for (size_t i=0; i<out.size()-1; ++i) out[i].yaw = out[i].angleTo(out[i+1]);
|
||||
if (!out.empty()) out.back().yaw = out[out.size()-2].yaw;
|
||||
return out;
|
||||
}
|
||||
|
||||
} // namespace car_nav_lite
|
||||
247
src/grid_map.cpp
Normal file
247
src/grid_map.cpp
Normal file
@@ -0,0 +1,247 @@
|
||||
#include "car_nav_lite/grid_map.hpp"
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace car_nav_lite {
|
||||
|
||||
GridMap::GridMap() : cells_(GRID_SIZE * GRID_SIZE, 0) {}
|
||||
|
||||
bool GridMap::loadPGM(const std::string& path) {
|
||||
std::ifstream file(path, std::ios::binary);
|
||||
if (!file) return false;
|
||||
|
||||
std::string magic;
|
||||
file >> magic; // P5
|
||||
if (magic != "P5") return false;
|
||||
|
||||
int w, h, maxval;
|
||||
// Skip comments
|
||||
char c;
|
||||
while (file.peek() == '\n' || file.peek() == '#') {
|
||||
if (file.peek() == '#') {
|
||||
std::string line; std::getline(file, line);
|
||||
} else {
|
||||
file.get();
|
||||
}
|
||||
}
|
||||
file >> w >> h >> maxval;
|
||||
|
||||
if (w != GRID_SIZE || h != GRID_SIZE) {
|
||||
fprintf(stderr, "[GridMap] PGM size %dx%d != %dx%d, will crop/clamp\n",
|
||||
w, h, GRID_SIZE, GRID_SIZE);
|
||||
}
|
||||
file.get(); // consume whitespace before binary
|
||||
|
||||
std::vector<uint8_t> raw(w * h);
|
||||
file.read(reinterpret_cast<char*>(raw.data()), w * h);
|
||||
|
||||
// Copy into our grid, inverting: PGM 0=black(obstacle), 255=white(free)
|
||||
// We want 255=obstacle, 0=free
|
||||
for (int gy = 0; gy < std::min(h, GRID_SIZE); ++gy) {
|
||||
int src_row = h - 1 - gy; // PGM origin is top-left, we want bottom-left
|
||||
for (int gx = 0; gx < std::min(w, GRID_SIZE); ++gx) {
|
||||
uint8_t pgm = raw[src_row * w + gx];
|
||||
// Invert: black (0) -> obstacle (255), white (255) -> free (0)
|
||||
// Threshold at 128: <128 = occupied
|
||||
cells_[gy * GRID_SIZE + gx] = (pgm < 128) ? 255 : 0;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GridMap::worldToGrid(double wx, double wy, int& gx, int& gy) const {
|
||||
gx = (int)((wx - GRID_ORIGIN_X) / GRID_RES);
|
||||
gy = (int)((wy - GRID_ORIGIN_Y) / GRID_RES);
|
||||
return inBounds(gx, gy);
|
||||
}
|
||||
|
||||
void GridMap::gridToWorld(int gx, int gy, double& wx, double& wy) const {
|
||||
wx = GRID_ORIGIN_X + gx * GRID_RES + GRID_RES / 2.0;
|
||||
wy = GRID_ORIGIN_Y + gy * GRID_RES + GRID_RES / 2.0;
|
||||
}
|
||||
|
||||
uint8_t GridMap::at(int gx, int gy) const {
|
||||
if (!inBounds(gx, gy)) return 255; // out of bounds = obstacle
|
||||
return cells_[gy * GRID_SIZE + gx];
|
||||
}
|
||||
|
||||
void GridMap::set(int gx, int gy, uint8_t v) {
|
||||
if (!inBounds(gx, gy)) return;
|
||||
cells_[gy * GRID_SIZE + gx] = v;
|
||||
}
|
||||
|
||||
bool GridMap::inBounds(int gx, int gy) const {
|
||||
return gx >= 0 && gx < GRID_SIZE && gy >= 0 && gy < GRID_SIZE;
|
||||
}
|
||||
|
||||
bool GridMap::isFree(int gx, int gy) const {
|
||||
return isFree(gx, gy, INFLATION_CELLS);
|
||||
}
|
||||
|
||||
bool GridMap::isFree(int gx, int gy, int inflate_cells) const {
|
||||
if (!inBounds(gx, gy)) return false;
|
||||
for (int dy = -inflate_cells; dy <= inflate_cells; ++dy) {
|
||||
for (int dx = -inflate_cells; dx <= inflate_cells; ++dx) {
|
||||
int nx = gx + dx, ny = gy + dy;
|
||||
if (!inBounds(nx, ny)) continue;
|
||||
if (at(nx, ny) > 128) return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GridMap::isFreeFootprint(double wx, double wy, double yaw) const {
|
||||
// ── Oriented rectangle check (like EXACT-MPPI's polygon footprint) ──
|
||||
// Car = 30cm×19cm oriented at yaw. Check all occupied cells in AABB.
|
||||
double cos_y = std::cos(yaw), sin_y = std::sin(yaw);
|
||||
|
||||
// 4 corners of the oriented rectangle in world coords
|
||||
static constexpr double dx_c[4] = {CAR_HALF_L, CAR_HALF_L, -CAR_HALF_L, -CAR_HALF_L};
|
||||
static constexpr double dy_c[4] = {CAR_HALF_W, -CAR_HALF_W, CAR_HALF_W, -CAR_HALF_W};
|
||||
|
||||
double min_x = 1e9, max_x = -1e9, min_y = 1e9, max_y = -1e9;
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
double cx = wx + dx_c[i]*cos_y - dy_c[i]*sin_y;
|
||||
double cy = wy + dx_c[i]*sin_y + dy_c[i]*cos_y;
|
||||
if (cx < min_x) min_x = cx; if (cx > max_x) max_x = cx;
|
||||
if (cy < min_y) min_y = cy; if (cy > max_y) max_y = cy;
|
||||
}
|
||||
|
||||
// AABB in grid coords
|
||||
int gx_min, gy_min, gx_max, gy_max;
|
||||
if (!worldToGrid(min_x, min_y, gx_min, gy_min)) { gx_min = 0; gy_min = 0; }
|
||||
if (!worldToGrid(max_x, max_y, gx_max, gy_max)) { gx_max = GRID_SIZE-1; gy_max = GRID_SIZE-1; }
|
||||
gx_min = std::max(0, gx_min); gy_min = std::max(0, gy_min);
|
||||
gx_max = std::min(GRID_SIZE-1, gx_max); gy_max = std::min(GRID_SIZE-1, gy_max);
|
||||
|
||||
for (int gy = gy_min; gy <= gy_max; ++gy) {
|
||||
for (int gx = gx_min; gx <= gx_max; ++gx) {
|
||||
if (at(gx, gy) <= 128) continue; // free cell, skip
|
||||
|
||||
// Transform occupied cell center → car body frame
|
||||
double cwx, cwy;
|
||||
gridToWorld(gx, gy, cwx, cwy);
|
||||
double bx = (cwx - wx)*cos_y + (cwy - wy)*sin_y;
|
||||
double by = -(cwx - wx)*sin_y + (cwy - wy)*cos_y;
|
||||
|
||||
// Check if inside oriented rectangle
|
||||
if (std::abs(bx) <= CAR_HALF_L && std::abs(by) <= CAR_HALF_W)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) {
|
||||
int ox, oy;
|
||||
if (!worldToGrid(pose.x, pose.y, ox, oy)) return;
|
||||
|
||||
for (int i = 0; i < scan.size(); ++i) {
|
||||
double r = scan.ranges[i];
|
||||
if (r < scan.range_min || r > scan.range_max) continue;
|
||||
double angle = scan.angle_min + i * scan.angle_increment + pose.yaw;
|
||||
|
||||
int hx, hy;
|
||||
double hwx = pose.x + r * std::cos(angle);
|
||||
double hwy = pose.y + r * std::sin(angle);
|
||||
if (worldToGrid(hwx, hwy, hx, hy)) {
|
||||
set(hx, hy, 255); // hit → fully occupied immediately
|
||||
}
|
||||
|
||||
// Raytrace clear
|
||||
double clear_r = std::max(0.1, r - 0.1);
|
||||
int cx, cy;
|
||||
double cwx = pose.x + clear_r * std::cos(angle);
|
||||
double cwy = pose.y + clear_r * std::sin(angle);
|
||||
if (worldToGrid(cwx, cwy, cx, cy)) {
|
||||
bresenhamLine(ox, oy, cx, cy, 5); // clear → -occupied
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GridMap::bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val) {
|
||||
int dx = std::abs(gx1 - gx0), dy = -std::abs(gy1 - gy0);
|
||||
int sx = gx0 < gx1 ? 1 : -1, sy = gy0 < gy1 ? 1 : -1;
|
||||
int err = dx + dy;
|
||||
int x = gx0, y = gy0;
|
||||
while (true) {
|
||||
if (inBounds(x, y)) {
|
||||
uint8_t& v = cells_[y * GRID_SIZE + x];
|
||||
if (v >= clear_val) v -= clear_val;
|
||||
}
|
||||
if (x == gx1 && y == gy1) break;
|
||||
int e2 = 2 * err;
|
||||
if (e2 >= dy) { err += dy; x += sx; }
|
||||
if (e2 <= dx) { err += dx; y += sy; }
|
||||
}
|
||||
}
|
||||
|
||||
void GridMap::markCircle(double wx, double wy, double radius, uint8_t value) {
|
||||
int cx, cy, cr;
|
||||
if (!worldToGrid(wx, wy, cx, cy)) return;
|
||||
cr = (int)(radius / GRID_RES);
|
||||
for (int dy = -cr; dy <= cr; ++dy) {
|
||||
for (int dx = -cr; dx <= cr; ++dx) {
|
||||
if (dx*dx + dy*dy <= cr*cr) {
|
||||
set(cx+dx, cy+dy, value);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double GridMap::raycast(double wx, double wy, double angle, double max_range) const {
|
||||
// DDA (Digital Differential Analyzer) raycast on static occupancy grid.
|
||||
// Steps along grid cells from (wx,wy) in direction angle until hitting
|
||||
// an occupied cell (>128) or reaching max_range.
|
||||
double cos_a = std::cos(angle), sin_a = std::sin(angle);
|
||||
int gx, gy;
|
||||
if (!worldToGrid(wx, wy, gx, gy)) return max_range;
|
||||
|
||||
// Direction signs and step sizes
|
||||
double step_x = (cos_a != 0) ? std::abs(GRID_RES / cos_a) : 1e9;
|
||||
double step_y = (sin_a != 0) ? std::abs(GRID_RES / sin_a) : 1e9;
|
||||
int step_gx = (cos_a > 0) ? 1 : -1;
|
||||
int step_gy = (sin_a > 0) ? 1 : -1;
|
||||
|
||||
// Distance to next grid boundary
|
||||
double next_x = (cos_a > 0)
|
||||
? (GRID_ORIGIN_X + (gx + 1) * GRID_RES - wx) / cos_a
|
||||
: (GRID_ORIGIN_X + gx * GRID_RES - wx) / cos_a;
|
||||
double next_y = (sin_a > 0)
|
||||
? (GRID_ORIGIN_Y + (gy + 1) * GRID_RES - wy) / sin_a
|
||||
: (GRID_ORIGIN_Y + gy * GRID_RES - wy) / sin_a;
|
||||
|
||||
double t = 0;
|
||||
int max_steps = (int)(max_range / GRID_RES) + 2;
|
||||
for (int step = 0; step < max_steps; ++step) {
|
||||
if (t >= max_range) break;
|
||||
if (inBounds(gx, gy) && at(gx, gy) > 128)
|
||||
return t; // hit obstacle
|
||||
|
||||
if (next_x < next_y) {
|
||||
t = next_x;
|
||||
if (t >= max_range) break;
|
||||
gx += step_gx;
|
||||
next_x += step_x;
|
||||
} else {
|
||||
t = next_y;
|
||||
if (t >= max_range) break;
|
||||
gy += step_gy;
|
||||
next_y += step_y;
|
||||
}
|
||||
}
|
||||
return max_range; // no hit
|
||||
}
|
||||
|
||||
void GridMap::markRect(double x1, double y1, double x2, double y2, uint8_t value) {
|
||||
int gx1, gy1, gx2, gy2;
|
||||
worldToGrid(x1, y1, gx1, gy1);
|
||||
worldToGrid(x2, y2, gx2, gy2);
|
||||
for (int gy = std::min(gy1, gy2); gy <= std::max(gy1, gy2); ++gy)
|
||||
for (int gx = std::min(gx1, gx2); gx <= std::max(gx1, gx2); ++gx)
|
||||
set(gx, gy, value);
|
||||
}
|
||||
|
||||
} // namespace car_nav_lite
|
||||
181
src/local_planner.cpp
Normal file
181
src/local_planner.cpp
Normal file
@@ -0,0 +1,181 @@
|
||||
#include "car_nav_lite/local_planner.hpp"
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <cstdio>
|
||||
#include <chrono>
|
||||
|
||||
namespace car_nav_lite {
|
||||
|
||||
LocalPlanner::LocalPlanner() {
|
||||
prev_vx_ = 0.5;
|
||||
rng_state_ = (uint32_t)(std::chrono::system_clock::now().time_since_epoch().count());
|
||||
}
|
||||
|
||||
void LocalPlanner::setParams(double la, double ms, double mr, double sl, double cf) {
|
||||
ms_ = ms; mr_ = mr; sl_ = sl; cf_ = cf;
|
||||
(void)la;
|
||||
}
|
||||
|
||||
void LocalPlanner::computePathDistances(const std::vector<Pose2D>& path,
|
||||
std::vector<double>& dists) {
|
||||
dists.resize(path.size());
|
||||
if (path.empty()) return;
|
||||
dists[0] = 0.0;
|
||||
for (size_t i = 1; i < path.size(); ++i) {
|
||||
double dx = path[i].x - path[i-1].x;
|
||||
double dy = path[i].y - path[i-1].y;
|
||||
dists[i] = dists[i-1] + std::sqrt(dx*dx + dy*dy);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Simple RNG ─────────────────────────────────────────────
|
||||
double LocalPlanner::randNormal() {
|
||||
uint32_t x = rng_state_;
|
||||
x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
|
||||
double u1 = (double)(x & 0xFFFFFF) / 16777216.0 + 1e-10;
|
||||
x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
|
||||
double u2 = (double)(x & 0xFFFFFF) / 16777216.0;
|
||||
double z = std::sqrt(-2.0 * std::log(u1)) * std::cos(2.0 * M_PI * u2);
|
||||
return limit(z, -4.0, 4.0);
|
||||
}
|
||||
|
||||
// ── Rollout one trajectory ─────────────────────────────────
|
||||
double LocalPlanner::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) {
|
||||
|
||||
double cost = 0;
|
||||
double sx = pose.x, sy = pose.y, syaw = pose.yaw;
|
||||
const auto& goal = path.back();
|
||||
|
||||
for (int t = 0; t < T; ++t) {
|
||||
syaw += wz * DT;
|
||||
sx += vx * std::cos(syaw) * DT;
|
||||
sy += vx * std::sin(syaw) * DT;
|
||||
|
||||
// ── Obstacle: check with forward margin (car moves DT*vx per step,
|
||||
// at 1m/s that's 10cm — cone might be between steps) ──
|
||||
// Project forward by half a step for safety
|
||||
double fwd_x = sx + vx * DT * 0.5 * std::cos(syaw);
|
||||
double fwd_y = sy + vx * DT * 0.5 * std::sin(syaw);
|
||||
if (!map.isFreeFootprint(sx, sy, syaw) ||
|
||||
!map.isFreeFootprint(fwd_x, fwd_y, syaw)) {
|
||||
cost += 500.0;
|
||||
return cost;
|
||||
}
|
||||
}
|
||||
|
||||
// ── PathFollow (ENDPOINT, like EXACT-MPPI): distance from endpoint to
|
||||
// nearest path point. Allows intermediate deviation for obstacle avoidance. ──
|
||||
double min_d2 = 1e9;
|
||||
for (size_t i = 0; i < path.size(); ++i) {
|
||||
double dx = sx - path[i].x;
|
||||
double dy = sy - path[i].y;
|
||||
double d2 = dx*dx + dy*dy;
|
||||
if (d2 < min_d2) min_d2 = d2;
|
||||
}
|
||||
cost += std::sqrt(min_d2) * W_ALIGN;
|
||||
|
||||
// ── Goal: endpoint → final goal ──
|
||||
double end_to_goal = std::hypot(goal.x - sx, goal.y - sy);
|
||||
cost += W_FOLLOW * end_to_goal;
|
||||
|
||||
// ── Goal: only near goal ──
|
||||
if (total_path_len < 1.4)
|
||||
cost += W_GOAL * end_to_goal;
|
||||
|
||||
// ── Reverse penalty (mild) ──
|
||||
if (vx < 0) cost += std::abs(vx) * 3.0;
|
||||
|
||||
return cost;
|
||||
}
|
||||
|
||||
// ── Lightweight MPPI ───────────────────────────────────────
|
||||
Twist LocalPlanner::compute(const Pose2D& pose,
|
||||
const std::vector<Pose2D>& path,
|
||||
const GridMap& map) {
|
||||
Twist cmd{};
|
||||
if (path.size() < 2) return cmd;
|
||||
|
||||
std::vector<double> path_dists;
|
||||
computePathDistances(path, path_dists);
|
||||
double total_path_len = path_dists.back();
|
||||
|
||||
// ── Sample & evaluate K trajectories (80% exploitation, 20% exploration) ──
|
||||
double best_costs[K];
|
||||
double best_vx[K], best_wz[K];
|
||||
double min_cost = 1e9;
|
||||
|
||||
// If stuck (many trajectories killed), use more exploration
|
||||
static int alive_last = K;
|
||||
int exploit_ratio = (alive_last < K / 5) ? 2 : 4; // 40% or 80% exploit
|
||||
int exploit_n = K * exploit_ratio / 5;
|
||||
|
||||
for (int k = 0; k < K; ++k) {
|
||||
double vx, wz;
|
||||
if (k < exploit_n) {
|
||||
// Exploit: sample around previous optimal
|
||||
vx = prev_vx_ + VX_STD * randNormal();
|
||||
wz = prev_wz_ + WZ_STD * randNormal();
|
||||
} else {
|
||||
// Explore: uniform random across full range (including reverse)
|
||||
uint32_t x = rng_state_;
|
||||
x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
|
||||
vx = (double)(x & 0xFFFF) / 65535.0 * (ms_ + 0.5) - 0.5;
|
||||
x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
|
||||
wz = ((double)(x & 0xFFFF) / 65535.0 - 0.5) * 3.0;
|
||||
}
|
||||
|
||||
// Clamp + Ackermann
|
||||
vx = limit(vx, -0.5, ms_);
|
||||
wz = limit(wz, -1.5, 1.5);
|
||||
double max_wz = std::abs(vx) / mr_;
|
||||
wz = limit(wz, -max_wz, max_wz);
|
||||
|
||||
double c = evaluateTrajectory(pose, vx, wz, path, path_dists,
|
||||
total_path_len, map);
|
||||
best_costs[k] = c;
|
||||
best_vx[k] = vx;
|
||||
best_wz[k] = wz;
|
||||
if (c < min_cost) min_cost = c;
|
||||
}
|
||||
|
||||
// Track survival rate for adaptive exploration
|
||||
int alive_now = 0;
|
||||
for (int k = 0; k < K; ++k)
|
||||
if (best_costs[k] < 500.0) ++alive_now;
|
||||
alive_last = alive_now;
|
||||
|
||||
// ── Softmax ──
|
||||
double sum_weights = 0, weighted_vx = 0, weighted_wz = 0;
|
||||
for (int k = 0; k < K; ++k) {
|
||||
double w = std::exp(-(best_costs[k] - min_cost) / TEMPERATURE);
|
||||
weighted_vx += w * best_vx[k];
|
||||
weighted_wz += w * best_wz[k];
|
||||
sum_weights += w;
|
||||
}
|
||||
|
||||
if (sum_weights < 1e-9) return cmd;
|
||||
cmd.vx = weighted_vx / sum_weights;
|
||||
cmd.wz = weighted_wz / sum_weights;
|
||||
|
||||
double max_wz = std::abs(cmd.vx) / mr_;
|
||||
cmd.wz = limit(cmd.wz, -max_wz, max_wz);
|
||||
|
||||
prev_vx_ = cmd.vx;
|
||||
prev_wz_ = cmd.wz;
|
||||
|
||||
// Debug
|
||||
static int dbg = 0;
|
||||
if (++dbg % 10 == 0) {
|
||||
fprintf(stderr, "[MPPI] out=(%.2f,%.2f) alive=%d/%d minC=%.1f sumW=%.1f\n",
|
||||
cmd.vx, cmd.wz, alive_now, K, min_cost, sum_weights);
|
||||
}
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
} // namespace car_nav_lite
|
||||
60
src/local_planner.hpp
Normal file
60
src/local_planner.hpp
Normal file
@@ -0,0 +1,60 @@
|
||||
#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
|
||||
185
src/localizer.cpp
Normal file
185
src/localizer.cpp
Normal file
@@ -0,0 +1,185 @@
|
||||
#include "car_nav_lite/localizer.hpp"
|
||||
#include "car_nav_lite/grid_map.hpp"
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <algorithm>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
namespace car_nav_lite {
|
||||
|
||||
Localizer::Localizer() {}
|
||||
|
||||
void Localizer::updateOdom(double vx, double wz, double dt) {
|
||||
yaw_odom_ += wz * dt;
|
||||
if (std::abs(yaw_imu_ - yaw_odom_) > 0.1) yaw_imu_ = yaw_odom_;
|
||||
double yaw = yaw_odom_;
|
||||
odom_pose_.x += vx * std::cos(yaw) * dt;
|
||||
odom_pose_.y += vx * std::sin(yaw) * dt;
|
||||
odom_pose_.yaw = yaw;
|
||||
}
|
||||
|
||||
void Localizer::updateIMU(double gyro_z, double dt) {
|
||||
if (std::abs(gyro_z) < 0.005) return;
|
||||
yaw_imu_ += gyro_z * dt;
|
||||
}
|
||||
|
||||
void Localizer::setOdomPose(const Pose2D& p) { odom_pose_ = p; }
|
||||
|
||||
Pose2D Localizer::pose() const {
|
||||
return {odom_pose_.x + correction_.x,
|
||||
odom_pose_.y + correction_.y,
|
||||
odom_pose_.yaw + correction_.yaw};
|
||||
}
|
||||
|
||||
// ── Fence-line-based scan correction ───────────────────────
|
||||
//
|
||||
// The 5m×5m arena has a rectangular fence. We detect Hough lines
|
||||
// in the scan, keep near-H/V ones (= fence walls), and compare
|
||||
// their world positions against the known fence at x=±2.5, y=±2.5.
|
||||
// This gives dx,dy correction. Yaw comes from the line angle bias.
|
||||
//
|
||||
void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map) {
|
||||
if (scan.size() < 30) return;
|
||||
|
||||
const int N = scan.size();
|
||||
const Pose2D& base = odom_pose_;
|
||||
|
||||
// ── 1. Render scan to local image (world coords, ±1.5m around car) ──
|
||||
constexpr int IMG_SZ = 600; // 3m × 3m at 5mm
|
||||
constexpr int HALF = IMG_SZ / 2;
|
||||
int cx_g, cy_g;
|
||||
map.worldToGrid(base.x, base.y, cx_g, cy_g);
|
||||
|
||||
cv::Mat scan_img(IMG_SZ, IMG_SZ, CV_8UC1, cv::Scalar(0));
|
||||
int hit_count = 0;
|
||||
for (int i = 0; i < N; i += 2) {
|
||||
double r = scan.ranges[i];
|
||||
if (r < 0.1 || r > 4.5) continue;
|
||||
double a = scan.angle_min + i * scan.angle_increment + base.yaw;
|
||||
double wx = base.x + r * std::cos(a);
|
||||
double wy = base.y + r * std::sin(a);
|
||||
int gx, gy;
|
||||
if (!map.worldToGrid(wx, wy, gx, gy)) continue;
|
||||
int px = (gx - cx_g) + HALF;
|
||||
int py = (gy - cy_g) + HALF;
|
||||
if (px >= 0 && px < IMG_SZ && py >= 0 && py < IMG_SZ) {
|
||||
scan_img.at<uint8_t>(py, px) = 255;
|
||||
++hit_count;
|
||||
}
|
||||
}
|
||||
if (hit_count < 30) return;
|
||||
|
||||
// ── 2. Hough line detection ────────────────────────────
|
||||
std::vector<cv::Vec4i> lines;
|
||||
cv::HoughLinesP(scan_img, lines,
|
||||
1, CV_PI / 180.0, // rho=1px, theta=1°
|
||||
20, // vote threshold
|
||||
40, // min line length (40px = 20cm)
|
||||
10); // max gap
|
||||
|
||||
// ── 3. Fence line analysis (no angle filter — car yaw can be anything) ──
|
||||
// Use double-angle voting: 0° and 90° fence lines both map to 0° after
|
||||
// doubling. Deviation of the dominant direction from 0 = yaw error.
|
||||
double sum_cos2 = 0, sum_sin2 = 0;
|
||||
int yaw_count = 0;
|
||||
|
||||
for (auto& l : lines) {
|
||||
double ldx = l[2] - l[0], ldy = l[3] - l[1];
|
||||
if (ldx == 0 && ldy == 0) continue;
|
||||
double ang = std::atan2(ldy, ldx); // [-π, π]
|
||||
sum_cos2 += std::cos(2.0 * ang);
|
||||
sum_sin2 += std::sin(2.0 * ang);
|
||||
++yaw_count;
|
||||
}
|
||||
|
||||
if (yaw_count < 3) {
|
||||
correction_.x *= 0.9; correction_.y *= 0.9;
|
||||
return;
|
||||
}
|
||||
|
||||
// Dominant fence orientation → yaw error
|
||||
double dyaw = std::atan2(sum_sin2, sum_cos2) / 2.0;
|
||||
while (dyaw > M_PI/4) dyaw -= M_PI/2;
|
||||
while (dyaw < -M_PI/4) dyaw += M_PI/2;
|
||||
|
||||
// ── 4. Re-classify lines AFTER yaw correction ──────────
|
||||
// Now we know the fence direction. Lines at ang≈dyaw or ang≈dyaw+π/2
|
||||
// are fence walls.
|
||||
double sum_dx = 0, sum_dy = 0;
|
||||
int line_count = 0;
|
||||
constexpr double TOL = 15.0 * M_PI / 180.0;
|
||||
|
||||
for (auto& l : lines) {
|
||||
double ldx = l[2] - l[0], ldy = l[3] - l[1];
|
||||
if (ldx == 0 && ldy == 0) continue;
|
||||
double ang = std::atan2(ldy, ldx);
|
||||
if (ang < 0) ang += M_PI; // [0, π)
|
||||
|
||||
// Check if this line aligns with the detected fence direction
|
||||
double diff1 = std::abs(ang - dyaw);
|
||||
while (diff1 > M_PI) diff1 -= M_PI;
|
||||
if (diff1 > M_PI/2) diff1 = M_PI - diff1;
|
||||
|
||||
double diff2 = std::abs(ang - (dyaw + M_PI/2));
|
||||
while (diff2 > M_PI) diff2 -= M_PI;
|
||||
if (diff2 > M_PI/2) diff2 = M_PI - diff2;
|
||||
|
||||
double diff = std::min(diff1, diff2);
|
||||
if (diff > TOL) continue; // not aligned with fence
|
||||
|
||||
++line_count;
|
||||
|
||||
// Line midpoint → world coords → compare with known fence (±2.5m)
|
||||
double mx = (l[0] + l[2]) / 2.0;
|
||||
double my = (l[1] + l[3]) / 2.0;
|
||||
double mwx, mwy;
|
||||
map.gridToWorld(cx_g - HALF + (int)mx, cy_g - HALF + (int)my, mwx, mwy);
|
||||
|
||||
// Which fence wall? Use the corrected yaw to determine H vs V
|
||||
bool is_horiz = (diff1 < diff2); // closer to dyaw = horizontal in fence frame
|
||||
|
||||
if (is_horiz) {
|
||||
double expected = (mwy > 0) ? 2.5 : -2.5;
|
||||
sum_dy += (expected - mwy);
|
||||
} else {
|
||||
double expected = (mwx > 0) ? 2.5 : -2.5;
|
||||
sum_dx += (expected - mwx);
|
||||
}
|
||||
}
|
||||
|
||||
if (line_count < 2) {
|
||||
correction_.yaw = -dyaw; // at least update yaw
|
||||
correction_.x *= 0.9; correction_.y *= 0.9;
|
||||
return;
|
||||
}
|
||||
|
||||
double dx = sum_dx / line_count;
|
||||
double dy = sum_dy / line_count;
|
||||
double corr_dist = std::sqrt(dx*dx + dy*dy);
|
||||
|
||||
// ── 5. Safety checks before updating offset ──
|
||||
bool trust_translation = (corr_dist < 0.2 && line_count >= 2);
|
||||
bool trust_yaw = (std::abs(dyaw) < 0.15 && yaw_count >= 2);
|
||||
|
||||
// Don't let correction push pose outside arena
|
||||
double new_x = odom_pose_.x + dx;
|
||||
double new_y = odom_pose_.y + dy;
|
||||
if (std::abs(new_x) > 2.3 || std::abs(new_y) > 2.3)
|
||||
trust_translation = false;
|
||||
|
||||
if (trust_translation && trust_yaw) {
|
||||
correction_.x = dx;
|
||||
correction_.y = dy;
|
||||
correction_.yaw = -dyaw;
|
||||
fprintf(stderr, "[Localizer] fence: d(%.3f,%.3f,%.2f°) lines=%d/%d\n",
|
||||
dx, dy, -dyaw*57.3, line_count, yaw_count);
|
||||
} else if (trust_yaw) {
|
||||
correction_.yaw = -dyaw;
|
||||
correction_.x *= 0.9; correction_.y *= 0.9;
|
||||
} else {
|
||||
correction_.x *= 0.9; correction_.y *= 0.9;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace car_nav_lite
|
||||
34
src/localizer.hpp
Normal file
34
src/localizer.hpp
Normal file
@@ -0,0 +1,34 @@
|
||||
#pragma once
|
||||
|
||||
#include "car_nav_lite/types.hpp"
|
||||
|
||||
namespace car_nav_lite {
|
||||
|
||||
class GridMap;
|
||||
|
||||
class Localizer {
|
||||
public:
|
||||
Localizer();
|
||||
|
||||
// ── Odometry update (called at odom rate) ──────────
|
||||
void updateOdom(double vx, double wz, double dt);
|
||||
void updateIMU(double gyro_z, double dt);
|
||||
|
||||
// Set raw odom pose. pose() returns odom + correction offset.
|
||||
void setOdomPose(const Pose2D& p);
|
||||
|
||||
// ── Scan-to-map correction (updates persistent offset) ──
|
||||
// Correction = matched_pose - odom_pose, stored as offset.
|
||||
// Only updates if confidence is high; otherwise offset decays to zero.
|
||||
void correctWithScanCV(const LaserScan& scan, const GridMap& map);
|
||||
|
||||
// ── Final pose = odom + correction offset ──────────
|
||||
Pose2D pose() const;
|
||||
|
||||
private:
|
||||
Pose2D odom_pose_;
|
||||
Pose2D correction_;
|
||||
double yaw_imu_ = 0.0, yaw_odom_ = 0.0;
|
||||
};
|
||||
|
||||
} // namespace car_nav_lite
|
||||
245
src/main.cpp
Normal file
245
src/main.cpp
Normal file
@@ -0,0 +1,245 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <nav_msgs/msg/occupancy_grid.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include "car_nav_lite/types.hpp"
|
||||
#include "car_nav_lite/grid_map.hpp"
|
||||
#include "car_nav_lite/localizer.hpp"
|
||||
#include "car_nav_lite/bt_executor.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace car_nav_lite;
|
||||
using LaserScanMsg = sensor_msgs::msg::LaserScan;
|
||||
using OdometryMsg = nav_msgs::msg::Odometry;
|
||||
using ImuMsg = sensor_msgs::msg::Imu;
|
||||
using PoseStMsg = geometry_msgs::msg::PoseStamped;
|
||||
|
||||
class NavLiteNode : public rclcpp::Node {
|
||||
public:
|
||||
NavLiteNode() : Node("nav_lite_node") {
|
||||
// ── ROS2 dynamic parameters ──
|
||||
declare_parameter("map_file", std::string(""));
|
||||
declare_parameter("test_mode", false);
|
||||
declare_parameter("lookahead", 0.5);
|
||||
declare_parameter("max_speed", 1.0);
|
||||
declare_parameter("min_turn_radius", 0.20);
|
||||
declare_parameter("steer_limit", 0.6);
|
||||
declare_parameter("speed_curv_factor", 6.0);
|
||||
_read_params();
|
||||
|
||||
param_cb_ = add_on_set_parameters_callback(
|
||||
[this](const std::vector<rclcpp::Parameter>& params) {
|
||||
for (auto& p : params) {
|
||||
std::string n = p.get_name();
|
||||
if (n=="lookahead") la_=p.as_double();
|
||||
else if (n=="max_speed") ms_=p.as_double();
|
||||
else if (n=="min_turn_radius") mr_=p.as_double();
|
||||
else if (n=="steer_limit") sl_=p.as_double();
|
||||
else if (n=="speed_curv_factor") cf_=p.as_double();
|
||||
else if (n=="test_mode") { test_=p.as_bool(); tps_=false; }
|
||||
}
|
||||
RCLCPP_INFO(get_logger(),"params: la=%.2f ms=%.1f mr=%.2f sl=%.2f cf=%.1f test=%d",
|
||||
la_,ms_,mr_,sl_,cf_,test_);
|
||||
return rcl_interfaces::msg::SetParametersResult().set__successful(true);
|
||||
});
|
||||
|
||||
// ── Subscribers ──
|
||||
sub_scan_ = create_subscription<LaserScanMsg>("/scan", 10,
|
||||
std::bind(&NavLiteNode::scan_cb, this, std::placeholders::_1));
|
||||
sub_odom_ = create_subscription<OdometryMsg>("/odom", 10,
|
||||
std::bind(&NavLiteNode::odom_cb, this, std::placeholders::_1));
|
||||
sub_imu_ = create_subscription<ImuMsg>("/imu/data_raw", 10,
|
||||
std::bind(&NavLiteNode::imu_cb, this, std::placeholders::_1));
|
||||
sub_goal_ = create_subscription<PoseStMsg>("/goal_pose", 10,
|
||||
std::bind(&NavLiteNode::goal_cb, this, std::placeholders::_1));
|
||||
|
||||
// ── Publishers ──
|
||||
pub_cmd_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
|
||||
pub_path_ = create_publisher<nav_msgs::msg::Path>("/global_path", 10);
|
||||
pub_map_ = create_publisher<nav_msgs::msg::OccupancyGrid>("/map", rclcpp::QoS(1).transient_local());
|
||||
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
|
||||
|
||||
if (!mf_.empty()) {
|
||||
if (!map_.loadPGM(mf_)) RCLCPP_WARN(get_logger(),"Map load fail");
|
||||
else RCLCPP_INFO(get_logger(),"Map OK %dx%d",map_.width(),map_.height());
|
||||
}
|
||||
|
||||
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));
|
||||
|
||||
RCLCPP_INFO(get_logger(),"car_nav_lite ready. test_mode:=true for auto path. "
|
||||
"ros2 param set /nav_lite_node <name> <value>");
|
||||
RCLCPP_INFO(get_logger(),"Params: la=%.2f ms=%.1f mr=%.2f sl=%.2f cf=%.1f",
|
||||
la_,ms_,mr_,sl_,cf_);
|
||||
}
|
||||
|
||||
private:
|
||||
// ── Dynamic params ──
|
||||
std::string mf_;
|
||||
bool test_=false, tps_=false;
|
||||
double la_=0.5, ms_=1.0, mr_=0.20, sl_=0.6, cf_=6.0;
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;
|
||||
void _read_params(){
|
||||
mf_=get_parameter("map_file").as_string();
|
||||
test_=get_parameter("test_mode").as_bool();
|
||||
la_=get_parameter("lookahead").as_double();
|
||||
ms_=get_parameter("max_speed").as_double();
|
||||
mr_=get_parameter("min_turn_radius").as_double();
|
||||
sl_=get_parameter("steer_limit").as_double();
|
||||
cf_=get_parameter("speed_curv_factor").as_double();
|
||||
}
|
||||
|
||||
// ── State ──
|
||||
std::mutex mtx_;
|
||||
LaserScan latest_scan_; bool has_scan_=false;
|
||||
double latest_vx_=0, latest_wz_=0, latest_gyro_z_=0;
|
||||
rclcpp::Time last_odom_t_{0,0,RCL_ROS_TIME};
|
||||
rclcpp::Time last_imu_t_{0,0,RCL_ROS_TIME};
|
||||
GridMap map_; Localizer localizer_; BTExecutor bt_;
|
||||
rclcpp::Subscription<LaserScanMsg>::SharedPtr sub_scan_;
|
||||
rclcpp::Subscription<OdometryMsg>::SharedPtr sub_odom_;
|
||||
rclcpp::Subscription<ImuMsg>::SharedPtr sub_imu_;
|
||||
rclcpp::Subscription<PoseStMsg>::SharedPtr sub_goal_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_cmd_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_path_;
|
||||
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_map_;
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_;
|
||||
|
||||
void scan_cb(LaserScanMsg::SharedPtr m){std::lock_guard lk(mtx_);
|
||||
latest_scan_.angle_min=m->angle_min;latest_scan_.angle_max=m->angle_max;
|
||||
latest_scan_.angle_increment=m->angle_increment;
|
||||
latest_scan_.range_min=m->range_min;latest_scan_.range_max=m->range_max;
|
||||
latest_scan_.ranges=m->ranges;has_scan_=true;}
|
||||
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.
|
||||
auto& p = m->pose.pose;
|
||||
double qx=p.orientation.x, qy=p.orientation.y, qz=p.orientation.z, qw=p.orientation.w;
|
||||
double yaw = std::atan2(2.0*(qw*qz + qx*qy), 1.0 - 2.0*(qy*qy + qz*qz));
|
||||
localizer_.setOdomPose({p.position.x, p.position.y, yaw});
|
||||
last_odom_t_ = rclcpp::Time(m->header.stamp);
|
||||
latest_vx_ = m->twist.twist.linear.x;
|
||||
latest_wz_ = m->twist.twist.angular.z;}
|
||||
void imu_cb(ImuMsg::SharedPtr m){std::lock_guard lk(mtx_);
|
||||
auto now=rclcpp::Time(m->header.stamp);
|
||||
double dt=last_imu_t_.nanoseconds()>0?(now-last_imu_t_).seconds():0.005;
|
||||
last_imu_t_=now;latest_gyro_z_=m->angular_velocity.z;
|
||||
if(dt>0&&dt<1.0)localizer_.updateIMU(latest_gyro_z_,dt);}
|
||||
void goal_cb(PoseStMsg::SharedPtr m){std::lock_guard lk(mtx_);
|
||||
if(m->header.frame_id!="map"){RCLCPP_WARN(get_logger(),"goal frame not map");return;}
|
||||
Pose2D g(m->pose.position.x,m->pose.position.y,0.0);
|
||||
double qw=m->pose.orientation.w,qz=m->pose.orientation.z;
|
||||
if(qw!=0||qz!=0)g.yaw=std::atan2(2.0*qw*qz,qw*qw-qz*qz);
|
||||
bt_.setGoal(g);tps_=false;}
|
||||
|
||||
// ── Test path: figure-8 with 30cm circles ──
|
||||
std::vector<Pose2D> _genTestPath(){
|
||||
std::vector<Pose2D> wp;
|
||||
double cx=0,cy=-0.5,r=0.15; // 15cm radius = 30cm diameter
|
||||
int N=30;
|
||||
// Circle 1: counter-clockwise
|
||||
for(int i=0;i<=N;++i){
|
||||
double a=2.0*M_PI*i/N;
|
||||
wp.push_back({cx-r*std::cos(a),cy-r+r*std::sin(a),0});
|
||||
}
|
||||
// Straight connecting segment
|
||||
wp.push_back({cx,cy-2*r,0});
|
||||
// Circle 2: clockwise (other direction), centered right
|
||||
for(int i=0;i<=N;++i){
|
||||
double a=-2.0*M_PI*i/N;
|
||||
wp.push_back({cx+r*std::cos(a),cy-3*r+r*std::sin(a),0});
|
||||
}
|
||||
// Straight back
|
||||
wp.push_back({cx,cy,0});
|
||||
// Rectangle 2x1.5m
|
||||
for(auto& p:std::vector<Pose2D>{{cx-1,cy-0.75,0},{cx+1,cy-0.75,0},{cx+1,cy+0.75,0},{cx-1,cy+0.75,0},{cx-1,cy-0.75,0}})
|
||||
wp.push_back(p);
|
||||
// Interpolate
|
||||
std::vector<Pose2D> dense;
|
||||
for(size_t i=0;i+1<wp.size();++i){
|
||||
double d=wp[i].distTo(wp[i+1]);
|
||||
int steps=std::max(1,(int)(d/0.05));
|
||||
for(int s=0;s<steps;++s){
|
||||
double t=(double)s/steps;
|
||||
dense.push_back({wp[i].x+t*(wp[i+1].x-wp[i].x),wp[i].y+t*(wp[i+1].y-wp[i].y),0});
|
||||
}
|
||||
}
|
||||
for(size_t i=0;i+1<dense.size();++i)dense[i].yaw=dense[i].angleTo(dense[i+1]);
|
||||
if(!dense.empty())dense.back().yaw=dense[dense.size()-2].yaw;
|
||||
return dense;
|
||||
}
|
||||
|
||||
void tick(){
|
||||
std::lock_guard lk(mtx_);
|
||||
auto pose=localizer_.pose();publish_tf(pose);
|
||||
if(has_scan_)map_.updateScan(latest_scan_,pose);
|
||||
|
||||
// Test mode: auto-activate figure-8 after 3s
|
||||
if(test_&&!tps_){
|
||||
static int tc=0;
|
||||
if(++tc>60){
|
||||
auto tp=_genTestPath();
|
||||
bt_.setTestPath(tp);tps_=true;
|
||||
RCLCPP_INFO(get_logger(),"Test path: fig8+rect, %zu pts",tp.size());
|
||||
}
|
||||
}
|
||||
|
||||
// Update BT with current params
|
||||
bt_.setParams(la_,ms_,mr_,sl_,cf_);
|
||||
Twist cmd=bt_.tick(pose,map_);
|
||||
|
||||
auto tw=geometry_msgs::msg::Twist();tw.linear.x=cmd.vx;tw.angular.z=cmd.wz;
|
||||
pub_cmd_->publish(tw);
|
||||
if(!bt_.currentPath().empty()){
|
||||
auto pm=nav_msgs::msg::Path();pm.header.stamp=now();pm.header.frame_id="map";
|
||||
for(auto&p:bt_.currentPath()){
|
||||
geometry_msgs::msg::PoseStamped ps;ps.header.frame_id="map";
|
||||
ps.pose.position.x=p.x;ps.pose.position.y=p.y;
|
||||
ps.pose.orientation.z=std::sin(p.yaw/2.0);ps.pose.orientation.w=std::cos(p.yaw/2.0);
|
||||
pm.poses.push_back(ps);
|
||||
}pub_path_->publish(pm);
|
||||
}
|
||||
static int ct=0;
|
||||
if(++ct%60==0)RCLCPP_INFO(get_logger(),"pose=(%.2f,%.2f,%.0f) cmd=(%.2f,%.2f)",
|
||||
pose.x,pose.y,pose.yaw*57.3,cmd.vx,cmd.wz);
|
||||
}
|
||||
|
||||
void publish_map(){std::lock_guard lk(mtx_);
|
||||
auto m=nav_msgs::msg::OccupancyGrid();m.header.stamp=now();m.header.frame_id="map";
|
||||
m.info.resolution=GRID_RES;m.info.width=m.info.height=GRID_SIZE;
|
||||
m.info.origin.position.x=GRID_ORIGIN_X;m.info.origin.position.y=GRID_ORIGIN_Y;
|
||||
m.data.resize(GRID_SIZE*GRID_SIZE);
|
||||
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(){
|
||||
if(!has_scan_)return;
|
||||
auto pose_before = localizer_.pose();
|
||||
localizer_.correctWithScanCV(latest_scan_, map_);
|
||||
auto pose_after = localizer_.pose();
|
||||
(void)pose_before; (void)pose_after;
|
||||
}
|
||||
|
||||
void publish_tf(const Pose2D& pose){
|
||||
geometry_msgs::msg::TransformStamped tf;
|
||||
tf.header.stamp=now();tf.header.frame_id="map";tf.child_frame_id="base_link";
|
||||
tf.transform.translation.x=pose.x;tf.transform.translation.y=pose.y;
|
||||
double hy=pose.yaw/2.0;tf.transform.rotation.z=std::sin(hy);tf.transform.rotation.w=std::cos(hy);
|
||||
tf_broadcaster_->sendTransform(tf);}
|
||||
};
|
||||
|
||||
int main(int argc,char*argv[]){rclcpp::init(argc,argv);rclcpp::spin(std::make_shared<NavLiteNode>());rclcpp::shutdown();return 0;}
|
||||
31
src/nav_lite.launch.py
Normal file
31
src/nav_lite.launch.py
Normal file
@@ -0,0 +1,31 @@
|
||||
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"),
|
||||
}],
|
||||
),
|
||||
])
|
||||
83
src/types.hpp
Normal file
83
src/types.hpp
Normal file
@@ -0,0 +1,83 @@
|
||||
#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
|
||||
Reference in New Issue
Block a user