From 85598eaa514837448dba487fee73496155da478c Mon Sep 17 00:00:00 2001 From: cyy Date: Fri, 3 Jul 2026 07:14:57 +0000 Subject: [PATCH] Critical fixes #1-8: wall coords, calib reset, cmd=0 guard, fence marking, loc_conf, params callback, CMake maps MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - SessionMapper: fix wall line construction (0/w/h → x_min/x_max/y_min/y_max) - session_mapper.reset() on calibration failure - CALIBRATING state forces cmd_vel=0 - GridMap::markFenceModel() draws calibrated fence into static_cells_ - /nav_metrics now publishes loc_conf - add_on_set_parameters_callback handles all new params - CMake maps install guarded with EXISTS check --- CMakeLists.txt | 5 ++++- include/car_nav_lite/grid_map.hpp | 2 ++ include/car_nav_lite/session_mapper.hpp | 1 + src/grid_map.cpp | 24 ++++++++++++++++++++++++ src/main.cpp | 21 +++++++++++++++++---- src/session_mapper.cpp | 16 +++++----------- 6 files changed, 53 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 22fc126..93c9b89 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,6 +42,9 @@ 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}) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/maps") + install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) +endif() ament_package() diff --git a/include/car_nav_lite/grid_map.hpp b/include/car_nav_lite/grid_map.hpp index 4c91aa5..59d3b90 100644 --- a/include/car_nav_lite/grid_map.hpp +++ b/include/car_nav_lite/grid_map.hpp @@ -1,6 +1,7 @@ #pragma once #include "car_nav_lite/types.hpp" +#include "car_nav_lite/fence_model.hpp" #include #include @@ -54,6 +55,7 @@ public: // ── Pre-built obstacle marks ─────────────────────── void markCircle(double wx, double wy, double radius, uint8_t value = 255); void markCone(double wx, double wy, double radius, uint8_t value = 255); + void markFenceModel(const FenceModel& fence, double thickness = 0.04); void markRect(double x1, double y1, double x2, double y2, uint8_t value = 255); // ── Getters ──────────────────────────────────────── diff --git a/include/car_nav_lite/session_mapper.hpp b/include/car_nav_lite/session_mapper.hpp index 0741995..25ef420 100644 --- a/include/car_nav_lite/session_mapper.hpp +++ b/include/car_nav_lite/session_mapper.hpp @@ -20,6 +20,7 @@ public: double expected_size = 5.0, double size_tol = 0.35, double ransac_thresh = 0.04); + void reset() { points_world_.clear(); scan_count_ = 0; } int scanCount() const { return scan_count_; } const std::vector& points() const { return points_world_; } diff --git a/src/grid_map.cpp b/src/grid_map.cpp index d2e5eec..e69c5af 100644 --- a/src/grid_map.cpp +++ b/src/grid_map.cpp @@ -239,6 +239,30 @@ void GridMap::markCircle(double wx, double wy, double radius, uint8_t value) { } } +void GridMap::markFenceModel(const FenceModel& fence, double thickness) { + if (!fence.valid) return; + // Build four corners from boundary positions (already world-aligned) + double lx = fence.left_x, rx = fence.right_x; + double by = fence.bottom_y, ty = fence.top_y; + // Draw four edges + struct Seg { double x1,y1,x2,y2; }; + Seg segs[] = {{lx,by,lx,ty}, {rx,by,rx,ty}, {lx,by,rx,by}, {lx,ty,rx,ty}}; + int t = (int)(thickness / GRID_RES) + 1; + for (auto& s : segs) { + int gx1, gy1, gx2, gy2; + if (!worldToGrid(s.x1,s.y1,gx1,gy1) || !worldToGrid(s.x2,s.y2,gx2,gy2)) continue; + int dx=std::abs(gx2-gx1), dy=-std::abs(gy2-gy1); + int sx=gx1=dy){err+=dy;x+=sx;}if(e2<=dx){err+=dx;y+=sy;} + } + } +} + void GridMap::markCone(double wx, double wy, double radius, uint8_t value) { int cx, cy, cr; if (!worldToGrid(wx, wy, cx, cy)) return; diff --git a/src/main.cpp b/src/main.cpp index 0f9367c..59e3a6e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -58,6 +58,12 @@ public: 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; } + else if (n=="calib_scan_count") calib_scan_count_=p.as_int(); + else if (n=="fence_expected_size") fence_expected_size_=p.as_double(); + else if (n=="fence_size_tolerance") fence_size_tolerance_=p.as_double(); + else if (n=="wall_ransac_thresh") wall_ransac_thresh_=p.as_double(); + else if (n=="cone_mark_radius") cone_mark_radius_=p.as_double(); + else if (n=="dynamic_obstacle_enable") dynamic_obstacle_enable_=p.as_bool(); } RCLCPP_INFO(get_logger(),"params: la=%.2f ms=%.1f mr=%.2f sl=%.2f cf=%.1f test=%d", la_,ms_,mr_,sl_,cf_,test_); @@ -167,13 +173,14 @@ private: if (session_mapper_.build(fence_model_, cones, fence_expected_size_, fence_size_tolerance_, wall_ransac_thresh_)) { localizer_.setFenceModel(fence_model_); for (auto& c : cones) map_.markCone(c.x(), c.y(), cone_mark_radius_, 255); + map_.markFenceModel(fence_model_); map_.morphologyClose(3); map_.freezeSessionMap(); session_state_ = READY; fprintf(stderr, "[Session] READY — fence %.2fx%.2f yaw=%.1f°, %zu cones\n", fence_model_.width, fence_model_.height, fence_model_.yaw_field*57.3, cones.size()); } else { - calib_cnt_ = 0; + calib_cnt_ = 0; session_mapper_.reset(); fprintf(stderr, "[Session] calibration failed, retrying...\n"); } } @@ -241,9 +248,15 @@ private: void tick(){ std::lock_guard lk(mtx_); + // CALIBRATING: force zero cmd, wait for fence model + if (session_state_ == CALIBRATING) { + auto zero = geometry_msgs::msg::Twist(); + pub_cmd_->publish(zero); + return; + } auto pose=localizer_.pose();publish_tf(pose); // READY: only update scan if dynamic obstacles enabled - if(has_scan_ && (session_state_==CALIBRATING || dynamic_obstacle_enable_)) + if(has_scan_ && dynamic_obstacle_enable_) map_.updateScan(latest_scan_,pose); // Test mode: auto-activate figure-8 after 3s @@ -301,10 +314,10 @@ private: char buf[512]; snprintf(buf, sizeof(buf), "{\"state\":\"%s\",\"x\":%.2f,\"y\":%.2f,\"yaw\":%.1f," - "\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f," + "\"loc_conf\":%.2f,\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f," "\"mppi_alive\":%d,\"mppi_total\":%d,\"mppi_minC\":%.1f}", m.state, m.pose_x, m.pose_y, m.pose_yaw*57.3, - m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3, + localizer_.loc_conf_, m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3, m.mppi_alive, m.mppi_total, m.mppi_min_cost); std_msgs::msg::String msg; msg.data = buf; pub_metrics_->publish(msg); diff --git a/src/session_mapper.cpp b/src/session_mapper.cpp index 3eba6cf..30e3304 100644 --- a/src/session_mapper.cpp +++ b/src/session_mapper.cpp @@ -116,20 +116,14 @@ bool SessionMapper::build(FenceModel& fence, std::vector& cones fence.left_x = lw.x(); fence.right_x = rw.x(); fence.bottom_y = bw.y(); fence.top_y = tw.y(); - // Left wall: field-x = x_min, direction = field-y (0, 1) - Eigen::Vector2d lb = rotBack(x_min, 0), lt = rotBack(x_min, h); + // Build fence lines from field-aligned extents (NOT from 0/w/h!) + Eigen::Vector2d lb = rotBack(x_min, y_min), lt = rotBack(x_min, y_max); fence.left.setFromTwoPoints(lb.x(), lb.y(), lt.x(), lt.y()); - - // Right wall - Eigen::Vector2d rb = rotBack(x_max, 0), rt = rotBack(x_max, h); + Eigen::Vector2d rb = rotBack(x_max, y_min), rt = rotBack(x_max, y_max); fence.right.setFromTwoPoints(rb.x(), rb.y(), rt.x(), rt.y()); - - // Bottom wall: field-y = y_min - Eigen::Vector2d bb = rotBack(0, y_min), br = rotBack(w, y_min); + Eigen::Vector2d bb = rotBack(x_min, y_min), br = rotBack(x_max, y_min); fence.bottom.setFromTwoPoints(bb.x(), bb.y(), br.x(), br.y()); - - // Top wall - Eigen::Vector2d tb = rotBack(0, y_max), tr_ = rotBack(w, y_max); + Eigen::Vector2d tb = rotBack(x_min, y_max), tr_ = rotBack(x_max, y_max); fence.top.setFromTwoPoints(tb.x(), tb.y(), tr_.x(), tr_.y()); fence.valid = true;