Stanley control algorithm add

For real vehicle test

.gitignore

@@ -11,3 +11,6 @@ log/
 logs/
 data/
 files/
+*.csv
+*.png
+

README.md

@@ -1,2 +0,0 @@
-auto drive misson center webots simulation:
-

Readme.md

@@ -0,0 +1,40 @@
+# auto drive misson center webots simulation:
+该分支修复了原LQR算法中矩阵错误问题，并对参数进行调教，增加stanley前轮转向控制算法，并适配webots仿真环境。后续实车测试优先使用该分支
+## 1, `ubuntu20.04`, `ubuntu22.04`
+## 2, 安装 `webots`
+`https://github.com/cyberbotics/webots/tags`，  版本： `ubuntu20.04： R2023a， ubuntu22.04：R2025a`
+## 3, 创建ros2 工作空间
+如  `~/sweet_ws `, 编译 `sweet_webots_robot,  robots_rviz_panel, webots_simulation, auto_drive_proj_utils,auto_drive_proj_msg, auto_drive_misson_center`
+```shell
+cd ~/sweet_ws
+colcon build --packages-select sweet_webots_robot --cmake-clean-cache
+source install/setup.bash
+colcon build --packages-select robots_rviz_panel --cmake-clean-cache
+source install/setup.bash
+colcon build --packages-select webots_simulation --cmake-clean-cache
+source install/setup.bash
+colcon build --packages-select auto_drive_proj_utils --cmake-clean-cache 
+source install/setup.bash
+colcon build --packages-select auto_drive_proj_msg --cmake-clean-cache 
+source install/setup.bash
+colcon build --packages-select auto_drive_misson_center --cmake-clean-cache 
+source install/setup.bash
+```
+## 4, 运行 webots: 
+终端执行命令： `webots`
+## 5, 运行 webots 与 rviz可视化界面
+`ros2 launch webots_simulation robot_launch.py` ，等待rviz2界面打开后，加载配置文件：` File->Open Config->` 选择  `SweetAiRviz.rviz` ，方便起见，打开后，File->Save Config 下次不用再重复加载。该 rviz界面 左右两侧拉开留出视图空间。(test:1701895544295370752.json)
+测试版本中path_visualizer pkg目前与launch一起启动，其路径代码可在src/webots_simulation/src/PathVisualizer.cpp 中修改file_name实现
+## 6, 模拟 `/start_pose` 服务
+另启一个终端，模拟 `/start_pose` 服务，常开，执行脚本(注意路径)：`python3 /home/ada/sweet_ws/install/auto_drive_misson_center/share/auto_drive_misson_center/scripts/mock_start_pose_and_publish_rtk.py `
+## 7, 运行 mission_center 任务节点
+运行 `ros2 launch auto_drive_misson_center mission_center_launch.py` 启动 任务节点。
+## 8, 请求 /invoke/missionOn 服务
+另启一个终端，执行发送任务命令，这里的任务，必须和 `5` 中的匹配，将1701895544295370752.json内容填充到指令中：
+```shell
+ros2 service call /invoke/missionOn auto_drive_proj_msg/srv/ReqString "{request: '[{\"name\":\"value\",\"value\":\"https://test.sweetai.cn:4433/load/locationCmdPath/1701895544295370752.json\"}]', value: ''}"
+```
+> 观察任务轨迹以及rviz2右侧panel面板参数
+
+rviz2 panel：
+    ![界面示意图](./rviz2.jpg) 

lqr_plot_path_tracking_error.py

@@ -0,0 +1,355 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+"""
+读取当前目录下：
+    mission_center_logger.txt
+    mission_center_logger1.txt
+    mission_center_logger2.txt
+    ...
+
+从 [path_follow] 日志中提取：
+    LQR: dis=..., alpha=...
+    path controller, current pose point:x:..., y:..., yaw:...
+
+绘制：
+    1. 横向误差 dis 随实际行驶路程变化
+    2. 航向误差 alpha 随实际行驶路程变化
+
+说明：
+- dis：按你日志中的 LQR dis，单位通常是 m
+- alpha：按你日志中的 alpha，单位是 rad
+- 路程：根据连续 current pose 的 x/y 计算累计行驶距离
+- 日志中同一时刻可能重复打印多次，因此脚本会保留数据，
+  但累计距离对相同位置不会增加。
+"""
+
+import re
+from pathlib import Path
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+# ============================================================
+# 1. 日志文件
+# ============================================================
+
+LOG_PATTERN = "logs/mission_center_logger*.txt"
+
+files = sorted(Path(".").glob(LOG_PATTERN))
+
+if not files:
+    raise FileNotFoundError(
+        f"当前目录没有找到 {LOG_PATTERN}"
+    )
+
+print("找到日志文件：")
+for f in files:
+    print("  ", f)
+
+
+# ============================================================
+# 2. 正则表达式
+# ============================================================
+
+# LQR:
+# [LQR: dis=0.001038186, alpha=-0.003193076, ...
+lqr_pattern = re.compile(
+    r"\[LQR:\s*"
+    r"dis=([-+0-9.eE]+),\s*"
+    r"alpha=([-+0-9.eE]+)"
+)
+
+# current pose:
+# [path controller, current pose point:x:24.68, y:1.16, yaw:0.026;
+pose_pattern = re.compile(
+    r"current pose point:x:([-+0-9.eE]+),\s*"
+    r"y:([-+0-9.eE]+),\s*"
+    r"yaw:([-+0-9.eE]+)"
+)
+
+yaw_command_pattern = re.compile(
+    r"PP output angular=([-+0-9.eE]+)"
+)
+
+# ============================================================
+# 3. 读取日志
+# ============================================================
+
+all_data = []
+
+for file in files:
+    print(f"\n读取：{file}")
+
+    pending_lqr = None
+    pending_yaw = None
+
+    with file.open("r", encoding="utf-8", errors="ignore") as f:
+        for line in f:
+
+            # ---- LQR误差 ----
+            m_lqr = lqr_pattern.search(line)
+            if m_lqr:
+                dis = float(m_lqr.group(1))
+                alpha = float(m_lqr.group(2))
+
+                pending_lqr = {
+                    "dis": dis,
+                    "alpha": alpha,
+                }
+
+            # ---- 当前车辆位置 ----
+            m_pose = pose_pattern.search(line)
+            m_yaw = yaw_command_pattern.search(line)
+            if m_yaw:
+                pending_yaw = float(m_yaw.group(1))
+            if m_pose and pending_lqr is not None:
+
+                x = float(m_pose.group(1))
+                y = float(m_pose.group(2))
+                yaw = float(m_pose.group(3))
+
+
+                all_data.append({
+                    "file": file.name,
+                    "x": x,
+                    "y": y,
+                    "yaw": yaw,
+                    "dis": pending_lqr["dis"],
+                    "alpha": pending_lqr["alpha"],
+                    "yaw_command": pending_yaw,
+                })
+
+                pending_lqr = None
+
+
+if not all_data:
+    raise RuntimeError(
+        "没有解析到数据，请检查日志格式是否和示例一致。"
+    )
+
+
+print(f"\n总共解析 {len(all_data)} 个数据点")
+
+
+# ============================================================
+# 4. 根据 x/y 计算实际行驶路程
+# ============================================================
+
+x = np.array([d["x"] for d in all_data])
+y = np.array([d["y"] for d in all_data])
+
+dis = np.array([d["dis"] for d in all_data])
+alpha = np.array([d["alpha"] for d in all_data])
+yaw_command = np.array([d["yaw_command"] for d in all_data])
+
+# 连续位置之间的距离
+step_distance = np.sqrt(
+    np.diff(x) ** 2 +
+    np.diff(y) ** 2
+)
+
+# 累计路程
+distance = np.zeros(len(x))
+
+if len(x) > 1:
+    distance[1:] = np.cumsum(step_distance)
+
+
+# ============================================================
+# 5. 如果希望横向误差取绝对值，可以取消下面注释
+# ============================================================
+
+# dis = np.abs(dis)
+
+# 注意：
+# 不建议默认取绝对值。
+# 保留正负号可以观察车辆是在路径左侧还是右侧。
+
+
+# ============================================================
+# 6. 输出基本统计信息
+# ============================================================
+
+print("\n========== 误差统计 ==========")
+
+print(
+    f"总行驶距离      : {distance[-1]:.3f} m"
+)
+
+print(
+    f"横向误差最大值  : {np.max(dis):.6f} m"
+)
+
+print(
+    f"横向误差最小值  : {np.min(dis):.6f} m"
+)
+
+print(
+    f"横向误差绝对最大: {np.max(np.abs(dis)):.6f} m"
+)
+
+print(
+    f"横向误差 RMS    : {np.sqrt(np.mean(dis ** 2)):.6f} m"
+)
+
+print(
+    f"航向误差最大值  : {np.max(alpha):.6f} rad "
+    f"({np.degrees(np.max(alpha)):.3f} deg)"
+)
+
+print(
+    f"航向误差最小值  : {np.min(alpha):.6f} rad "
+    f"({np.degrees(np.min(alpha)):.3f} deg)"
+)
+
+print(
+    f"航向误差绝对最大: {np.max(np.abs(alpha)):.6f} rad "
+    f"({np.degrees(np.max(np.abs(alpha))):.3f} deg)"
+)
+
+print(
+    f"航向误差 RMS    : {np.sqrt(np.mean(alpha ** 2)):.6f} rad "
+    f"({np.degrees(np.sqrt(np.mean(alpha ** 2))):.3f} deg)"
+)
+
+
+# ============================================================
+# 7. 转换航向误差为角度
+# ============================================================
+
+alpha_deg = np.degrees(alpha)
+
+
+# ============================================================
+# 8. 图1：横向误差
+# ============================================================
+
+plt.figure(figsize=(12, 5))
+
+plt.plot(
+    distance,
+    dis,
+    linewidth=2.0
+)
+
+plt.axhline(
+    0.0,
+    linestyle="--",
+    linewidth=0.8
+)
+
+plt.xlabel("Travel Distance (m)")
+plt.ylabel("Lateral Error dis (m)")
+plt.title("Lateral Error vs Travel Distance")
+plt.grid(True, alpha=0.3)
+
+plt.tight_layout()
+
+plt.savefig(
+    "lateral_error_vs_distance.png",
+    dpi=200
+)
+
+plt.show()
+
+
+# ============================================================
+# 9. 图2：航向误差
+# ============================================================
+
+plt.figure(figsize=(12, 5))
+
+plt.plot(
+    distance,
+    alpha_deg,
+    linewidth=2.0
+)
+
+plt.axhline(
+    0.0,
+    linestyle="--",
+    linewidth=0.8
+)
+
+plt.xlabel("Travel Distance (m)")
+plt.ylabel("Heading Error alpha (deg)")
+plt.title("Heading Error vs Travel Distance")
+plt.grid(True, alpha=0.3)
+
+plt.tight_layout()
+
+plt.savefig(
+    "heading_error_vs_distance.png",
+    dpi=200
+)
+
+plt.show()
+
+# ============================================================
+# 13. 航向误差 rad
+# ============================================================
+
+plt.figure(
+    figsize=(12, 5)
+)
+
+plt.plot(
+    distance,
+    yaw_command,
+    linewidth=1.2
+)
+
+plt.axhline(
+    0.0,
+    linestyle="--",
+    linewidth=0.8
+)
+
+plt.xlabel(
+    "Distance (m)"
+)
+
+plt.ylabel(
+    "Yaw Command (rad)"
+)
+
+plt.title(
+    "Recalculated Yaw Command"
+)
+
+plt.grid(True)
+
+plt.tight_layout()
+
+plt.show()
+# ============================================================
+# 10. 同时保存 CSV，方便后续 MATLAB / Excel 分析
+# ============================================================
+
+output = np.column_stack(
+    (
+        distance,
+        x,
+        y,
+        dis,
+        alpha,
+        alpha_deg,
+        yaw_command,
+    )
+)
+
+np.savetxt(
+    "path_tracking_error.csv",
+    output,
+    delimiter=",",
+    header="distance_m,x_m,y_m,lateral_error_m,heading_error_rad,heading_error_deg,yaw_command",
+    comments="",
+    fmt="%.8f"
+)
+
+print("\n输出文件：")
+print("  lateral_error_vs_distance.png")
+print("  heading_error_vs_distance.png")
+print("  path_tracking_error.csv")

