diff --git a/src/main.cpp b/src/main.cpp index d4954dc..47c1067 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -203,7 +203,7 @@ private: 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;} + 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); 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); @@ -248,13 +248,12 @@ private: void tick(){ std::lock_guard lk(mtx_); + auto pose=localizer_.pose();publish_tf(pose); // CALIBRATING: force zero cmd, wait for fence model if (session_state_ == CALIBRATING) { - auto zero = geometry_msgs::msg::Twist(); - pub_cmd_->publish(zero); + pub_cmd_->publish(geometry_msgs::msg::Twist()); return; } - auto pose=localizer_.pose();publish_tf(pose); // READY: only update scan if dynamic obstacles enabled if(has_scan_ && dynamic_obstacle_enable_) map_.updateScan(latest_scan_,pose); diff --git a/src/session_mapper.cpp b/src/session_mapper.cpp index 00c45f1..da37e19 100644 --- a/src/session_mapper.cpp +++ b/src/session_mapper.cpp @@ -89,12 +89,16 @@ bool SessionMapper::build(FenceModel& fence, std::vector& cones double y_min = ys[lo_idx], y_max = ys[hi_idx]; 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 || std::abs(h - expected_size) > size_tol) { - fprintf(stderr, "[SessionMapper] size mismatch: %.2fx%.2f (expected %.1f±%.1f)\n", - w, h, expected_size, size_tol); - return false; + fprintf(stderr, "[SessionMapper] partial fence %.2fx%.2f, fallback to known %.1fx%.1f\n", + w, h, expected_size, expected_size); + // 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;