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

Fix: TF during calibrating, goal frame debug, known-size fallback for partial fence

Browse Source
This commit is contained in:
cyy
2026-07-03 08:00:33 +00:00
parent ed23cf5482
commit 4ad21bba6a
2 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/main.cpp
View File

@@ -203,7 +203,7 @@ private:
last_imu_t_=now;latest_gyro_z_=m->angular_velocity.z; last_imu_t_=now;latest_gyro_z_=m->angular_velocity.z;
if(dt>0&&dt<1.0)localizer_.updateIMU(latest_gyro_z_,dt);} if(dt>0&&dt<1.0)localizer_.updateIMU(latest_gyro_z_,dt);}
void goal_cb(PoseStMsg::SharedPtr m){std::lock_guard lk(mtx_); 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;} if(m->header.frame_id!="map"){RCLCPP_WARN(get_logger(),"goal frame not map: '%s'",m->header.frame_id.c_str());return;}
Pose2D g(m->pose.position.x,m->pose.position.y,0.0); Pose2D g(m->pose.position.x,m->pose.position.y,0.0);
double qw=m->pose.orientation.w,qz=m->pose.orientation.z; 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); if(qw!=0||qz!=0)g.yaw=std::atan2(2.0*qw*qz,qw*qw-qz*qz);
@@ -248,13 +248,12 @@ private:
void tick(){ void tick(){
std::lock_guard lk(mtx_); std::lock_guard lk(mtx_);
auto pose=localizer_.pose();publish_tf(pose);
// CALIBRATING: force zero cmd, wait for fence model // CALIBRATING: force zero cmd, wait for fence model
if (session_state_ == CALIBRATING) { if (session_state_ == CALIBRATING) {
auto zero = geometry_msgs::msg::Twist(); pub_cmd_->publish(geometry_msgs::msg::Twist());
pub_cmd_->publish(zero);
return; return;
} }
auto pose=localizer_.pose();publish_tf(pose);
// READY: only update scan if dynamic obstacles enabled // READY: only update scan if dynamic obstacles enabled
if(has_scan_ && dynamic_obstacle_enable_) if(has_scan_ && dynamic_obstacle_enable_)
map_.updateScan(latest_scan_,pose); map_.updateScan(latest_scan_,pose);

12
src/session_mapper.cpp
View File

@@ -89,12 +89,16 @@ bool SessionMapper::build(FenceModel& fence, std::vector<Eigen::Vector2d>& cones
double y_min = ys[lo_idx], y_max = ys[hi_idx]; double y_min = ys[lo_idx], y_max = ys[hi_idx];
double w = x_max - x_min, h = y_max - y_min; double w = x_max - x_min, h = y_max - y_min;
// Validate size // Validate size — allow known-size fallback if partial fence
if (std::abs(w - expected_size) > size_tol || if (std::abs(w - expected_size) > size_tol ||
std::abs(h - expected_size) > size_tol) { std::abs(h - expected_size) > size_tol) {
fprintf(stderr, "[SessionMapper] size mismatch: %.2fx%.2f (expected %.1f±%.1f)\n", fprintf(stderr, "[SessionMapper] partial fence %.2fx%.2f, fallback to known %.1fx%.1f\n",
w, h, expected_size, size_tol); w, h, expected_size, expected_size);
return false; // Use known field size centered at origin with estimated yaw
double half = expected_size / 2.0;
x_min = -half; x_max = half;
y_min = -half; y_max = half;
w = h = expected_size;
} }
fence.width = w; fence.height = h; fence.width = w; fence.height = h;