pp_lqr_stanley_error.py

@@ -0,0 +1,163 @@
+import csv
+import os
+import matplotlib.pyplot as plt
+import numpy as np
+
+files = [
+    "U_turn_0.32_lqr.csv",
+    "U_turn_0.32_pp.csv",
+    "U_turn_0.32_stanley.csv",
+]
+
+data = {}
+
+for file in files:
+    if not os.path.exists(file):
+        raise FileNotFoundError(f"找不到文件: {file}")
+
+    distance = []
+    lateral_error = []
+    heading_error = []
+    yaw_command = []
+
+    with open(file, "r", encoding="utf-8-sig", newline="") as f:
+        reader = csv.DictReader(f)
+
+        required = {"distance_m", "lateral_error_m", "heading_error_rad","yaw_command"}
+        if not required.issubset(reader.fieldnames or []):
+            raise ValueError(
+                f"{file} 缺少必要列，需要: {required}，实际列: {reader.fieldnames}"
+            )
+
+        for row in reader:
+            try:
+                distance.append(float(row["distance_m"]))
+                lateral_error.append(abs(float(row["lateral_error_m"])))
+                heading_error.append(abs(float(row["heading_error_rad"])))
+                yaw_command.append(float(row["yaw_command"]))
+            except (ValueError, TypeError):
+                continue
+
+    name = os.path.splitext(os.path.basename(file))[0]
+    data[name] = {
+        "distance": distance,
+        "lateral_error": lateral_error,
+        "heading_error": heading_error,
+        "yaw_command": yaw_command,
+    }
+
+# 颜色由 matplotlib 自动分配，不手动指定
+names = list(data.keys())
+
+# =========================
+# 图1：Lateral Error
+# =========================
+plt.figure(figsize=(12, 6))
+
+for name in names:
+    plt.plot(
+        data[name]["distance"],
+        data[name]["lateral_error"],
+        label=name,
+        linewidth=1.5,
+    )
+
+
+plt.xlabel("Distance (m)")
+plt.ylabel("Lateral Error (m)")
+plt.title("Lateral Error Comparison")
+plt.grid(True, alpha=0.3)
+plt.legend(["LQR", "Pure Pursuit", "Stanley"])
+plt.tight_layout()
+
+lateral_output = "U_turn_lateral_error_comparison.png"
+plt.savefig(lateral_output, dpi=200)
+plt.show()
+
+# =========================
+# 图2：Heading Error
+# =========================
+plt.figure(figsize=(12, 6))
+
+for name in names:
+    plt.plot(
+        data[name]["distance"],
+        data[name]["heading_error"],
+        label=name,
+        linewidth=1.5,
+    )
+
+plt.axhline(0, linewidth=2.0)
+plt.xlabel("Distance (m)")
+plt.ylabel("Heading Error (rad)")
+plt.title("Heading Error Comparison")
+plt.grid(True, alpha=0.3)
+plt.legend(["LQR", "Pure Pursuit", "Stanley"])
+plt.tight_layout()
+
+heading_output = "U_turn_heading_error_comparison.png"
+plt.savefig(heading_output, dpi=200)
+plt.show()
+
+
+# =========================
+# 图3：Yaw Command
+# =========================
+plt.figure(figsize=(12, 6))
+
+for name in names:
+    plt.plot(
+        data[name]["distance"],
+        data[name]["yaw_command"],
+        label=name,
+        linewidth=1.5,
+    )
+
+plt.axhline(0, linewidth=2.0)
+plt.xlabel("Distance (m)")
+plt.ylabel("Yaw Command (rad)")
+plt.title("Yaw Command Comparison")
+plt.grid(True, alpha=0.3)
+plt.legend(["LQR", "Pure Pursuit", "Stanley"])
+plt.tight_layout()
+
+yaw_output = "U_turn_yaw_command_comparison.png"
+plt.savefig(yaw_output, dpi=200)
+plt.show()
+print("读取完成：")
+for name in names:
+    print(f"{name}: {len(data[name]['distance'])} points")
+
+print("\n已保存：")
+print(lateral_output)
+print(heading_output)
+print(yaw_output)
+
+
+print("\n" + "=" * 100)
+print(
+    f"{'Controller':<20}"
+    f"{'Lat Max(m)':>12}"
+    f"{'Lat Mean(m)':>13}"
+    f"{'Lat RMSE(m)':>14}"
+    f"{'Lat P95(m)':>13}"
+    f"{'Head Max(deg)':>15}"
+    f"{'Head RMSE(deg)':>16}"
+)
+print("=" * 100)
+
+for name in names:
+    lat = np.array(data[name]["lateral_error"])
+    head = np.array(data[name]["heading_error"])
+
+    print(
+        f"{name:<20}"
+        f"{np.max(lat):>12.4f}"
+        f"{np.mean(lat):>13.4f}"
+        f"{np.sqrt(np.mean(lat**2)):>14.4f}"
+        f"{np.percentile(lat, 95):>13.4f}"
+        f"{np.degrees(np.max(head)):>15.4f}"
+        f"{np.degrees(np.sqrt(np.mean(head**2))):>16.4f}"
+    )
+
+print("=" * 100)

pp_stanley_plot_path_tracking_error.py

