42 lines
1.6 KiB
Python
42 lines
1.6 KiB
Python
import sys
|
|
|
|
# Read the file
|
|
with open('qt_gui_demo.cpp', 'r', encoding='utf-8') as f:
|
|
lines = f.readlines()
|
|
|
|
# Find and replace the initial state section (around line 448-451)
|
|
new_lines = []
|
|
skip_count = 0
|
|
for i, line in enumerate(lines):
|
|
if skip_count > 0:
|
|
skip_count -= 1
|
|
continue
|
|
|
|
if i >= 447 and 'Set up tracker' in line:
|
|
# Add the original comment
|
|
new_lines.append(line)
|
|
# Add the next line (setReferencePath)
|
|
new_lines.append(lines[i+1])
|
|
# Add blank line
|
|
new_lines.append('\n')
|
|
# Add the new initial state code
|
|
new_lines.append(' // 修复: 从路径起点获取初始状态，确保完美匹配\n')
|
|
new_lines.append(' const auto& path_points = path.getPathPoints();\n')
|
|
new_lines.append(' AGVModel::State initial_state;\n')
|
|
new_lines.append(' if (!path_points.empty()) {\n')
|
|
new_lines.append(' const PathPoint& start = path_points[0];\n')
|
|
new_lines.append(' initial_state = AGVModel::State(start.x, start.y, start.theta);\n')
|
|
new_lines.append(' } else {\n')
|
|
new_lines.append(' initial_state = AGVModel::State(0.0, 0.0, 0.0);\n')
|
|
new_lines.append(' }\n')
|
|
new_lines.append(' tracker_->setInitialState(initial_state);\n')
|
|
skip_count = 3 # Skip the next 3 lines (setReferencePath, old initial_state, setInitialState)
|
|
else:
|
|
new_lines.append(line)
|
|
|
|
# Write back
|
|
with open('qt_gui_demo.cpp', 'w', encoding='utf-8') as f:
|
|
f.writelines(new_lines)
|
|
|
|
print("Initial state fix applied successfully!")
|