14 lines
540 B
Plaintext
14 lines
540 B
Plaintext
// Set up tracker
|
|
tracker_->setReferencePath(path);
|
|
|
|
// 修复: 从路径起点获取初始状态，确保完美匹配
|
|
const auto& path_points = path.getPathPoints();
|
|
AGVModel::State initial_state;
|
|
if (!path_points.empty()) {
|
|
const PathPoint& start = path_points[0];
|
|
initial_state = AGVModel::State(start.x, start.y, start.theta);
|
|
} else {
|
|
initial_state = AGVModel::State(0.0, 0.0, 0.0);
|
|
}
|
|
tracker_->setInitialState(initial_state);
|