@@ -0,0 +1,778 @@
+import re
+import csv
+from pathlib import Path
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+# ============================================================
+# 参数
+# ============================================================
+
+LOG_PATTERN = "logs/mission_center_logger*.txt"
+
+# 每 5 个完整数据取 1 个
+SAMPLE_STEP = 40
+
+CSV_FILE = "recalculated_tracking_error.csv"
+
+
+# ============================================================
+# 1. 查找日志文件
+# ============================================================
+
+files = sorted(
+    Path(".").glob(LOG_PATTERN)
+)
+
+if not files:
+
+    raise FileNotFoundError(
+        f"当前目录没有找到：{LOG_PATTERN}"
+    )
+
+print("找到日志文件：")
+
+for file in files:
+
+    print("  ", file)
+
+
+# ============================================================
+# 2. 正则表达式
+# ============================================================
+
+# ------------------------------------------------------------
+# current pose
+#
+# [debug] [path_follow]
+# [current_pose_x:11.57,
+#  current_pose_y:-2.13,
+#  current_pose_yaw:-3.10]
+# ------------------------------------------------------------
+
+current_pose_pattern = re.compile(
+    r"current_pose_x:([-+0-9.eE]+),\s*"
+    r"current_pose_y:([-+0-9.eE]+),\s*"
+    r"current_pose_yaw:([-+0-9.eE]+)"
+)
+
+
+# ------------------------------------------------------------
+# current path
+#
+# [debug] [path_follow]
+# [current_path_x:11.59,
+#  current_path_y:-2.12,
+#  current_path_yaw:-3.10]
+# ------------------------------------------------------------
+
+current_path_pattern = re.compile(
+    r"current_path_x:([-+0-9.eE]+),\s*"
+    r"current_path_y:([-+0-9.eE]+),\s*"
+    r"current_path_yaw:([-+0-9.eE]+)"
+)
+
+
+# ============================================================
+# 3. 函数：角度归一化
+# ============================================================
+
+def normalize_angle(angle):
+
+    return (
+        angle + np.pi
+    ) % (
+        2.0 * np.pi
+    ) - np.pi
+
+
+# ============================================================
+# 4. 函数：根据 current pose + path 重新计算
+# ============================================================
+
+def calculate_errors(
+    current_x,
+    current_y,
+    current_yaw,
+    path_x,
+    path_y,
+    path_yaw
+):
+
+    # --------------------------------------------------------
+    # 世界坐标系下的位置差
+    # --------------------------------------------------------
+
+    dx = path_x - current_x
+
+    dy = path_y - current_y
+
+
+    # --------------------------------------------------------
+    # 转换到车辆坐标系
+    #
+    # vehicle frame:
+    #
+    #        x
+    #        ↑
+    #        |
+    #        |
+    #        ● -----> y
+    #
+    # 根据当前 yaw 进行旋转
+    # --------------------------------------------------------
+
+    lon_dis = (
+        np.cos(current_yaw) * dx
+        +
+        np.sin(current_yaw) * dy
+    )
+
+
+    dis = (
+        -np.sin(current_yaw) * dx
+        +
+        np.cos(current_yaw) * dy
+    )
+
+
+    # --------------------------------------------------------
+    # 车辆到路径点的欧氏距离
+    # --------------------------------------------------------
+
+    distance = np.sqrt(
+        dx * dx +
+        dy * dy
+    )
+
+
+    # --------------------------------------------------------
+    # 航向误差
+    # --------------------------------------------------------
+
+    alpha = normalize_angle(
+        path_yaw - current_yaw
+    )
+
+
+    # --------------------------------------------------------
+    # deg
+    # --------------------------------------------------------
+
+    alpha_deg = np.degrees(alpha)
+
+
+    return (
+        distance,
+        lon_dis,
+        dis,
+        alpha,
+        alpha_deg
+    )
+
+
+# ============================================================
+# 5. 读取日志
+# ============================================================
+
+all_data = []
+
+total_raw_count = 0
+
+
+for file in files:
+
+    print()
+    print("=" * 70)
+    print(f"读取：{file}")
+    print("=" * 70)
+
+
+    # --------------------------------------------------------
+    # 每个文件单独计数
+    # --------------------------------------------------------
+
+    file_data_count = 0
+
+
+    # --------------------------------------------------------
+    # 临时保存 current pose
+    # --------------------------------------------------------
+
+    pending_pose = None
+
+
+    with file.open(
+        "r",
+        encoding="utf-8",
+        errors="ignore"
+    ) as f:
+
+
+        for line in f:
+
+
+            # =================================================
+            # current pose
+            # =================================================
+
+            m_pose = current_pose_pattern.search(
+                line
+            )
+
+
+            if m_pose:
+
+                pending_pose = {
+
+                    "x": float(
+                        m_pose.group(1)
+                    ),
+
+                    "y": float(
+                        m_pose.group(2)
+                    ),
+
+                    "yaw": float(
+                        m_pose.group(3)
+                    ),
+                }
+
+                continue
+
+
+            # =================================================
+            # current path
+            # =================================================
+
+            m_path = current_path_pattern.search(
+                line
+            )
+
+
+            if (
+                m_path is not None
+                and
+                pending_pose is not None
+            ):
+
+
+                path_x = float(
+                    m_path.group(1)
+                )
+
+                path_y = float(
+                    m_path.group(2)
+                )
+
+                path_yaw = float(
+                    m_path.group(3)
+                )
+
+
+                # =================================================
+                # 重新计算
+                # =================================================
+
+                (
+                    point_distance,
+                    lon_dis,
+                    dis,
+                    alpha,
+                    alpha_deg
+                ) = calculate_errors(
+
+                    pending_pose["x"],
+                    pending_pose["y"],
+                    pending_pose["yaw"],
+
+                    path_x,
+                    path_y,
+                    path_yaw
+                )
+
+
+                # =================================================
+                # 每 SAMPLE_STEP 个数据取一个
+                # =================================================
+
+                if file_data_count % SAMPLE_STEP == 0:
+
+                    all_data.append({
+
+                        "file": file.name,
+
+                        "current_x":
+                            pending_pose["x"],
+
+                        "current_y":
+                            pending_pose["y"],
+
+                        "current_yaw":
+                            pending_pose["yaw"],
+
+                        "path_x":
+                            path_x,
+
+                        "path_y":
+                            path_y,
+
+                        "path_yaw":
+                            path_yaw,
+
+                        "point_distance":
+                            point_distance,
+
+                        "lon_dis":
+                            lon_dis,
+
+                        "dis":
+                            dis,
+
+                        "alpha":
+                            alpha,
+
+                        "alpha_deg":
+                            alpha_deg,
+                    })
+
+
+                file_data_count += 1
+
+                total_raw_count += 1
+
+
+                # ------------------------------------------------
+                # 清空
+                # ------------------------------------------------
+
+                pending_pose = None
+
+
+    print(
+        f"原始完整数据：{file_data_count}"
+    )
+
+    print(
+        f"抽样后数据："
+        f"{(file_data_count + SAMPLE_STEP - 1) // SAMPLE_STEP}"
+    )
+
+
+# ============================================================
+# 6. 检查
+# ============================================================
+
+if not all_data:
+
+    raise RuntimeError(
+        "没有解析到 current_pose + current_path 数据。"
+    )
+
+
+print()
+print("=" * 70)
+print("解析完成")
+print("=" * 70)
+
+print(
+    f"原始数据：{total_raw_count}"
+)
+
+print(
+    f"最终数据：{len(all_data)}"
+)
+
+
+# ============================================================
+# 7. 提取数据
+# ============================================================
+
+current_x = np.array([
+    d["current_x"]
+    for d in all_data
+])
+
+current_y = np.array([
+    d["current_y"]
+    for d in all_data
+])
+
+current_yaw = np.array([
+    d["current_yaw"]
+    for d in all_data
+])
+
+
+path_x = np.array([
+    d["path_x"]
+    for d in all_data
+])
+
+path_y = np.array([
+    d["path_y"]
+    for d in all_data
+])
+
+path_yaw = np.array([
+    d["path_yaw"]
+    for d in all_data
+])
+
+
+point_distance = np.array([
+    d["point_distance"]
+    for d in all_data
+])
+
+lon_dis = np.array([
+    d["lon_dis"]
+    for d in all_data
+])
+
+dis = np.array([
+    d["dis"]
+    for d in all_data
+])
+
+alpha = np.array([
+    d["alpha"]
+    for d in all_data
+])
+
+alpha_deg = np.array([
+    d["alpha_deg"]
+    for d in all_data
+])
+
+
+# ============================================================
+# 8. 计算车辆累计行驶距离
+# ============================================================
+
+if len(path_x) > 1:
+
+    dx = np.diff(
+        path_x
+    )
+
+    dy = np.diff(
+        path_y
+    )
+
+    step_distance = np.sqrt(
+        dx ** 2 +
+        dy ** 2
+    )
+
+else:
+
+    step_distance = np.array([])
+for i in range(len(step_distance)
+):
+
+    if step_distance[i] > 0.5:
+        step_distance[i] = 0.0
+        print(
+            f"Warning: step_distance[{i}] = {step_distance[i]:.3f} m, which is too large. Setting it to 0.0.")
+
+
+distance_m = np.zeros(
+    len(path_x)
+)
+
+
+if len(path_x) > 1:
+
+    distance_m[1:] = np.cumsum(
+        step_distance
+    )
+
+
+# ============================================================
+# 9. 保存 CSV
+# ============================================================
+
+print()
+print("=" * 70)
+print("保存 CSV")
+print("=" * 70)
+
+
+with open(
+    CSV_FILE,
+    "w",
+    newline="",
+    encoding="utf-8-sig"
+) as f:
+
+    writer = csv.writer(f)
+
+
+    writer.writerow([
+
+        "distance_m",
+
+        "current_x_m",
+        "current_y_m",
+
+        "lateral_error_m",
+
+        "heading_error_rad",
+
+        "heading_error_deg",
+
+    ])
+
+
+    for i in range(
+        len(all_data)
+    ):
+
+        writer.writerow([
+
+            f"{distance_m[i]:.8f}",
+
+            f"{current_x[i]:.8f}",
+            f"{current_y[i]:.8f}",
+
+
+            f"{dis[i]:.8f}",
+
+            f"{alpha[i]:.8f}",
+
+            f"{alpha_deg[i]:.8f}",
+
+        ])
+
+
+print(
+    f"CSV 已保存："
+    f"{Path(CSV_FILE).resolve()}"
+)
+
+
+# ============================================================
+# 11. 横向误差
+# ============================================================
+
+plt.figure(
+    figsize=(12, 5)
+)
+
+plt.plot(
+    distance_m,
+    dis,
+    linewidth=1.2
+)
+
+plt.axhline(
+    0.0,
+    linestyle="--",
+    linewidth=0.8
+)
+
+plt.xlabel(
+    "Distance (m)"
+)
+
+plt.ylabel(
+    "Lateral Error (m)"
+)
+
+plt.title(
+    "Recalculated Lateral Error"
+)
+
+plt.grid(True)
+
+plt.tight_layout()
+
+plt.show()
+
+
+# ============================================================
+# 13. 航向误差 rad
+# ============================================================
+
+plt.figure(
+    figsize=(12, 5)
+)
+
+plt.plot(
+    distance_m,
+    alpha,
+    linewidth=1.2
+)
+
+plt.axhline(
+    0.0,
+    linestyle="--",
+    linewidth=0.8
+)
+
+plt.xlabel(
+    "Distance (m)"
+)
+
+plt.ylabel(
+    "Heading Error (rad)"
+)
+
+plt.title(
+    "Recalculated Heading Error"
+)
+
+plt.grid(True)
+
+plt.tight_layout()
+
+plt.show()
+
+
+# ============================================================
+# 15. 车辆轨迹 + 路径点
+# ============================================================
+
+plt.figure(
+    figsize=(8, 8)
+)
+
+plt.plot(
+    current_x,
+    current_y,
+    linewidth=1.2,
+    label="Current Pose"
+)
+
+plt.plot(
+    path_x,
+    path_y,
+    linestyle="--",
+    linewidth=1.0,
+    label="Current Path Point"
+)
+
+
+plt.scatter(
+    current_x[0],
+    current_y[0],
+    s=50,
+    label="Start"
+)
+
+plt.scatter(
+    current_x[-1],
+    current_y[-1],
+    s=50,
+    label="End"
+)
+
+
+plt.xlabel(
+    "X (m)"
+)
+
+plt.ylabel(
+    "Y (m)"
+)
+
+plt.title(
+    "Current Pose and Path"
+)
+
+plt.axis(
+    "equal"
+)
+
+plt.grid(True)
+
+plt.legend()
+
+plt.tight_layout()
+
+
+plt.show()
+
+
+# ============================================================
+# 16. 误差统计
+# ============================================================
+
+print()
+print("=" * 70)
+print("误差统计")
+print("=" * 70)
+
+
+print(
+    f"最大横向误差："
+    f"{np.max(np.abs(dis)):.6f} m"
+)
+
+print(
+    f"平均绝对横向误差："
+    f"{np.mean(np.abs(dis)):.6f} m"
+)
+
+print(
+    f"横向误差 RMSE："
+    f"{np.sqrt(np.mean(dis ** 2)):.6f} m"
+)
+
+
+print()
+
+print(
+    f"最大纵向误差："
+    f"{np.max(np.abs(lon_dis)):.6f} m"
+)
+
+print(
+    f"平均绝对纵向误差："
+    f"{np.mean(np.abs(lon_dis)):.6f} m"
+)
+
+print(
+    f"纵向误差 RMSE："
+    f"{np.sqrt(np.mean(lon_dis ** 2)):.6f} m"
+)
+
+
+print()
+
+print(
+    f"最大航向误差："
+    f"{np.max(np.abs(alpha_deg)):.6f} deg"
+)
+
+print(
+    f"平均绝对航向误差："
+    f"{np.mean(np.abs(alpha_deg)):.6f} deg"
+)
+
+print(
+    f"航向误差 RMSE："
+    f"{np.sqrt(np.mean(alpha_deg ** 2)):.6f} deg"
+)
+
+
+print()
+
+print(
+    f"最大车辆-路径点距离："
+    f"{np.max(point_distance):.6f} m"
+)
+
+print(
+    f"平均车辆-路径点距离："
+    f"{np.mean(point_distance):.6f} m"
+)
+
+print(
+    f"总行驶距离："
+    f"{distance_m[-1]:.3f} m"
+)
+

