initial

examples/fix_initial_state.py

@@ -0,0 +1,41 @@
+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!")