commit 5156a8d766dd0bee773cef98f4a8be4a24e7fdd6 Author: cyy Date: Thu Jul 2 21:29:41 2026 +0000 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) diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..fe29f06 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.8) +project(car_nav_lite) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) + +set(INCLUDE_DIR include) + +add_executable(nav_lite_node + src/main.cpp + src/grid_map.cpp + src/localizer.cpp + src/global_planner.cpp + src/local_planner.cpp + src/bt_executor.cpp +) + +target_include_directories(nav_lite_node PRIVATE + ${INCLUDE_DIR} + ${EIGEN3_INCLUDE_DIRS} +) + +ament_target_dependencies(nav_lite_node + rclcpp std_msgs geometry_msgs nav_msgs sensor_msgs tf2 tf2_ros +) +target_link_libraries(nav_lite_node ${OpenCV_LIBS}) + +add_executable(calib_check src/calib_check.cpp) +target_include_directories(calib_check PRIVATE ${INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS}) +ament_target_dependencies(calib_check rclcpp geometry_msgs nav_msgs tf2 tf2_ros) +install(TARGETS nav_lite_node calib_check DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch config maps DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/config/params.yaml b/config/params.yaml new file mode 100644 index 0000000..9842e7d --- /dev/null +++ b/config/params.yaml @@ -0,0 +1,4 @@ +nav_lite_node: + ros__parameters: + map_file: "" # PGM path, set via launch argument + lookahead: 0.5 # Pure Pursuit lookahead distance (m) diff --git a/include/car_nav_lite/bt_executor.hpp b/include/car_nav_lite/bt_executor.hpp new file mode 100644 index 0000000..423461f --- /dev/null +++ b/include/car_nav_lite/bt_executor.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include "car_nav_lite/types.hpp" +#include "car_nav_lite/grid_map.hpp" +#include "car_nav_lite/localizer.hpp" +#include "car_nav_lite/global_planner.hpp" +#include "car_nav_lite/local_planner.hpp" + +namespace car_nav_lite { + +class BTExecutor { +public: + BTExecutor(); + + void setGoal(const Pose2D& goal); + void setTestPath(const std::vector& path); + void setParams(double la, double ms, double mr, double sl, double cf); + bool hasGoal() const { return goal_.has_value(); } + + Twist tick(const Pose2D& pose, const GridMap& map); + + NavState state() const { return state_; } + const std::vector& currentPath() const { return current_path_; } + +private: + NavState state_ = NavState::IDLE; + std::optional goal_; + std::vector current_path_; + GlobalPlanner planner_; + LocalPlanner local_planner_; + + bool atGoal(const Pose2D& pose) const; +}; + +} // namespace car_nav_lite diff --git a/include/car_nav_lite/global_planner.hpp b/include/car_nav_lite/global_planner.hpp new file mode 100644 index 0000000..5c580ae --- /dev/null +++ b/include/car_nav_lite/global_planner.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include "car_nav_lite/types.hpp" +#include "car_nav_lite/grid_map.hpp" +#include +#include +#include + +namespace car_nav_lite { + +class GlobalPlanner { +public: + GlobalPlanner(); + + // ── A* search, returns world-coordinate path ─────── + // inflation_override < 0 → use default INFLATION_CELLS + std::vector plan(const Pose2D& start, const Pose2D& goal, + const GridMap& map, int inflation_override = -1); + + // ── Smooth path (funnel + approximate) ───────────── + std::vector smooth(const std::vector& raw_path, + const GridMap& map); + +private: + int manhattan(int x1, int y1, int x2, int y2) const { + return 10 * (std::abs(x1-x2) + std::abs(y1-y2)); + } + int euclidean(int x1, int y1, int x2, int y2) const { + double dx = x1 - x2, dy = y1 - y2; + return (int)(10.0 * std::sqrt(dx*dx + dy*dy)); + } + int moveCost(int x1, int y1, int x2, int y2) const { + return (x1 != x2 && y1 != y2) ? 14 : 10; // diagonal costs more + } +}; + +} // namespace car_nav_lite diff --git a/include/car_nav_lite/grid_map.cpp b/include/car_nav_lite/grid_map.cpp new file mode 100644 index 0000000..ba0c38b --- /dev/null +++ b/include/car_nav_lite/grid_map.cpp @@ -0,0 +1,158 @@ +#include "car_nav_lite/grid_map.hpp" +#include +#include +#include +#include + +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 raw(w * h); + file.read(reinterpret_cast(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 { + if (!inBounds(gx, gy)) return false; + for (int dy = -INFLATION_CELLS; dy <= INFLATION_CELLS; ++dy) { + for (int dx = -INFLATION_CELLS; dx <= INFLATION_CELLS; ++dx) { + int nx = gx + dx, ny = gy + dy; + if (!inBounds(nx, ny)) continue; // skip OOB; don't treat as obstacle + if (at(nx, ny) > 128) 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)) { + uint8_t cur = at(hx, hy); + set(hx, hy, std::min(255, (int)cur + 20)); // hit → +occupied + } + + // 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); + } + } + } +} + +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 diff --git a/include/car_nav_lite/grid_map.hpp b/include/car_nav_lite/grid_map.hpp new file mode 100644 index 0000000..7802556 --- /dev/null +++ b/include/car_nav_lite/grid_map.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include "car_nav_lite/types.hpp" +#include +#include + +namespace car_nav_lite { + +// ── 8-connected neighbor deltas ────────────────────────── +constexpr std::array, 8> NEIGHBORS = {{ + {1,0},{-1,0},{0,1},{0,-1},{1,1},{-1,-1},{1,-1},{-1,1} +}}; + +class GridMap { +public: + GridMap(); + + // ── Load PGM P5 from file ────────────────────────── + bool loadPGM(const std::string& path); + + // ── Coordinate transforms ────────────────────────── + bool worldToGrid(double wx, double wy, int& gx, int& gy) const; + void gridToWorld(int gx, int gy, double& wx, double& wy) const; + + // ── Access ────────────────────────────────────────── + uint8_t at(int gx, int gy) const; + void set(int gx, int gy, uint8_t v); + bool inBounds(int gx, int gy) const; + bool isFree(int gx, int gy) const; + bool isFree(int gx, int gy, int inflate_cells) const; // configurable inflation + + // ── Raycast on static obstacles (for scan-to-map matching) ── + // Returns range to first obstacle >128 along (wx,wy) → angle. + double raycast(double wx, double wy, double angle, double max_range = 5.0) const; + + // ── Oriented footprint collision check (EXACT-MPPI style) ── + // Checks raw occupancy against car's oriented rectangle at (wx,wy,yaw). + // Handles car width/length correctly regardless of orientation. + bool isFreeFootprint(double wx, double wy, double yaw) const; + + // ── Obstacle update from scan ────────────────────── + void updateScan(const LaserScan& scan, const Pose2D& pose); + + // ── Pre-built obstacle marks ─────────────────────── + void markCircle(double wx, double wy, double radius, uint8_t value = 255); + void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255); + + // ── Getters ──────────────────────────────────────── + int width() const { return GRID_SIZE; } + int height() const { return GRID_SIZE; } + const OccupancyGrid& data() const { return cells_; } + +private: + OccupancyGrid cells_; + void bresenhamLine(int gx0, int gy0, int gx1, int gy1, uint8_t clear_val); +}; + +} // namespace car_nav_lite diff --git a/include/car_nav_lite/local_planner.hpp b/include/car_nav_lite/local_planner.hpp new file mode 100644 index 0000000..a83d569 --- /dev/null +++ b/include/car_nav_lite/local_planner.hpp @@ -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& path, + const GridMap& map); + +private: + // ── Params ── + double ms_ = 1.0, mr_ = 0.25, sl_ = 0.6, cf_ = 6.0; + + // ── MPPI settings ── + static constexpr int K = 200; // trajectory count + static constexpr int T = 15; // timesteps + static constexpr double DT = 0.1; // step duration (1.5s horizon) + static constexpr double TEMPERATURE = 0.5; + static constexpr double VX_STD = 0.2; + static constexpr double WZ_STD = 0.8; + + // ── Cost weights (EXACT-MPPI critic ratios) ── + static constexpr double W_ALIGN = 10.0; // PathAlignCritic + static constexpr double W_FOLLOW = 5.0; // PathFollowCritic + static constexpr double W_GOAL = 5.0; // GoalCritic + static constexpr double W_OBS = 5.0; // ObstaclesCritic (proportional) + static constexpr double W_FORWARD = 3.0; // PreferForwardCritic (penalize reverse) + static constexpr double W_HEADING = 1.5; // heading-to-goal (keep low — path_align dominates) + static constexpr double W_SMOOTH = 0.5; // control smoothness + + // ── Warm-start ── + double prev_vx_ = 0.5, prev_wz_ = 0; + + // ── Simple RNG (XORshift + Box-Muller, no stdlib issues on ARM) ── + uint32_t rng_state_ = 123456789; + double randNormal(); // returns N(0,1) + + // ── Rollout one trajectory, return total cost ── + double evaluateTrajectory(const Pose2D& pose, double vx, double wz, + const std::vector& path, + const std::vector& path_dists, + double total_path_len, + const GridMap& map); + + static void computePathDistances(const std::vector& path, + std::vector& dists); +}; + +} // namespace car_nav_lite diff --git a/include/car_nav_lite/localizer.hpp b/include/car_nav_lite/localizer.hpp new file mode 100644 index 0000000..18d8ed9 --- /dev/null +++ b/include/car_nav_lite/localizer.hpp @@ -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 diff --git a/include/car_nav_lite/main.cpp b/include/car_nav_lite/main.cpp new file mode 100644 index 0000000..16a8a75 --- /dev/null +++ b/include/car_nav_lite/main.cpp @@ -0,0 +1,179 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#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") { + declare_parameter("map_file", std::string("")); + declare_parameter("lookahead", 0.5); + map_file_ = get_parameter("map_file").as_string(); + lookahead_ = get_parameter("lookahead").as_double(); + + sub_scan_ = create_subscription("/scan", 10, + std::bind(&NavLiteNode::scan_cb, this, std::placeholders::_1)); + sub_odom_ = create_subscription("/odom", 10, + std::bind(&NavLiteNode::odom_cb, this, std::placeholders::_1)); + sub_imu_ = create_subscription("/imu/data_raw", 10, + std::bind(&NavLiteNode::imu_cb, this, std::placeholders::_1)); + sub_goal_ = create_subscription("/goal_pose", 10, + std::bind(&NavLiteNode::goal_cb, this, std::placeholders::_1)); + + pub_cmd_ = create_publisher("/cmd_vel", 10); + pub_path_ = create_publisher("/global_path", 10); + pub_map_ = create_publisher("/map", + rclcpp::QoS(1).transient_local()); + + tf_broadcaster_ = std::make_shared(*this); + + if (!map_file_.empty()) { + RCLCPP_INFO(get_logger(), "Loading map: %s", map_file_.c_str()); + if (!map_.loadPGM(map_file_)) + RCLCPP_WARN(get_logger(), "Failed to load map"); + 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(1s, std::bind(&NavLiteNode::correct_localization, this)); + + RCLCPP_INFO(get_logger(), "car_nav_lite ready. /goal_pose -> /cmd_vel"); + } + +private: + std::mutex mtx_; + LaserScan latest_scan_; + double latest_vx_ = 0, latest_wz_ = 0, latest_gyro_z_ = 0; + bool has_scan_ = false; + rclcpp::Time last_odom_t_{0, 0, RCL_ROS_TIME}; + rclcpp::Time last_imu_t_ {0, 0, RCL_ROS_TIME}; + + 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_); + latest_vx_ = m->twist.twist.linear.x; + latest_wz_ = m->twist.twist.angular.z; + // Directly use absolute pose from odom (no twist integration drift) + double qx=m->pose.pose.orientation.x, qy=m->pose.pose.orientation.y; + double qz=m->pose.pose.orientation.z, qw=m->pose.pose.orientation.w; + double yaw = std::atan2(2.0*(qw*qz+qx*qy), 1.0-2.0*(qy*qy+qz*qz)); + localizer_.setPose(Pose2D(m->pose.pose.position.x, + m->pose.pose.position.y, yaw)); + } + + 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_id='%s', expecting 'map'. Ignored.", + m->header.frame_id.c_str()); + 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); + RCLCPP_INFO(get_logger(), "New goal: map(%.2f, %.2f)", g.x, g.y); + } + + void tick() { + std::lock_guard lk(mtx_); + auto pose = localizer_.pose(); publish_tf(pose); + if (has_scan_) map_.updateScan(latest_scan_, pose); + 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.pose.position.x=p.x; ps.pose.position.y=p.y; + pm.poses.push_back(ps); + } pub_path_->publish(pm); + } + static int c=0; + if (++c%60==0) RCLCPP_INFO(get_logger(),"pose=(%.2f,%.2f,%.1f) 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;i128)?100:-1);} + pub_map_->publish(m); + } + + void correct_localization() { + // Disabled until PGM static map is loaded — scan-to-map needs populated obstacles + // std::lock_guard lk(mtx_); + // if (has_scan_) localizer_.correctWithScan(latest_scan_, map_); + } + + 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); + } + + GridMap map_; Localizer localizer_; BTExecutor bt_; + rclcpp::Subscription::SharedPtr sub_scan_; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_goal_; + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_path_; + rclcpp::Publisher::SharedPtr pub_map_; + std::shared_ptr tf_broadcaster_; + rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_; + std::string map_file_; + double lookahead_ = 0.5; + bool initialized_ = false; +}; + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; +} diff --git a/include/car_nav_lite/types.hpp b/include/car_nav_lite/types.hpp new file mode 100644 index 0000000..37f1453 --- /dev/null +++ b/include/car_nav_lite/types.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include +#include +#include + +namespace car_nav_lite { + +// ── 2D Pose ───────────────────────────────────────────── +struct Pose2D { + double x = 0.0, y = 0.0, yaw = 0.0; + Pose2D() = default; + Pose2D(double x_, double y_, double yaw_) : x(x_), y(y_), yaw(yaw_) {} + double distTo(const Pose2D& o) const { + double dx = x - o.x, dy = y - o.y; + return std::sqrt(dx*dx + dy*dy); + } + double angleTo(const Pose2D& o) const { + return std::atan2(o.y - y, o.x - x); + } +}; + +// ── Twist command ─────────────────────────────────────── +struct Twist { + double vx = 0.0, wz = 0.0; +}; + +// ── LaserScan (deserialized from /scan HTTP JSON) ─────── +struct LaserScan { + double angle_min = 0.0, angle_max = 0.0, angle_increment = 0.0; + double range_min = 0.0, range_max = 0.0; + std::vector ranges; + int size() const { return (int)ranges.size(); } +}; + +// ── Grid cell index ───────────────────────────────────── +struct GridIndex { + int x = 0, y = 0; + GridIndex() = default; + GridIndex(int x_, int y_) : x(x_), y(y_) {} + bool operator==(const GridIndex& o) const { return x == o.x && y == o.y; } +}; + +// ── A* Node ───────────────────────────────────────────── +struct AStarNode { + int x, y; + int g = 0, h = 0, f = 0; + AStarNode* parent = nullptr; +}; + +// ── State enum ────────────────────────────────────────── +enum class NavState { IDLE, PLANNING, TRACKING, ARRIVED, FAILED }; + +// ── Binary occupancy grid (0-255) ─────────────────────── +using OccupancyGrid = std::vector; + +// ── Grid map parameters ───────────────────────────────── +constexpr int GRID_SIZE = 1008; // cells (5.04m @ 5mm) +constexpr double GRID_RES = 0.005; // m/cell (5mm) +constexpr double GRID_ORIGIN_X = -2.52; // world x at grid[0] (buffer for 2.5 edge) +constexpr double GRID_ORIGIN_Y = -2.52; // world y at grid[0] +constexpr int INFLATION_CELLS = 30; // 15cm expansion + +// ── Vehicle parameters ────────────────────────────────── +constexpr double WHEELBASE = 0.1682; // m (front-rear axle) +constexpr double MIN_TURN_R = 0.25; // m minimum turning radius +constexpr double DEFAULT_LOOKAHEAD = 1.2; // m Pure Pursuit lookahead + +// ── Car footprint (origincar: 30cm×19cm) ────────────── +constexpr double CAR_HALF_L = 0.15; // half-length +constexpr double CAR_HALF_W = 0.095; // half-width +constexpr double CAR_HALF_DIAG = 0.178; // sqrt(L²+W²) + +// ── Planner limits ────────────────────────────────────── +constexpr double MAX_SPEED = 4.0; // m/s +constexpr double MIN_SPEED = 0.5; // m/s (minimum for Ackermann turning) + +// ── Helper ────────────────────────────────────────────── +inline double limit(double v, double lo, double hi) { + return v < lo ? lo : (v > hi ? hi : v); +} + +} // namespace car_nav_lite diff --git a/launch/all_in_one.launch.py b/launch/all_in_one.launch.py new file mode 100644 index 0000000..5ac0a47 --- /dev/null +++ b/launch/all_in_one.launch.py @@ -0,0 +1,73 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # ── origincar_bridge params ── + declare_http_host = DeclareLaunchArgument( + "http_host", default_value="192.168.64.1", + description="simulation host IP (macOS)") + declare_http_port = DeclareLaunchArgument( + "http_port", default_value="8765") + + # ── car_nav_lite params ── + declare_map_file = DeclareLaunchArgument( + "map_file", default_value="/home/cyy/sim_map.pgm") + declare_lookahead = DeclareLaunchArgument( + "lookahead", default_value="0.8") + declare_test_mode = DeclareLaunchArgument( + "test_mode", default_value="false") + declare_foxglove = DeclareLaunchArgument( + "foxglove", default_value="true", + description="also launch foxglove_bridge") + + return LaunchDescription([ + declare_http_host, declare_http_port, + declare_map_file, declare_lookahead, declare_test_mode, declare_foxglove, + + # ── 1. HTTP→ROS2 bridge (sim ↔ nav) ── + Node( + package="origincar_bridge", + executable="bridge_node", + name="origincar_bridge", + output="screen", + parameters=[{ + "http_host": LaunchConfiguration("http_host"), + "http_port": LaunchConfiguration("http_port"), + }], + ), + + # ── 2. 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"], + ), + + # ── 3. 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"), + }], + ), + + # ── 4. Foxglove bridge (for visualization) ── + Node( + package="foxglove_bridge", + executable="foxglove_bridge", + name="foxglove_bridge", + output="screen", + parameters=[{"port": 8766}], + condition=IfCondition(LaunchConfiguration("foxglove")), + ), + ]) diff --git a/launch/nav_lite.launch.py b/launch/nav_lite.launch.py new file mode 100644 index 0000000..8561509 --- /dev/null +++ b/launch/nav_lite.launch.py @@ -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"), + }], + ), + ]) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..5242b99 --- /dev/null +++ b/package.xml @@ -0,0 +1,27 @@ + + + + car_nav_lite + 0.1.0 + Lightweight Ackermann car navigation (A* + DWA-MPC + dead-reckoning + ICP) + cyy + Apache-2.0 + + ament_cmake + + rclcpp + std_msgs + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_ros + Eigen3 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/scripts/test_goal.py b/scripts/test_goal.py new file mode 100755 index 0000000..aaefa97 --- /dev/null +++ b/scripts/test_goal.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 +"""Send a goal pose to car_nav_lite via /goal_pose topic. + +Usage: + python3 test_goal.py # default goal + python3 test_goal.py --x 1.0 --y -1.5 # custom goal + python3 test_goal.py --auto # auto-cycle multiple goals +""" + +import time +import argparse +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped + + +GOALS = [ + # (x, y) — world coords in map frame (origin at center, 5m×5m) + # Car spawns near (-2.0, -2.3) facing +x + + # Goal A: straight ahead ~3m, slight right + # Should trigger multi-pass inflation (cone at -1.58,-2.31 blocks straight path) + (0.98, -1.49), + + # Goal B: far corner + (2.0, 2.0), + + # Goal C: left side + (-1.5, 0.5), +] + + +class GoalSender(Node): + def __init__(self): + super().__init__("goal_sender") + self.pub = self.create_publisher(PoseStamped, "/goal_pose", 10) + self.idx = 0 + + def send(self, x, y, yaw=0.0): + msg = PoseStamped() + msg.header.frame_id = "map" + msg.header.stamp = self.get_clock().now().to_msg() + msg.pose.position.x = float(x) + msg.pose.position.y = float(y) + msg.pose.position.z = 0.0 + # yaw encoded as quaternion (scalar-last: qx,qy,qz,qw) + msg.pose.orientation.z = float(__import__('math').sin(yaw / 2.0)) + msg.pose.orientation.w = float(__import__('math').cos(yaw / 2.0)) + self.pub.publish(msg) + self.get_logger().info(f"Sent goal: ({x:.2f}, {y:.2f}, yaw={yaw:.2f})") + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--x", type=float, default=None) + parser.add_argument("--y", type=float, default=None) + parser.add_argument("--yaw", type=float, default=0.0) + parser.add_argument("--auto", action="store_true", + help="Auto-cycle through test goals") + parser.add_argument("--interval", type=float, default=30.0, + help="Seconds between auto goals") + args = parser.parse_args() + + rclpy.init() + node = GoalSender() + + if args.auto: + node.get_logger().info(f"Auto mode: {len(GOALS)} goals, {args.interval}s interval") + while rclpy.ok(): + x, y = GOALS[node.idx % len(GOALS)] + node.send(x, y, 0.0) + node.idx += 1 + time.sleep(args.interval) + elif args.x is not None and args.y is not None: + node.send(args.x, args.y, args.yaw) + else: + # Default: use first goal + x, y = GOALS[0] + node.get_logger().info(f"No args — sending default goal ({x:.2f}, {y:.2f})") + node.send(x, y, 0.0) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/bt_executor.cpp b/src/bt_executor.cpp new file mode 100644 index 0000000..6839855 --- /dev/null +++ b/src/bt_executor.cpp @@ -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& 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 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 diff --git a/src/bt_executor.hpp b/src/bt_executor.hpp new file mode 100644 index 0000000..57dc7af --- /dev/null +++ b/src/bt_executor.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include "car_nav_lite/types.hpp" +#include "car_nav_lite/grid_map.hpp" +#include "car_nav_lite/localizer.hpp" +#include "car_nav_lite/global_planner.hpp" +#include "car_nav_lite/local_planner.hpp" + +namespace car_nav_lite { + +class BTExecutor { +public: + BTExecutor(); + + // ── Set navigation goal ──────────────────────────── + void setGoal(const Pose2D& goal); + void setTestPath(const std::vector& path); + void setParams(double la, double ms, double mr, double sl, double cf); + bool hasGoal() const { return goal_.has_value(); } + + // ── Tick the state machine (call at 10Hz) ────────── + Twist tick(const Pose2D& pose, const GridMap& map); + + // ── Status query ─────────────────────────────────── + NavState state() const { return state_; } + const std::vector& currentPath() const { return current_path_; } + bool hasGoal() const { return goal_.has_value(); } + +private: + NavState state_ = NavState::IDLE; + std::optional goal_; + std::vector 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 diff --git a/src/calib_check.cpp b/src/calib_check.cpp new file mode 100644 index 0000000..7c7ac56 --- /dev/null +++ b/src/calib_check.cpp @@ -0,0 +1,51 @@ +#include +#include +#include +#include +#include +#include +#include + +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("/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::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()); rclcpp::shutdown(); return 0; } diff --git a/src/global_planner.cpp b/src/global_planner.cpp new file mode 100644 index 0000000..e64d790 --- /dev/null +++ b/src/global_planner.cpp @@ -0,0 +1,126 @@ +#include "car_nav_lite/global_planner.hpp" +#include +#include +#include +#include + +namespace car_nav_lite { + +GlobalPlanner::GlobalPlanner() {} + +std::vector 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(new bool[SZ * SZ]()); + + using NodePtr = AStarNode*; + auto cmp = [](NodePtr a, NodePtr b) { return a->f > b->f; }; + std::priority_queue, 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 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 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 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 GlobalPlanner::smooth(const std::vector& raw, + const GridMap& map) { + if (raw.size() <= 2) return raw; + std::vector 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 +#include +#include +#include + +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 raw(w * h); + file.read(reinterpret_cast(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 diff --git a/src/local_planner.cpp b/src/local_planner.cpp new file mode 100644 index 0000000..caeb86a --- /dev/null +++ b/src/local_planner.cpp @@ -0,0 +1,181 @@ +#include "car_nav_lite/local_planner.hpp" +#include +#include +#include +#include + +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& path, + std::vector& 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& path, + const std::vector& 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& path, + const GridMap& map) { + Twist cmd{}; + if (path.size() < 2) return cmd; + + std::vector 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 diff --git a/src/local_planner.hpp b/src/local_planner.hpp new file mode 100644 index 0000000..07da861 --- /dev/null +++ b/src/local_planner.hpp @@ -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& path, + const GridMap& map); + +private: + // ── Params ── + double ms_ = 1.5, mr_ = 0.25, sl_ = 0.6, cf_ = 6.0; + + // ── MPPI settings ── + static constexpr int K = 200; // trajectory count + static constexpr int T = 15; // timesteps + static constexpr double DT = 0.1; // step duration (1.5s horizon) + static constexpr double TEMPERATURE = 0.5; + static constexpr double VX_STD = 0.2; + static constexpr double WZ_STD = 0.8; + + // ── Cost weights (EXACT-MPPI critic ratios) ── + static constexpr double W_ALIGN = 10.0; // PathAlignCritic + static constexpr double W_FOLLOW = 5.0; // PathFollowCritic + static constexpr double W_GOAL = 5.0; // GoalCritic + static constexpr double W_OBS = 5.0; // ObstaclesCritic (proportional) + static constexpr double W_FORWARD = 3.0; // PreferForwardCritic (penalize reverse) + static constexpr double W_HEADING = 1.5; // heading-to-goal (keep low — path_align dominates) + static constexpr double W_SMOOTH = 0.5; // control smoothness + + // ── Warm-start ── + double prev_vx_ = 0.5, prev_wz_ = 0; + + // ── Simple RNG (XORshift + Box-Muller, no stdlib issues on ARM) ── + uint32_t rng_state_ = 123456789; + double randNormal(); // returns N(0,1) + + // ── Rollout one trajectory, return total cost ── + double evaluateTrajectory(const Pose2D& pose, double vx, double wz, + const std::vector& path, + const std::vector& path_dists, + double total_path_len, + const GridMap& map); + + static void computePathDistances(const std::vector& path, + std::vector& dists); +}; + +} // namespace car_nav_lite diff --git a/src/localizer.cpp b/src/localizer.cpp new file mode 100644 index 0000000..c1f3b4e --- /dev/null +++ b/src/localizer.cpp @@ -0,0 +1,185 @@ +#include "car_nav_lite/localizer.hpp" +#include "car_nav_lite/grid_map.hpp" +#include +#include +#include +#include +#include + +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(py, px) = 255; + ++hit_count; + } + } + if (hit_count < 30) return; + + // ── 2. Hough line detection ──────────────────────────── + std::vector lines; + cv::HoughLinesP(scan_img, lines, + 1, CV_PI / 180.0, // rho=1px, theta=1° + 20, // vote threshold + 40, // min line length (40px = 20cm) + 10); // max gap + + // ── 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 diff --git a/src/localizer.hpp b/src/localizer.hpp new file mode 100644 index 0000000..18d8ed9 --- /dev/null +++ b/src/localizer.hpp @@ -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 diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..f96a167 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,245 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#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& 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("/scan", 10, + std::bind(&NavLiteNode::scan_cb, this, std::placeholders::_1)); + sub_odom_ = create_subscription("/odom", 10, + std::bind(&NavLiteNode::odom_cb, this, std::placeholders::_1)); + sub_imu_ = create_subscription("/imu/data_raw", 10, + std::bind(&NavLiteNode::imu_cb, this, std::placeholders::_1)); + sub_goal_ = create_subscription("/goal_pose", 10, + std::bind(&NavLiteNode::goal_cb, this, std::placeholders::_1)); + + // ── Publishers ── + pub_cmd_ = create_publisher("/cmd_vel", 10); + pub_path_ = create_publisher("/global_path", 10); + pub_map_ = create_publisher("/map", rclcpp::QoS(1).transient_local()); + tf_broadcaster_ = std::make_shared(*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 "); + 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::SharedPtr sub_scan_; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_goal_; + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_path_; + rclcpp::Publisher::SharedPtr pub_map_; + std::shared_ptr 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 _genTestPath(){ + std::vector 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{{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 dense; + for(size_t i=0;i+160){ + 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;i128)?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());rclcpp::shutdown();return 0;} diff --git a/src/nav_lite.launch.py b/src/nav_lite.launch.py new file mode 100644 index 0000000..8561509 --- /dev/null +++ b/src/nav_lite.launch.py @@ -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"), + }], + ), + ]) diff --git a/src/types.hpp b/src/types.hpp new file mode 100644 index 0000000..5b9aa0d --- /dev/null +++ b/src/types.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include +#include +#include + +namespace car_nav_lite { + +// ── 2D Pose ───────────────────────────────────────────── +struct Pose2D { + double x = 0.0, y = 0.0, yaw = 0.0; + Pose2D() = default; + Pose2D(double x_, double y_, double yaw_) : x(x_), y(y_), yaw(yaw_) {} + double distTo(const Pose2D& o) const { + double dx = x - o.x, dy = y - o.y; + return std::sqrt(dx*dx + dy*dy); + } + double angleTo(const Pose2D& o) const { + return std::atan2(o.y - y, o.x - x); + } +}; + +// ── Twist command ─────────────────────────────────────── +struct Twist { + double vx = 0.0, wz = 0.0; +}; + +// ── LaserScan (deserialized from /scan HTTP JSON) ─────── +struct LaserScan { + double angle_min = 0.0, angle_max = 0.0, angle_increment = 0.0; + double range_min = 0.0, range_max = 0.0; + std::vector ranges; + int size() const { return (int)ranges.size(); } +}; + +// ── Grid cell index ───────────────────────────────────── +struct GridIndex { + int x = 0, y = 0; + GridIndex() = default; + GridIndex(int x_, int y_) : x(x_), y(y_) {} + bool operator==(const GridIndex& o) const { return x == o.x && y == o.y; } +}; + +// ── A* Node ───────────────────────────────────────────── +struct AStarNode { + int x, y; + int g = 0, h = 0, f = 0; + AStarNode* parent = nullptr; +}; + +// ── State enum ────────────────────────────────────────── +enum class NavState { IDLE, PLANNING, TRACKING, ARRIVED, FAILED }; + +// ── Binary occupancy grid (0-255) ─────────────────────── +using OccupancyGrid = std::vector; + +// ── Grid map parameters ───────────────────────────────── +constexpr int GRID_SIZE = 1008; // cells (5.04m @ 5mm) +constexpr double GRID_RES = 0.005; // m/cell (5mm) +constexpr double GRID_ORIGIN_X = -2.52; // world x at grid[0] (buffer for 2.5 edge) +constexpr double GRID_ORIGIN_Y = -2.52; // world y at grid[0] +constexpr int INFLATION_CELLS = 30; // 15cm (covers car half-diagonal + margin for A*) + +// ── Vehicle parameters ────────────────────────────────── +constexpr double WHEELBASE = 0.1682; // m (front-rear axle) +constexpr double MIN_TURN_R = 0.25; // m minimum turning radius +constexpr double DEFAULT_LOOKAHEAD = 1.2; // m Pure Pursuit lookahead + +// ── Car footprint (origincar: 30cm×19cm) ────────────── +constexpr double CAR_HALF_L = 0.15; // half-length +constexpr double CAR_HALF_W = 0.095; // half-width +constexpr double CAR_HALF_DIAG = 0.178; // sqrt(L²+W²) + +// ── Planner limits ────────────────────────────────────── +constexpr double MAX_SPEED = 4.0; // m/s +constexpr double MIN_SPEED = 0.5; // m/s (minimum for Ackermann turning) + +// ── Helper ────────────────────────────────────────────── +inline double limit(double v, double lo, double hi) { + return v < lo ? lo : (v > hi ? hi : v); +} + +} // namespace car_nav_lite