src/auto_drive_misson_center/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(auto_drive_proj_utils REQUIRED)
 find_package(auto_drive_proj_msg REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(fmt REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 include_directories(
   include
@@ -66,6 +67,7 @@ ament_target_dependencies(mission_center_node
   builtin_interfaces
   auto_drive_proj_utils
   auto_drive_proj_msg
+  visualization_msgs
 )
 
 ament_target_dependencies(auto_mission_handler_node

src/auto_drive_misson_center/config/misson_center_params.yaml

@@ -2,7 +2,7 @@ auto_mission_handler_node:
   # parameters
   ros__parameters:
     # redis
-    redis_host: "192.168.1.110" # "127.0.0.1"
+    redis_host: "127.0.0.1" # "127.0.0.1"
     redis_port: 6379
     redis_db: 0
 
@@ -20,13 +20,13 @@ path_follow_handler_node:
   # parameters
   ros__parameters:
     # redis
-    redis_host: "192.168.1.110" # "127.0.0.1"
+    redis_host: "127.0.0.1" # "127.0.0.1"
     redis_port: 6379
     redis_db: 0
 
     device_id: "2072868304711761920"
 
-    controller_type: "pure_pursuit"  # "pure_pursuit", "lqr", "pid"
+    controller_type: "stanley"  # "pure_pursuit", "lqr", "pid","stanley"
     
     # logger
     logger_logger_name: "path_follow"
@@ -40,7 +40,7 @@ mission_center_node:
   # parameters
   ros__parameters:
     # redis
-    redis_host: "192.168.1.110" # "127.0.0.1"
+    redis_host: "127.0.0.1" # "127.0.0.1"
     redis_port: 6379
     redis_db: 0
 

src/auto_drive_misson_center/include/auto_drive_misson_center/lqr_control.hpp

@@ -22,17 +22,18 @@ public:
         B.resize(2, 1); // 控制输入矩阵
         Q.resize(2, 2); // 状态误差的加权矩阵
         R.resize(1, 1); // 控制输入的加权矩阵
-        B << -1, 1;     // 控制输入矩阵
+        //B << 0, 1;     // 控制输入矩阵
 
         // 设计LQR的代价函数矩阵
-        Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
+        Q << 100, 0, 0, 10; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ
 
-        R << 10; // 控制输入的加权矩阵：惩罚角速度ω
+        R << 50; // 控制输入的加权矩阵：惩罚角速度ω
     };
     ~LQR_Controller_C() {};
     // LQR求解器函数
     MatrixXd lqr(const MatrixXd &A, const MatrixXd &B, const MatrixXd &Q, const MatrixXd &R)
     {
+       
         MatrixXd P = Q; // 初始化P为Q
         MatrixXd P_prev = MatrixXd::Zero(A.rows(), A.cols());
         for (int i = 0; i < 1000; ++i)
@@ -46,12 +47,16 @@ public:
         MatrixXd K = (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
         return K;
     };
-    float update(const float alpha, const float linear_val, const float dis)
+    float update(const float alpha, const float linear_val, const float dis, const float kappa)
     {
         // std::cout << "LQR update!" << std::endl;
         logger_->trace("LQR update!");
+        double dt = 0.01;
         MatrixXd A(2, 2);
-        A << 0, linear_val, 0, 0;
+        A << 1.0, linear_val * dt,
+               0, 1.0;
+        B << 0.5 * linear_val * dt * dt,
+             dt;
         Eigen::VectorXd x(2);
 
         x << dis, alpha;
@@ -61,8 +66,24 @@ public:
         //      << K << std::endl;
         std::stringstream ss;
         ss << K;
-        logger_->trace("LQR Gain Matrix K:\n{}", ss.str());
-        return -(K * x)(0, 0);
+        logger_->info("LQR Gain Matrix K:\n{}", ss.str());
+        double omega_feedback = -(K * x)(0, 0);
+            // Curvature feedforward
+        double omega_feedforward = linear_val * kappa;// feedforwward unused temporarily
+
+        double omega = omega_feedback;
+
+        logger_->info(
+            "LQR: dis={}, alpha={}, kappa={}, "
+            "omega_fb={}, omega_ff={}, omega={}",
+            dis,
+            alpha,
+            kappa,
+            omega_feedback,
+            omega_feedforward,
+            omega);
+
+        return omega;
     };
     /*void init_lqr(const MatrixXd B, const MatrixXd Q, const MatrixXd R)
     {
@@ -70,6 +91,26 @@ public:
         this->Q = Q;
         this->R = R;
     }*/
+   float update(const float alpha, const float linear_val, const float dis)
+    {
+        // std::cout << "LQR update!" << std::endl;
+        logger_->trace("LQR update!");
+        double dt = 0.01;
+        MatrixXd A(2, 2);
+        A << 1.0, linear_val*dt,0, 1.0;
+        B << 0.5 * linear_val * dt * dt,dt;
+        Eigen::VectorXd x(2);
+
+        x << dis, alpha;
+        // 求解LQR增益矩阵K
+        MatrixXd K = lqr(A, B, Q, R);
+        // std::cout << "LQR Gain Matrix K:" << std::endl
+        //      << K << std::endl;
+        std::stringstream ss;
+        ss << K;
+        logger_->info("LQR Gain Matrix K:\n{}", ss.str());
+        return -(K * x)(0, 0);
+    };
     void init(const float max_linear_vel, const float max_angular_vel, const float kp, const float ki,
               const float kd, const float look_ahead_dis, const float kv)
     {
@@ -82,7 +123,7 @@ public:
         B.resize(2, 1); // 控制输入矩阵
         Q.resize(2, 2); // 状态误差的加权矩阵
         R.resize(1, 1); // 控制输入的加权矩阵
-        B << -1, 1;     // 控制输入矩阵
+        B << 0, 1;     // 控制输入矩阵
 
         // 设计LQR的代价函数矩阵
         Q << 50, 0, 0, 15; // 状态误差的加权矩阵：惩罚横向误差e_x和角度误差θ

src/auto_drive_misson_center/include/auto_drive_misson_center/mission_center_node.hpp

@@ -55,7 +55,6 @@ private:
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_warn_pub_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_error_pub_;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr properties_report_pub_;
-
     std::vector<std::pair<int, int>> interpolation_map_;
 
     std::shared_future<typename auto_drive_proj_msg::srv::ReqString::Response::SharedPtr> mission_future_;
@@ -86,7 +85,11 @@ private:
                               std::string& mission_data,
                               std::string& follow_data);
     std::vector<std::vector<double>> interpolatePoints(const std::vector<std::vector<double>>& coordinates,
-                                                      double max_distance = 0.5);
+                                                      double max_distance);
+    // simulation only
+    std::vector<std::vector<double>> alignPathYaw( const std::vector<std::vector<double>>& coordinates,
+                                                    double vehicle_initial_yaw);
+
     int updatePathIndices(const int original_ind, const std::vector<std::pair<int, int>>& interpolation_map);
 
     void handleAsyncResponses();
@@ -145,6 +148,9 @@ private:
     int logger_max_size_;
     int logger_max_files_;
     int logger_set_level_;
+
+    // 
+    double vehicle_initial_yaw_ = 0.0;
 };
 
 } // namespace auto_drive_misson_center

src/auto_drive_misson_center/include/auto_drive_misson_center/path_follow_handler.hpp

@@ -45,7 +45,7 @@ public:
 private:
     MainState main_state_{MainState::IDLE};
     bool is_resuming_{false};
-    std::string controller_type_{"pure_pursuit"};
+    std::string controller_type_{"lqr"};
 
     rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;

src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp

@@ -15,7 +15,7 @@ struct PathPoint {
     double x;          // 经度或UTM X坐标
     double y;          // 纬度或UTM Y坐标
     double yaw;        // 朝向角度（弧度）
-    
+    double kappa;  // 曲率
     PathPoint(double x_val = 0.0, double y_val = 0.0, double yaw_val = 0.0)
         : x(x_val), y(y_val), yaw(yaw_val) {}
 };
@@ -60,7 +60,36 @@ public:
         }
         return false;
     }
