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

Critical fixes #1-8: wall coords, calib reset, cmd=0 guard, fence marking, loc_conf, params callback, CMake maps

Browse Source 
- 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
This commit is contained in:
cyy
2026-07-03 07:14:57 +00:00
parent 7c07eb66c8
commit 85598eaa51
6 changed files with 53 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/main.cpp
View File

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