-    
+    void computePathCurvature()
+    {
+        if (path_points_.size() < 3) {
+            return;
+        }
+        for (size_t i = 1; i < path_points_.size() - 1; ++i)
+        {
+            const auto& p_prev = path_points_[i - 1];
+            const auto& p_curr = path_points_[i];
+            const auto& p_next = path_points_[i + 1];
+            double ds1 = std::hypot(p_curr.x - p_prev.x,p_curr.y - p_prev.y);
+            double ds2 = std::hypot(p_next.x - p_curr.x,p_next.y - p_curr.y);
+            double ds = 0.5 * (ds1 + ds2);
+            if (ds < 1e-6) {
+                path_points_[i].kappa= 0.0;
+                continue;
+            }
+            double dyaw = p_next.yaw - p_prev.yaw;
+            while (dyaw > M_PI)
+                dyaw -= 2.0 * M_PI;
+
+            while (dyaw < -M_PI)
+                dyaw += 2.0 * M_PI;
+            path_points_[i].kappa = dyaw / (ds1 + ds2);
+            logger_->info("path ind:{}, kappa:{}", i, path_points_[i].kappa);
+        }
+        // 首尾点处理
+        path_points_.front().kappa= path_points_[1].kappa;
+        path_points_.back().kappa= path_points_[path_points_.size() - 2].kappa;
+    }
     // 运行一次控制计算
     ControlOutput run(const PathPoint& current_pose) {
         if (!is_initialized_) {
@@ -159,6 +188,7 @@ protected:
 
             logger_->debug("current_point_index_:{}, path_points_ size:{}, distance2:{}, distance1:{}",
                         current_point_index_, path_points_.size(), distance2, distance1);
+
             
             // 如果current_point_index_+1的距离小于current_point_index_距离，current_point_index_++
             if (distance2 < distance1) {
@@ -185,7 +215,9 @@ protected:
     
     // 保护成员变量，子类可以访问
     double mission_speed_{0.0};
-    double turn_radius_{1.0};
+    double turn_radius_{0.3};
+    double max_allowed_speed_{1.0};
+    double wheel_dis_{0.5};
     std::vector<PathPoint> path_points_;
     int current_point_index_{0};
     bool is_initialized_{false};

src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp

@@ -8,6 +8,7 @@
 #include "lqr_control.hpp"
 #include "pid_control.hpp"
 #include "pure_pursuit_control.hpp"
+#include "stanley_control.hpp"
 #include "spdlog/logger.h"
 #include "spdlog/spdlog.h"
 #include "spdlog/sinks/rotating_file_sink.h"
@@ -27,29 +28,35 @@ protected:
         // 从PathTrackingController的参数映射到原有控制器的init参数
         // 注意：原有控制器需要max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv
         // 这里需要根据需求设置合适的参数值
-        float max_linear_vel = mission_speed_;
+        float max_linear_vel = mission_speed_ ;
         float max_angular_vel = mission_speed_ / turn_radius_; // 根据转弯半径估算最大角速度
+        float max_angular_vel_theory = max_allowed_speed_/wheel_dis_;
+        max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory);
         float kp = 1.76;  // 需要根据实际情况调整
         float ki = 0.0;
         float kd = 0.8;
-        float look_ahead_dis = 0.4;  // 需要根据实际情况调整
-        float kv = 1.0;   // 需要根据实际情况调整
+        float look_ahead_dis = 0.6;  // 需要根据实际情况调整
+        float kv = 0.0;   // 需要根据实际情况调整
         
         controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
         return true;
     }
-    
+    bool kappa_cal = false;
     ControlOutput onRun(const PathPoint& current_pose) override {
         // 从PathPoint计算所需参数
         float alpha = 0.0;        // 角度误差
         float linear_val = 0.0;   // 线速度
         float dis = 0.0;          // 横向误差
-        
-        computeControlParams(current_pose, alpha, linear_val, dis);
-        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis);
+        float kappa = 0.0;        // 路徑曲率
+        if(!kappa_cal){
+            computePathCurvature();
+            kappa_cal = true;
+        }
+        computeControlParams(current_pose, alpha, linear_val, dis, kappa);
+        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}, kappa:{}", alpha, linear_val, dis,kappa);
         
         // 调用原有控制器的update方法
-        float angular_velocity = controller_->update(alpha, linear_val, dis);
+        float angular_velocity = controller_->update(alpha, linear_val, dis, kappa);
         logger_->debug("PP output angular={}", angular_velocity);
         
         // 判断是否完成
@@ -65,8 +72,8 @@ protected:
 private:
     std::shared_ptr<LQR_Controller_C> controller_;
     std::shared_ptr<spdlog::logger> logger_;
-    
-    void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis) {
+
+    void computeControlParams(const PathPoint& current_pose, float& alpha, float& linear_val, float& dis, float& kappa) {
         // 获取当前路径点的索引
         int idx = current_point_index_;
         if (idx >= static_cast<int>(path_points_.size()) - 1) {
@@ -82,13 +89,15 @@ private:
         dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
         
         // 角度误差
-        alpha = static_cast<float>(path_yaw - current_pose.yaw);
+        alpha = static_cast<float>(current_pose.yaw - path_yaw);
         // 归一化到 [-pi, pi]
         while (alpha > M_PI) alpha -= 2 * M_PI;
         while (alpha < -M_PI) alpha += 2 * M_PI;
         
         // 线速度
         linear_val = static_cast<float>(mission_speed_);
+        kappa = path_points_[idx].kappa;
+    
     }
 };
 
@@ -165,29 +174,38 @@ public:
 
 protected:
     bool onInitialize() override {
-        float max_linear_vel = mission_speed_;
+        float max_linear_vel = mission_speed_ ;
         float max_angular_vel = mission_speed_ / turn_radius_;
+        float max_angular_vel_theory = max_allowed_speed_/wheel_dis_;
+        max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory);
         float kp = 1.76;
         float ki = 0.0;
         float kd = 0.8;
         float look_ahead_dis = 0.4;   // 前视距离，需要根据实际情况调整
+        float kv = 0.6;               // 速度系数，需要根据实际情况调整
         adapter_look_ahead_dis_ = look_ahead_dis;
-        float kv = 1.0;               // 速度系数，需要根据实际情况调整
-        
-        controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
+        //logger_->debug("PP para mission_speed_={}, turn_radius_={} frontdis={}", mission_speed_, turn_radius_,adapter_look_ahead_dis_ );
+        controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, adapter_look_ahead_dis_, kv);
         return true;
     }
-    
+    bool kappa_cal = false;
     ControlOutput onRun(const PathPoint& current_pose) override {
         float alpha = 0.0;
         float linear_val = 0.0;
         float dis = 0.0;
-        
+        float lon_dis = 0.0;
+        float kappa = 0.0;
+        if(!kappa_cal){
+            computePathCurvature();
+            kappa_cal = true;
+        }
+            
         // Pure Pursuit使用前视点，计算需要特殊处理
-        computeControlParamsWithLookAhead(current_pose, alpha, linear_val, dis);
-        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}", alpha, linear_val, dis);
+        computeControlParamsWithLookAhead(current_pose, alpha, linear_val, dis, lon_dis, kappa);
+        logger_->debug("PPAdapter alpha={}, linear_val={}, dis={}, lon_dis={},kappa={}", alpha, linear_val, dis,lon_dis,kappa);
         
-        float angular_velocity = controller_->update(alpha, linear_val, dis);
+        float angular_velocity = controller_->update(alpha, linear_val, dis, lon_dis, kappa);
+        //float angular_velocity = controller_->update(alpha, linear_val, dis);
         logger_->debug("PP output angular={}", angular_velocity);
         bool completed = isPathCompleted(current_pose);
         
@@ -204,7 +222,7 @@ private:
     std::shared_ptr<spdlog::logger> logger_;
     
     void computeControlParamsWithLookAhead(const PathPoint& current_pose, 
-                                          float& alpha, float& linear_val, float& dis) {
+                                          float& alpha, float& linear_val, float& dis, float& lon_dis, float& kappa) {
         // 查找前视点
         PathPoint lookahead_point;
         if (!findLookAheadPoint(current_pose, lookahead_point)) {
@@ -237,7 +255,19 @@ private:
         double dx_path = current_pose.x - path_points_[idx].x;
         double dy_path = current_pose.y - path_points_[idx].y;
         double path_yaw = path_points_[idx].yaw;
-        dis = static_cast<float>(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw));
+        //dis = static_cast<float>(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw));
+        dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
+        lon_dis = static_cast<float>(sqrt(dx * dx + dy * dy));
+        kappa = static_cast<float>(path_points_[idx].kappa);
+        logger_->debug("current_pose_x:{}, current_pose_y:{},current_pose_yaw:{}",
+                    current_pose.x, current_pose.y,current_pose.yaw
+        );
+        logger_->debug("current_path_x:{}, current_path_y:{},current_path_yaw:{}",
+                    path_points_[idx].x,
+                    path_points_[idx].y,
+                    path_points_[idx].yaw
+        );
+                
     }
     
     bool findLookAheadPoint(const PathPoint& current_pose, PathPoint& lookahead_point) {
@@ -267,6 +297,7 @@ private:
                 lookahead_point.x = path_points_[i].x + ratio * dx;
                 lookahead_point.y = path_points_[i].y + ratio * dy;
                 lookahead_point.yaw = path_points_[i].yaw;
+                lookahead_point.kappa = path_points_[i].kappa;
                 return true;
             }
             accumulated_dist += segment_length;
@@ -278,6 +309,148 @@ private:
     }
 };
 
-} // namespace auto_drive_misson_center
+// Stanley控制器适配器
+class StanleyControllerAdapter : public PathTrackingController {
+public:
+    StanleyControllerAdapter(std::shared_ptr<spdlog::logger> logger = nullptr)
+        : logger_(logger), PathTrackingController(logger), controller_(std::make_shared<Stanley_Controller_C>(logger)) {}
 
+    protected:
+    bool onInitialize() override {
+        float max_linear_vel = mission_speed_ ;
+        float max_angular_vel = mission_speed_ / turn_radius_;
+        float max_angular_vel_theory = max_allowed_speed_/wheel_dis_;
+        max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory);
+        float kp = 1.76;
+        float ki = 0.0;
+        float kd = 0.8;
+        float look_ahead_dis = 0.4;   // 前视距离，需要根据实际情况调整
+        float kv = 0.6;               // 速度系数，需要根据实际情况调整
+        adapter_look_ahead_dis_ = look_ahead_dis;
+        //logger_->debug("PP para mission_speed_={}, turn_radius_={} frontdis={}", mission_speed_, turn_radius_,adapter_look_ahead_dis_ );
+        controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, adapter_look_ahead_dis_, kv);
+        return true;
+    }
+    bool kappa_cal = false;
+    ControlOutput onRun(const PathPoint& current_pose) override {
+        float alpha = 0.0;
+        float linear_val = 0.0;
+        float dis = 0.0;
+        float lon_dis = 0.0;
+        float kappa = 0.0;
+        if(!kappa_cal){
+            computePathCurvature();
+            kappa_cal = true;
+        }
+            
+        // Pure Pursuit使用前视点，计算需要特殊处理
+        computeControlParamsWithLookAhead(current_pose, alpha, linear_val, dis, lon_dis, kappa);
+        logger_->debug("Stanley Adapter alpha={}, linear_val={}, dis={}, lon_dis={},kappa={}", alpha, linear_val, dis,lon_dis,kappa);
+        
+        float angular_velocity = controller_->update(alpha, linear_val, dis, lon_dis, kappa);
+        //float angular_velocity = controller_->update(alpha, linear_val, dis);
+        logger_->debug("Stanley output angular={}", angular_velocity);
+        bool completed = isPathCompleted(current_pose);
+        
+        return ControlOutput(mission_speed_, angular_velocity, completed);
+    }
+    
+    void onReset() override {
+        controller_->reset();
+    }
+
+private:
+    std::shared_ptr<Stanley_Controller_C> controller_;
+    double adapter_look_ahead_dis_; //保存前视距离
+    std::shared_ptr<spdlog::logger> logger_;
+    
+    void computeControlParamsWithLookAhead(const PathPoint& current_pose, 
+                                          float& alpha, float& linear_val, float& dis, float& lon_dis, float& kappa) {
+        // 查找前视点
+        PathPoint lookahead_point;
+        if (!findLookAheadPoint(current_pose, lookahead_point)) {
+            // 如果找不到前视点，使用当前路径点
+            int idx = current_point_index_;
+            if (idx >= static_cast<int>(path_points_.size())) {
+                idx = path_points_.size() - 1;
+            }
+            lookahead_point = path_points_[idx];
+        }
+        
+        // 计算从当前点到前视点的角度误差
+        double dx = lookahead_point.x - current_pose.x;
+        double dy = lookahead_point.y - current_pose.y;
+        double target_yaw = atan2(dy, dx);
+        
+        // 预瞄角度误差
+        alpha = static_cast<float>(target_yaw - current_pose.yaw);
+        while (alpha > M_PI) alpha -= 2 * M_PI;
+        while (alpha < -M_PI) alpha += 2 * M_PI;
+        
+        // 线速度
+        linear_val = static_cast<float>(mission_speed_);
+        
+        // 横向误差（用于更新索引）
+        int idx = current_point_index_;
+        if (idx >= static_cast<int>(path_points_.size()) - 1) {
+            idx = path_points_.size() - 1;
+        }
+        double dx_path = current_pose.x - path_points_[idx].x;
+        double dy_path = current_pose.y - path_points_[idx].y;
+        double path_yaw = path_points_[idx].yaw;
+        //dis = static_cast<float>(-dx_path * sin(path_yaw) + dy_path * cos(path_yaw));
+        dis = static_cast<float>(-dx * sin(path_yaw) + dy * cos(path_yaw));
+        lon_dis = static_cast<float>(sqrt(dx * dx + dy * dy));
+        kappa = static_cast<float>(path_points_[idx].kappa);
+        logger_->debug("current_pose_x:{}, current_pose_y:{},current_pose_yaw:{}",
+                    current_pose.x, current_pose.y,current_pose.yaw
+        );
+        logger_->debug("current_path_x:{}, current_path_y:{},current_path_yaw:{}",
+                    path_points_[idx].x,
+                    path_points_[idx].y,
+                    path_points_[idx].yaw
+        );
+                
+    }
+    
+    bool findLookAheadPoint(const PathPoint& current_pose, PathPoint& lookahead_point) {
+        if (path_points_.empty()) {
+            return false;
+        }
+        
+        // 获取前视距离（从控制器参数获取）
+        // 这里需要从controller_获取lookAheadDis，但由于接口限制，我们使用默认值
+        double look_ahead_distance = adapter_look_ahead_dis_; // 需要与初始化时一致
+        
+        // 从当前点开始沿路径向前查找
+        int start_idx = current_point_index_;
+        if (start_idx >= static_cast<int>(path_points_.size())) {
+            start_idx = path_points_.size() - 1;
+        }
+        
+        double accumulated_dist = 0.0;
+        for (int i = start_idx; i < static_cast<int>(path_points_.size()) - 1; ++i) {
+            double dx = path_points_[i + 1].x - path_points_[i].x;
+            double dy = path_points_[i + 1].y - path_points_[i].y;
+            double segment_length = sqrt(dx * dx + dy * dy);
+            
+            if (accumulated_dist + segment_length >= look_ahead_distance) {
+                // 在前视距离内插值
+                double ratio = (look_ahead_distance - accumulated_dist) / segment_length;
+                lookahead_point.x = path_points_[i].x + ratio * dx;
+                lookahead_point.y = path_points_[i].y + ratio * dy;
+                lookahead_point.yaw = path_points_[i].yaw;
+                lookahead_point.kappa = path_points_[i].kappa;
+                return true;
+            }
+            accumulated_dist += segment_length;
+        }
+        
+        // 如果路径长度不够，使用最后一个点
+        lookahead_point = path_points_.back();
+        return true;
+    }
+}; // namespace auto_drive_misson_center
+
+}
 #endif // AUTO_DRIVE_MISSION_CENTER_PATH_TRACKING_CONTROLLER_ADAPTER_HPP_

src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp

@@ -27,17 +27,30 @@ public:
         lookAheadDis = look_ahead_dis;
         k_v = kv;
     }
-
     float update(const float alpha, const float linear_val, const float dis)
     {
         // std::cout<<"PurePursuit update!"<<std::endl;
         logger_->trace("PurePursuit update!");
-        float w_val = (2 * fabs(linear_val) * sin(alpha) / (k_v * fabs(linear_val) + lookAheadDis));  
+        float w_val = (2 * fabs(linear_val) * sin(alpha) / (lookAheadDis));  
         w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val;
         // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl;
         logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val);
         return w_val;
     };
+    float update(const float alpha, const float linear_val, const float dis, const float lon_dis,const float kappa)
+    {
+        // std::cout<<"PurePursuit update!"<<std::endl;
+        logger_->trace("PurePursuit update!");
+       // logger_->info("PurePursuit process! linear_val:{}, dis:{}, lon_dis:{}, kappa:{}", linear_val, dis, lon_dis, kappa);
+        float w_val = (2 * fabs(linear_val) * sin(alpha) / (lon_dis)) + dis; 
+        
+        //float w_val = linear_val*kappa;  
+        w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val;
+        // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl;
+        logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val);
+
+        return w_val;
+    };
 
     void reset()
     {

src/auto_drive_misson_center/include/auto_drive_misson_center/stanley_control.hpp

@@ -0,0 +1,71 @@
+#ifndef _STANLEY_CONTROLL_H_
+#define _STANLEY_CONTROLL_H_
+
+#include <iostream>
+#include <math.h>
+#include <Eigen/Dense>
+#include "base_following_controller.hpp"
+#include "pid_control.hpp"
+
+using namespace Eigen;
+
+class Stanley_Controller_C : public BaseFollowingController
+{
+public:
+    Stanley_Controller_C() = delete;
+    Stanley_Controller_C(std::shared_ptr<spdlog::logger> logger_recorder)
+    :BaseFollowingController(logger_recorder), logger_(logger_recorder){};
+    ~Stanley_Controller_C(){};
+
+    void init(const float max_linear_vel, const float max_angular_vel, const float kp, const float ki, \
+        const float kd, const float look_ahead_dis, const float kv)
+    {
+        // std::cout<<"PurePursuit init!"<<std::endl;
+        logger_->info("PurePursuit init! max_linear_vel:{}, max_angular_vel:{}", max_linear_vel, max_angular_vel);
+        maxLinearVel = max_linear_vel;
+        maxAngularVel = max_angular_vel;
+        lookAheadDis = look_ahead_dis;
+        k_v = kv;
+    }
+    float update(const float alpha, const float linear_val, const float dis)
+    {
+        // std::cout<<"PurePursuit update!"<<std::endl;
+        logger_->trace("PurePursuit update!");
+        float w_val = 1.5 * alpha +1.0 * std::atan2(dis,linear_val);
+        w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val;
+        // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl;
+        logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val);
+        return w_val;
+    };
+    float update(const float alpha, const float linear_val, const float dis, const float lon_dis,const float kappa)
+    {
+        // std::cout<<"PurePursuit update!"<<std::endl;
+        logger_->trace("PurePursuit update!");
+       // logger_->info("PurePursuit process! linear_val:{}, dis:{}, lon_dis:{}, kappa:{}", linear_val, dis, lon_dis, kappa);
+        float w_val =  1.5 * alpha +1.0 * std::atan2(dis,linear_val);
+        
+        //float w_val = linear_val*kappa;  
+        w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val;
+        // std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl;
+        logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val);
+
+        return w_val;
+    };
+
+    void reset()
+    {
+        //pid_.reset();
+    }
+
+    void test(){std::cout<<"pp test!"<<std::endl; logger_->debug("pp test!");};
+
+private:
+    float maxLinearVel;
+    float maxAngularVel;
+    float lookAheadDis, k_v;
+    float maxSlowAngularVel, minToleranceAngle;
+    //PID_Controller_C pid_;
+    std::shared_ptr<spdlog::logger> logger_;
+};
+
+#endif

src/auto_drive_misson_center/package.xml

@@ -12,6 +12,7 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>visualization_msgs</depend>
   <depend>auto_drive_proj_utils</depend>
   <depend>auto_drive_proj_msg</depend>
   <depend>libcurl-dev</depend>

src/auto_drive_misson_center/scripts/mock_start_pose_and_publish_rtk.py

@@ -20,20 +20,20 @@ class MockPoseNode(Node):
         )
         self.get_logger().info("Service /start_pose ready, always return success=True")
 
-        # 2. rtk_pose publisher
-        self.pub_rtk = self.create_publisher(
-            PoseWithCovarianceStamped,
-            "/rtk_pose",
-            10
-        )
+        # # 2. rtk_pose publisher
+        # self.pub_rtk = self.create_publisher(
+        #     PoseWithCovarianceStamped,
+        #     "/rtk_pose",
+        #     10
+        # )
 
-        self.url = url
-        self.coord_list = []
-        self.load_geojson()
+        # self.url = url
+        # self.coord_list = []
+        # self.load_geojson()
 
-        self.index = 0
-        self.timer_period = 0.2
-        self.timer = self.create_timer(self.timer_period, self.publish_rtk_pose_cb)
+        # self.index = 0
+        # self.timer_period = 0.2
+        # self.timer = self.create_timer(self.timer_period, self.publish_rtk_pose_cb)
 
     def load_geojson(self):
         try:
@@ -53,40 +53,40 @@ class MockPoseNode(Node):
         response.success = True
         return response
 
-    def publish_rtk_pose_cb(self):
-        if not self.coord_list:
-            return
+    # def publish_rtk_pose_cb(self):
+    #     if not self.coord_list:
+    #         return
 
-        lon, lat, alt = self.coord_list[self.index]
+    #     lon, lat, alt = self.coord_list[self.index]
 
-        msg = PoseWithCovarianceStamped()
-        msg.header.stamp = self.get_clock().now().to_msg()
-        msg.header.frame_id = "enu"
+    #     msg = PoseWithCovarianceStamped()
+    #     msg.header.stamp = self.get_clock().now().to_msg()
+    #     msg.header.frame_id = "enu"
 
-        msg.pose.pose.position.x = lon
-        msg.pose.pose.position.y = lat
-        msg.pose.pose.position.z = alt
+    #     msg.pose.pose.position.x = lon
+    #     msg.pose.pose.position.y = lat
+    #     msg.pose.pose.position.z = alt
 
-        msg.pose.pose.orientation.w = 1.0
-        msg.pose.pose.orientation.x = 0.0
-        msg.pose.pose.orientation.y = 0.0
-        msg.pose.pose.orientation.z = 0.0
+    #     msg.pose.pose.orientation.w = 1.0
+    #     msg.pose.pose.orientation.x = 0.0
+    #     msg.pose.pose.orientation.y = 0.0
+    #     msg.pose.pose.orientation.z = 0.0
 
-        for i in range(36):
-            msg.pose.covariance[i] = 0.0
+    #     for i in range(36):
+    #         msg.pose.covariance[i] = 0.0
 
-        self.pub_rtk.publish(msg)
+    #     self.pub_rtk.publish(msg)
 
-        self.index += 1
-        if self.index >= len(self.coord_list):
-            self.index = 0
+    #     self.index += 1
+    #     if self.index >= len(self.coord_list):
+    #         self.index = 0
 
 
 def main(args=None):
     # 解析命令行参数，注意：分离ros args和自定义argparse参数
     parser = argparse.ArgumentParser()
     parser.add_argument("--url", type=str,
-                        default="https://test.sweetai.cn:4433/load/locationCmdPath/1701148411059699712.json",
+                        default="https://test.sweetai.cn:4433/load/locationCmdPath/1700080420079468544.json",
                         help="geojson path url")
     parsed_args, ros_args = parser.parse_known_args(args)
 
@@ -119,4 +119,4 @@ if __name__ == "__main__":
 3. `/start_pose`服务不解析请求参数，任何入参直接返回`success=True`，完全匹配你的需求。
 4. frame_id 写为`map`，你可以根据实际工程改成`enu`。
 5. 发布周期`timer_period = 0.2`，可按需调快 / 调慢。
-'''
+'''

src/auto_drive_misson_center/src/mission_center_node.cpp

@@ -427,7 +427,7 @@ void MissionCenterNode::missionOnCallback(
         response->success = true;
         response->message = "Mission request accepted, processing in background";
         LOG_INFO("mission on cb, set main state to WAIT MIS RSP, Mission request accepted, processing in background");
-
+        LOG_INFO("========================== /invoke/missionOn request=  end");
     } catch (const std::exception& e) {
         response->success = false;
         response->message = std::string("Exception: ") + e.what();
@@ -744,6 +744,67 @@ std::string MissionCenterNode::downloadFile(const std::string& url)
     return readBuffer;
 }
 
+std::vector<std::vector<double>> MissionCenterNode::alignPathYaw(
+    const std::vector<std::vector<double>>& coordinates,
+    double vehicle_initial_yaw)
+{
+    std::vector<std::vector<double>> aligned_coordinates;
+
+    if (coordinates.empty())
+        return aligned_coordinates;
+
+    // Path 起点
+    const double origin_x = coordinates.front()[0];
+    const double origin_y = coordinates.front()[1];
+
+    // Path 原始起始航向
+    const double path_initial_yaw = coordinates.front()[2];
+
+    // 将 Path 初始航向旋转到车辆初始航向
+    const double yaw_offset =
+        vehicle_initial_yaw - path_initial_yaw;
+
+    const double cos_offset = std::cos(yaw_offset);
+    const double sin_offset = std::sin(yaw_offset);
+
+    aligned_coordinates.reserve(coordinates.size());
+
+    for (const auto& coord : coordinates)
+    {
+        if (coord.size() < 3)
+            continue;
+
+        const double dx = coord[0] - origin_x;
+        const double dy = coord[1] - origin_y;
+
+        const double x =
+            cos_offset * dx - sin_offset * dy;
+
+        const double y =
+            sin_offset * dx + cos_offset * dy;
+
+        double yaw = coord[2] + yaw_offset;
+
+        while (yaw > M_PI)
+            yaw -= 2.0 * M_PI;
+
+        while (yaw < -M_PI)
+            yaw += 2.0 * M_PI;
+
+        aligned_coordinates.push_back({
+            x,
+            y,
+            yaw
+        });
+        LOG_INFO(
+        "Path new: x={} y={} yaw={} deg",
+        x,
+        y,
+        yaw * 180.0 / M_PI);
+    }
+
+    return aligned_coordinates;
+}
 bool MissionCenterNode::processDownloadedData(const std::string& downloaded_data,
                                             std::string& mission_data, std::string& follow_data)
 {
@@ -786,8 +847,10 @@ bool MissionCenterNode::processDownloadedData(const std::string& downloaded_data
         }
 
         interpolation_map_.clear();
-        std::vector<std::vector<double>> interpolated_coords = interpolatePoints(coordinates, 0.5);
 
+        std::vector<std::vector<double>> aligned_coords = alignPathYaw(coordinates, vehicle_initial_yaw_);
+        std::vector<std::vector<double>> interpolated_coords = interpolatePoints(aligned_coords, 0.2);
+    
         json processed_data = data;
         json new_coordinates = json::array();
         for (const auto& coord : interpolated_coords) {
@@ -857,9 +920,28 @@ std::vector<std::vector<double>> MissionCenterNode::interpolatePoints(
                 double ratio = static_cast<double>(j) / num_segments;
                 double x = current[0] + dx * ratio;
                 double y = current[1] + dy * ratio;
-                result.push_back({x, y, current[2]});
-                inserted_points++;
-            }
+
+                 // yaw 插值
+                double yaw_diff = next[2] - current[2];
+                // normalize yaw_diff 到 [-pi, pi]
+                while (yaw_diff > M_PI) {
+                    yaw_diff -= 2.0 * M_PI;
+                }
+                while (yaw_diff < -M_PI) {
+                    yaw_diff += 2.0 * M_PI;
+                }
+                double yaw =current[2] + yaw_diff * ratio;
+                // normalize yaw 到 [-pi, pi]
+                while (yaw > M_PI) {
+                    yaw -= 2.0 * M_PI;
+                }
+                while (yaw < -M_PI) {
+                    yaw += 2.0 * M_PI;
+                }
+
+                    result.push_back({x, y, yaw});
+                    inserted_points++;
+                }
             interpolation_map_.push_back({i + 1, inserted_points});
         }
 

src/auto_drive_misson_center/src/path_follow_handler.cpp

@@ -9,7 +9,6 @@
 #include <chrono>
 #include <cmath>
 #include <limits>
-
 namespace auto_drive_misson_center {
 
 constexpr double PathFollowHandler::POS_LOST_TIMEOUT_SECONDS;
@@ -79,7 +78,7 @@ PathFollowHandler::PathFollowHandler(const std::string & node_name)
         10
     );
 
-    this->declare_parameter<std::string>("controller_type", "pure_pursuit");
+    this->declare_parameter<std::string>("controller_type", "lqr");
     this->get_parameter("controller_type", controller_type_);
     LOG_INFO("Controller type parameter: {}", controller_type_);
 
@@ -417,7 +416,7 @@ void PathFollowHandler::processRunningState()
         auto path_ind_msg = std_msgs::msg::Int32();
         path_ind_msg.data = path_controller_->getCurrentPointIndex() + path_ind_start_;
         path_ind_pub_->publish(path_ind_msg);
-
+        LOG_INFO("Published final path index 1: {}", path_ind_msg.data);
         saveCurrentStateToRedis();
 
         LOG_DEBUG("path controller, current pose point:x:{}, y:{}, yaw:{}; twist_msg pub:linear x:{}, angular z:{}, run is completed:{}, Current Point Index:{}, Path Point Count:{}", 
@@ -444,7 +443,7 @@ void PathFollowHandler::processRunningState()
                 }
                 final_ind_msg.data = total_points - 1;
                 path_ind_pub_->publish(final_ind_msg);
-
+                LOG_INFO("Published final path index 2: {}", final_ind_msg.data);
                 LOG_INFO("main state change(Running State:Mission completed):{} -> {}", mainStateToString(main_state_), "AUTO_OFF");
                 main_state_ = MainState::AUTO_OFF;
             } else {
@@ -693,7 +692,13 @@ bool PathFollowHandler::loadPathTrackingController(const std::string& controller
         LOG_INFO("Pure Pursuit path tracking controller loaded");
         reportEvent("info", "PATH_CONTROLLER_LOAD_OK", "Pure Pursuit控制器加载成功");
         return true;
-    } else {
+    } else if (controller_type == "stanley") {
+        path_controller_ = std::make_shared<StanleyControllerAdapter>(logger);
+        LOG_INFO("STAN path tracking controller loaded");
+        reportEvent("info", "PATH_CONTROLLER_LOAD_OK", "STAN控制器加载成功");
+        return true;
+    }
+    else {
         LOG_ERROR("load Path Tracking Controller ERROR");
     }