Local control algorithm test only
1.stanley test data accomplishment
This commit is contained in:
liuhaoyan
parent
e301e7cf67
commit
27c83bf0da
@ -1,5 +1,5 @@
|
||||
# auto drive misson center webots simulation:
|
||||
该分支修复了原LQR算法中矩阵错误问题，并对参数进行调教，增加stanley前轮转向控制算法，并适配webots仿真环境。后续实车测试优先使用该分支
|
||||
lhy本地控制测试分支，无本地控制测试需求请勿使用
|
||||
## 1, `ubuntu20.04`, `ubuntu22.04`
|
||||
## 2, 安装 `webots`
|
||||
`https://github.com/cyberbotics/webots/tags`， 版本： `ubuntu20.04： R2023a， ubuntu22.04：R2025a`
|
||||
|
||||
@ -10,12 +10,12 @@ import matplotlib.pyplot as plt
|
||||
# 参数
|
||||
# ============================================================
|
||||
|
||||
LOG_PATTERN = "logs/mission_center_logger*.txt"
|
||||
LOG_PATTERN = "path_visualizer.log"
|
||||
|
||||
# 每 5 个完整数据取 1 个
|
||||
SAMPLE_STEP = 40
|
||||
SAMPLE_STEP = 1
|
||||
|
||||
CSV_FILE = "recalculated_tracking_error.csv"
|
||||
CSV_FILE = "tracking_error.csv"
|
||||
|
||||
|
||||
# ============================================================
|
||||
@ -74,6 +74,9 @@ current_path_pattern = re.compile(
|
||||
r"current_path_yaw:([-+0-9.eE]+)"
|
||||
)
|
||||
|
||||
yaw_command_pattern = re.compile(
|
||||
r"current_cmd_angular_z:([-+0-9.eE]+)"
|
||||
)
|
||||
|
||||
# ============================================================
|
||||
# 3. 函数：角度归一化
|
||||
@ -178,6 +181,7 @@ def calculate_errors(
|
||||
# ============================================================
|
||||
|
||||
all_data = []
|
||||
yaw_command_data = []
|
||||
|
||||
total_raw_count = 0
|
||||
|
||||
@ -202,6 +206,7 @@ for file in files:
|
||||
# --------------------------------------------------------
|
||||
|
||||
pending_pose = None
|
||||
pending_yaw = None
|
||||
|
||||
|
||||
with file.open(
|
||||
@ -222,7 +227,11 @@ for file in files:
|
||||
line
|
||||
)
|
||||
|
||||
|
||||
m_yaw = yaw_command_pattern.search(line)
|
||||
if m_yaw:
|
||||
pending_yaw = float(m_yaw.group(1))
|
||||
if file_data_count % SAMPLE_STEP == 0:
|
||||
yaw_command_data.append(pending_yaw)
|
||||
if m_pose:
|
||||
|
||||
pending_pose = {
|
||||
@ -351,6 +360,7 @@ for file in files:
|
||||
pending_pose = None
|
||||
|
||||
|
||||
|
||||
print(
|
||||
f"原始完整数据：{file_data_count}"
|
||||
)
|
||||
@ -447,6 +457,12 @@ alpha_deg = np.array([
|
||||
for d in all_data
|
||||
])
|
||||
|
||||
yaw_command = yaw_command_data# 确保长度一致
|
||||
|
||||
print(
|
||||
f"yaw size: {len(yaw_command)}"
|
||||
)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 8. 计算车辆累计行驶距离
|
||||
@ -524,13 +540,16 @@ with open(
|
||||
|
||||
"heading_error_deg",
|
||||
|
||||
"yaw_command",
|
||||
|
||||
])
|
||||
|
||||
|
||||
for i in range(
|
||||
len(all_data)
|
||||
):
|
||||
|
||||
if i>= len(yaw_command):
|
||||
break
|
||||
writer.writerow([
|
||||
|
||||
f"{distance_m[i]:.8f}",
|
||||
@ -545,6 +564,8 @@ with open(
|
||||
|
||||
f"{alpha_deg[i]:.8f}",
|
||||
|
||||
f"{yaw_command[i]:.8f}"
|
||||
|
||||
])
|
||||
|
||||
|
||||
@ -630,7 +651,43 @@ plt.grid(True)
|
||||
plt.tight_layout()
|
||||
|
||||
plt.show()
|
||||
# ============================================================
|
||||
# 14. yaw command rad
|
||||
# ============================================================
|
||||
|
||||
plt.figure(
|
||||
figsize=(12, 5)
|
||||
)
|
||||
|
||||
plt.plot(
|
||||
distance_m,
|
||||
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()
|
||||
|
||||
# ============================================================
|
||||
# 15. 车辆轨迹 + 路径点
|
||||
@ -1,50 +1,67 @@
|
||||
import csv
|
||||
import os
|
||||
import re
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import glob
|
||||
|
||||
files = [
|
||||
"U_turn_0.32_lqr.csv",
|
||||
"U_turn_0.32_pp.csv",
|
||||
"U_turn_0.32_stanley.csv",
|
||||
"stanley_*_*_*_*.csv",
|
||||
]
|
||||
|
||||
data = {}
|
||||
for pattern in files:
|
||||
|
||||
for file in files:
|
||||
if not os.path.exists(file):
|
||||
raise FileNotFoundError(f"找不到文件: {file}")
|
||||
matched_files = glob.glob(pattern)
|
||||
|
||||
distance = []
|
||||
lateral_error = []
|
||||
heading_error = []
|
||||
yaw_command = []
|
||||
if not matched_files:
|
||||
raise FileNotFoundError(
|
||||
f"找不到文件: {pattern}"
|
||||
)
|
||||
matched_files.sort(
|
||||
key=lambda x: (
|
||||
float(x.split("_")[1]), # 速度
|
||||
float(x.split("_")[2]), # 参数1
|
||||
float(x.split("_")[3]), # 参数2
|
||||
float(x.split("_")[4].replace(".csv", "")) # 半径
|
||||
)
|
||||
)
|
||||
|
||||
with open(file, "r", encoding="utf-8-sig", newline="") as f:
|
||||
reader = csv.DictReader(f)
|
||||
print(f"匹配到 {len(matched_files)} 个文件:")
|
||||
for file in matched_files:
|
||||
if not os.path.exists(file):
|
||||
raise FileNotFoundError(f"找不到文件: {file}")
|
||||
|
||||
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}"
|
||||
)
|
||||
distance = []
|
||||
lateral_error = []
|
||||
heading_error = []
|
||||
yaw_command = []
|
||||
|
||||
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
|
||||
with open(file, "r", encoding="utf-8-sig", newline="") as f:
|
||||
reader = csv.DictReader(f)
|
||||
|
||||
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,
|
||||
}
|
||||
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())
|
||||
@ -56,21 +73,20 @@ plt.figure(figsize=(12, 6))
|
||||
|
||||
for name in names:
|
||||
plt.plot(
|
||||
data[name]["distance"],
|
||||
data[name]["lateral_error"],
|
||||
data[name]["distance"][::15],
|
||||
data[name]["lateral_error"][::15],
|
||||
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.legend()
|
||||
plt.tight_layout()
|
||||
|
||||
lateral_output = "U_turn_lateral_error_comparison.png"
|
||||
lateral_output = "Lateral_error_comparison.png"
|
||||
plt.savefig(lateral_output, dpi=200)
|
||||
plt.show()
|
||||
|
||||
@ -92,10 +108,10 @@ 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.legend()
|
||||
plt.tight_layout()
|
||||
|
||||
heading_output = "U_turn_heading_error_comparison.png"
|
||||
heading_output = "Heading_error_comparison.png"
|
||||
plt.savefig(heading_output, dpi=200)
|
||||
plt.show()
|
||||
|
||||
@ -118,10 +134,10 @@ 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.legend()
|
||||
plt.tight_layout()
|
||||
|
||||
yaw_output = "U_turn_yaw_command_comparison.png"
|
||||
yaw_output = "Yaw_command_comparison.png"
|
||||
plt.savefig(yaw_output, dpi=200)
|
||||
plt.show()
|
||||
print("读取完成：")
|
||||
@ -134,7 +150,7 @@ print(heading_output)
|
||||
print(yaw_output)
|
||||
|
||||
|
||||
print("\n" + "=" * 100)
|
||||
print("\n" + "=" * 90)
|
||||
print(
|
||||
f"{'Controller':<20}"
|
||||
f"{'Lat Max(m)':>12}"
|
||||
@ -1,355 +0,0 @@
|
||||
#!/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")
|
||||
224
path_generate_json.py
Normal file
224
path_generate_json.py
Normal file
@ -0,0 +1,224 @@
|
||||
import json
|
||||
import math
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# ============================================================
|
||||
# 参数配置
|
||||
# ============================================================
|
||||
|
||||
# 起点：采用你提供的第一个路径点
|
||||
START_LAT = 30.80062100422322
|
||||
START_LON = 121.21541599992909
|
||||
|
||||
TURN_RADIUS = 1.0
|
||||
MISSION_SPEED = 0.64
|
||||
ARC_SPACING = 0.3
|
||||
|
||||
# stanley para
|
||||
K_YAW = 2.0
|
||||
K_LAT = 4.0
|
||||
|
||||
# 地球半径（m）
|
||||
EARTH_RADIUS = 6378137.0
|
||||
|
||||
|
||||
#
|
||||
first_line_length = 6.0
|
||||
first_arc_rad = math.pi # 180° 半圆
|
||||
|
||||
second_line_length = 8.0
|
||||
second_arc_rad = math.pi / 2 # 90° 四分之一圆
|
||||
|
||||
third_line_length = 5.0
|
||||
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 工具函数
|
||||
# ============================================================
|
||||
|
||||
def normalize_yaw(yaw):
|
||||
"""将 yaw 归一化到 [-pi, pi]。"""
|
||||
return math.atan2(math.sin(yaw), math.cos(yaw))
|
||||
|
||||
|
||||
def local_to_latlon(x, y):
|
||||
"""
|
||||
局部 ENU 坐标转经纬度。
|
||||
x: East，向东为正，单位 m
|
||||
y: North，向北为正，单位 m
|
||||
"""
|
||||
lat0 = math.radians(START_LAT)
|
||||
|
||||
lat = START_LAT + math.degrees(y / EARTH_RADIUS)
|
||||
lon = START_LON + math.degrees(
|
||||
x / (EARTH_RADIUS * math.cos(lat0))
|
||||
)
|
||||
|
||||
return lat, lon
|
||||
|
||||
|
||||
def add_point(points, x, y, yaw):
|
||||
"""添加一个 [latitude, longitude, yaw] 路径点。"""
|
||||
lat, lon = local_to_latlon(x, y)
|
||||
|
||||
points.append([
|
||||
round(lat, 12),
|
||||
round(lon, 12),
|
||||
round(normalize_yaw(yaw), 9)
|
||||
])
|
||||
|
||||
|
||||
def add_arc(points, cx, cy, radius,
|
||||
start_angle, sweep_angle, spacing):
|
||||
"""
|
||||
添加圆弧采样点，不重复添加圆弧起点。
|
||||
yaw 为圆弧的切线方向，单位 rad。
|
||||
"""
|
||||
arc_length = abs(radius * sweep_angle)
|
||||
num_segments = max(1, math.ceil(arc_length / spacing))
|
||||
|
||||
for i in range(1, num_segments + 1):
|
||||
ratio = i / num_segments
|
||||
angle = start_angle + sweep_angle * ratio
|
||||
|
||||
x = cx + radius * math.cos(angle)
|
||||
y = cy + radius * math.sin(angle)
|
||||
|
||||
# CCW 左转时，切线方向 = 圆心角 + pi/2
|
||||
yaw = angle + (
|
||||
math.pi / 2 if sweep_angle > 0
|
||||
else -math.pi / 2
|
||||
)
|
||||
|
||||
add_point(points, x, y, yaw)
|
||||
|
||||
|
||||
|
||||
coordinates = []
|
||||
|
||||
# 1. 第一段直线：长度 6 m
|
||||
# 起点 (0, 0)，终点 (6, 0)
|
||||
add_point(coordinates, 0, 0, 0.0)
|
||||
add_point(coordinates, first_line_length, 0, 0.0)
|
||||
|
||||
# 第一段圆弧起始索引
|
||||
first_turn_index = len(coordinates) - 1
|
||||
|
||||
# 2. 第一段半圆：R=1，逆时针 180°
|
||||
# 圆心 (6, 1)，从 -90° 转到 90°
|
||||
add_arc(
|
||||
coordinates,
|
||||
cx=first_line_length,
|
||||
cy=TURN_RADIUS,
|
||||
radius=TURN_RADIUS,
|
||||
start_angle=-math.pi / 2,
|
||||
sweep_angle= first_arc_rad,
|
||||
spacing=ARC_SPACING
|
||||
)
|
||||
|
||||
# 3. 第二段直线：长度 8 m
|
||||
# 半圆终点 (6, 2)，直线终点 (-2, 2)
|
||||
second_line_end_x = first_line_length - second_line_length
|
||||
second_line_end_y = 2 * TURN_RADIUS
|
||||
|
||||
add_point(
|
||||
coordinates,
|
||||
second_line_end_x,
|
||||
second_line_end_y,
|
||||
math.pi
|
||||
)
|
||||
# 第二段圆弧起始索引
|
||||
second_turn_index = len(coordinates) - 1
|
||||
|
||||
# 4. 第二段四分之一圆：顺时针 90°
|
||||
# 与第一段圆弧方向相反
|
||||
second_arc_cx = second_line_end_x
|
||||
second_arc_cy = second_line_end_y + TURN_RADIUS
|
||||
|
||||
add_arc(
|
||||
coordinates,
|
||||
cx=second_arc_cx,
|
||||
cy=second_arc_cy,
|
||||
radius=TURN_RADIUS,
|
||||
start_angle=-math.pi / 2,
|
||||
sweep_angle=-second_arc_rad,
|
||||
spacing=ARC_SPACING
|
||||
)
|
||||
|
||||
# 5. 最后一段直线：长度 5 m
|
||||
|
||||
third_line_start_x = second_arc_cx
|
||||
third_line_start_y = second_arc_cy
|
||||
|
||||
add_point(
|
||||
coordinates,
|
||||
third_line_start_x - TURN_RADIUS,
|
||||
third_line_start_y + third_line_length,
|
||||
math.pi / 2
|
||||
)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 构造符合现有项目的 GeoJSON
|
||||
# ============================================================
|
||||
|
||||
path_json = {
|
||||
"type": "FeatureCollection",
|
||||
"properties": {
|
||||
"turnRadius": TURN_RADIUS,
|
||||
"misionSpeed": MISSION_SPEED,
|
||||
"k_yaw": K_YAW,
|
||||
"k_lat": K_LAT
|
||||
},
|
||||
"features": [
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"turnInds": [
|
||||
first_turn_index,
|
||||
second_turn_index
|
||||
],
|
||||
"mission": [],
|
||||
|
||||
},
|
||||
"geometry": {
|
||||
"type": "LineString",
|
||||
"coordinates": coordinates
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
|
||||
# ============================================================
|
||||
# 保存文件
|
||||
# ============================================================
|
||||
|
||||
output_file = "test_path.json"
|
||||
|
||||
with open(output_file, "w", encoding="utf-8") as f:
|
||||
json.dump(
|
||||
path_json,
|
||||
f,
|
||||
ensure_ascii=False,
|
||||
indent=2
|
||||
)
|
||||
|
||||
print(f"JSON 文件已生成: {output_file}")
|
||||
print(f"路径点数量: {len(coordinates)}")
|
||||
print(f"turnInds: {path_json['features'][0]['properties']['turnInds']}")
|
||||
print(f"起点: {coordinates[0]}")
|
||||
print(f"终点: {coordinates[-1]}")
|
||||
|
||||
|
||||
fig, ax = plt.subplots(figsize=(10, 6))
|
||||
ax.plot([p[0] for p in coordinates], [p[1] for p in coordinates], "-o", markersize=3, label="Path")
|
||||
|
||||
ax.set_xlabel("X (m)")
|
||||
ax.set_ylabel("Y (m)")
|
||||
ax.axis("equal")
|
||||
ax.grid(True)
|
||||
ax.legend()
|
||||
plt.tight_layout()
|
||||
plt.show()
|
||||
3373
path_visualizer.log
Normal file
3373
path_visualizer.log
Normal file
File diff suppressed because it is too large
Load Diff
@ -23,6 +23,11 @@
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <std_msgs/msg/int32.hpp>
|
||||
|
||||
#include <spdlog/spdlog.h>
|
||||
#include <spdlog/sinks/stdout_color_sinks.h>
|
||||
#include <spdlog/sinks/rotating_file_sink.h>
|
||||
#include <spdlog/sinks/basic_file_sink.h>
|
||||
|
||||
class PathVisualizer : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
@ -38,6 +43,8 @@ private:
|
||||
double w);
|
||||
double normalize_angle(double angle);
|
||||
bool downloadPathJson(const std::string &url,std::string &json_data);
|
||||
bool downloadPathJson(const std::string &file_path,std::string &json_data,
|
||||
bool is_local_file);
|
||||
bool loadPathFromJson(const std::string &json_data);
|
||||
|
||||
double gps_to_x(double lat,double lon);
|
||||
@ -100,8 +107,8 @@ private:
|
||||
bool has_pose_;
|
||||
// contorl paramss
|
||||
double max_speed_ = 0.8;
|
||||
double max_yaw_rate_ = 2.0;
|
||||
double lookahead_distance_ =0.6;
|
||||
double max_yaw_rate_ = 5.0;
|
||||
double lookahead_distance_ =0.4;
|
||||
double max_interval_distance_ = 0.2;
|
||||
double origin_lat_;
|
||||
double origin_lon_;
|
||||
@ -110,13 +117,22 @@ private:
|
||||
int forward_index_limit_=100;
|
||||
int path_track_index_ = -1;
|
||||
|
||||
// stanley parametres
|
||||
double k_yaw_ = 1.0;
|
||||
double k_lat_ = 1.0;
|
||||
|
||||
|
||||
bool origin_set_= false;
|
||||
bool path_transed = false;
|
||||
std::string file_path_;
|
||||
std::string file_name_;
|
||||
std::string path_url_ ;
|
||||
std::string json_data_;
|
||||
std::unique_ptr<GeodeticConverter> geodetic_converter_;
|
||||
|
||||
// logger
|
||||
std::shared_ptr<spdlog::logger> logger_;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,5 +1,7 @@
|
||||
#include "sweet_webots_robot/PathVisualizer.hpp"
|
||||
|
||||
|
||||
|
||||
PathVisualizer::PathVisualizer(const std::string& file_name):
|
||||
Node("path_visualizer"),
|
||||
target_index_(0),
|
||||
@ -7,6 +9,7 @@ has_pose_(false)
|
||||
{
|
||||
file_name_ = file_name;
|
||||
path_url_ = "https://test.sweetai.cn:4433/load/locationCmdPath/" + file_name_;
|
||||
file_path_ = "/home/sweetai/auto_drive_misson_center_webots_simulation/test_path.json";
|
||||
RCLCPP_INFO(this->get_logger(),"PathVisualizer started");
|
||||
// pub
|
||||
cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/twist_cmd_vel",10);
|
||||
@ -39,9 +42,35 @@ has_pose_(false)
|
||||
// urdf pose
|
||||
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
|
||||
// path get
|
||||
downloadPathJson(path_url_,json_data_);
|
||||
downloadPathJson(file_path_,json_data_,true);
|
||||
loadPathFromJson(json_data_);
|
||||
|
||||
// logger初始化
|
||||
auto console_sink =
|
||||
std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
|
||||
|
||||
// 每次启动时清空原文件
|
||||
auto file_sink =
|
||||
std::make_shared<spdlog::sinks::basic_file_sink_mt>(
|
||||
"./path_visualizer.log",
|
||||
true // truncate：清空原文件
|
||||
);
|
||||
|
||||
std::vector<spdlog::sink_ptr> sinks{
|
||||
console_sink,
|
||||
file_sink
|
||||
};
|
||||
|
||||
logger_ = std::make_shared<spdlog::logger>(
|
||||
"path_visualizer",
|
||||
sinks.begin(),
|
||||
sinks.end()
|
||||
);
|
||||
|
||||
logger_->set_level(spdlog::level::debug);
|
||||
|
||||
logger_->info("PathVisualizer started");
|
||||
|
||||
|
||||
}
|
||||
using json = nlohmann::json;
|
||||
@ -122,6 +151,60 @@ void PathVisualizer::transformPathToVehicleStart(double vehicle_initial_yaw)
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
bool PathVisualizer::downloadPathJson(const std::string &file_path,
|
||||
std::string &json_data,
|
||||
bool is_local_file)
|
||||
{
|
||||
json_data.clear();
|
||||
|
||||
// 打开本地 JSON 文件
|
||||
std::ifstream file(file_path);
|
||||
if (!file.is_open())
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(),
|
||||
"Failed to open JSON file: %s",
|
||||
file_path.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// 读取文件全部内容
|
||||
std::stringstream buffer;
|
||||
buffer << file.rdbuf();
|
||||
|
||||
if (file.bad())
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(),
|
||||
"Failed to read JSON file: %s",
|
||||
file_path.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
json_data = buffer.str();
|
||||
|
||||
if (json_data.empty())
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(),
|
||||
"JSON file is empty: %s",
|
||||
file_path.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Path JSON loaded successfully, size = %zu, path = %s",
|
||||
json_data.size(),
|
||||
file_path.c_str());
|
||||
|
||||
return true;
|
||||
}
|
||||
bool PathVisualizer::downloadPathJson(const std::string &url,
|
||||
std::string &json_data)
|
||||
{
|
||||
@ -171,6 +254,16 @@ bool PathVisualizer::loadPathFromJson(
|
||||
{
|
||||
// 解析 JSON
|
||||
json data =json::parse(json_data);
|
||||
// 检查 properties
|
||||
if (!data.contains("properties") ||
|
||||
!data["properties"].is_object())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(),"JSON does not contain valid properties");
|
||||
return false;
|
||||
}
|
||||
max_speed_ = data["properties"]["misionSpeed"].get<double>();
|
||||
k_yaw_ = data["properties"]["k_yaw"].get<double>();
|
||||
k_lat_ = data["properties"]["k_lat"].get<double>();
|
||||
// 检查 features
|
||||
if (!data.contains("features") ||
|
||||
!data["features"].is_array() ||
|
||||
@ -601,10 +694,11 @@ void PathVisualizer::control(){
|
||||
"control has not pose.");
|
||||
return;
|
||||
}
|
||||
|
||||
double speed=max_speed_;
|
||||
publishVehicleInfoRviz();
|
||||
publishLocalPose();
|
||||
target_index_= findClosestPoint(forward_index_limit_);
|
||||
int current_index = target_index_;
|
||||
int target =findLookAheadPoint();
|
||||
|
||||
double target_x =path_x_[target];
|
||||
@ -617,15 +711,41 @@ void PathVisualizer::control(){
|
||||
double local_x =cos(yaw_)*dx+sin(yaw_)*dy;
|
||||
double local_y =-sin(yaw_)*dx+cos(yaw_)*dy;
|
||||
|
||||
double target_angle = std::atan2(dy, dx);
|
||||
double alpha = target_angle - yaw_;
|
||||
while (alpha > M_PI) alpha -= 2 * M_PI;
|
||||
while (alpha < -M_PI) alpha += 2 * M_PI;
|
||||
|
||||
/*Pure Pursuit*/
|
||||
double Ld =sqrt(local_x*local_x+local_y*local_y);
|
||||
double curvature =2.0*local_y/(Ld*Ld);
|
||||
// double Ld =sqrt(local_x*local_x+local_y*local_y);
|
||||
// double w_cmd =2.0*speed*sin(alpha)/(Ld);
|
||||
|
||||
/*Stanley*/
|
||||
double curent_path_x = path_x_[current_index];
|
||||
double curent_path_y = path_y_[current_index];
|
||||
double current_path_yaw =path_yaw_[current_index];
|
||||
alpha = current_path_yaw - yaw_;
|
||||
while (alpha > M_PI) alpha -= 2 * M_PI;
|
||||
while (alpha < -M_PI) alpha += 2 * M_PI;
|
||||
|
||||
double dx_path = x_ - curent_path_x;
|
||||
double dy_path = y_ - curent_path_y;
|
||||
double error_lat = -dx_path * sin(current_path_yaw) + dy_path * cos(current_path_yaw);
|
||||
double w_cmd =k_yaw_ * alpha - k_lat_ * std::atan2(error_lat,speed);
|
||||
|
||||
geometry_msgs::msg::Twist cmd;
|
||||
|
||||
double speed=max_speed_;
|
||||
cmd.linear.x =speed;
|
||||
cmd.angular.z =speed*curvature;
|
||||
logger_->debug("current_pose_x:{}, current_pose_y:{},current_pose_yaw:{}",
|
||||
x_, y_,yaw_
|
||||
);
|
||||
logger_->debug("current_path_x:{}, current_path_y:{},current_path_yaw:{}",
|
||||
curent_path_x,
|
||||
curent_path_y,
|
||||
current_path_yaw
|
||||
);
|
||||
cmd.linear.x = speed;
|
||||
cmd.angular.z = w_cmd;
|
||||
|
||||
|
||||
if(cmd.angular.z >max_yaw_rate_)
|
||||
{
|
||||
@ -637,7 +757,8 @@ void PathVisualizer::control(){
|
||||
cmd.angular.z=-max_yaw_rate_;
|
||||
}
|
||||
|
||||
//cmd_pub_->publish(cmd);
|
||||
cmd_pub_->publish(cmd);
|
||||
logger_->debug("current_cmd_angular_z:{}",cmd.angular.z);
|
||||
|
||||
// RCLCPP_INFO_THROTTLE(
|
||||
// this->get_logger(),
|
||||
|
||||
131
test_path.json
Normal file
131
test_path.json
Normal file
@ -0,0 +1,131 @@
|
||||
{
|
||||
"type": "FeatureCollection",
|
||||
"properties": {
|
||||
"turnRadius": 1.0,
|
||||
"misionSpeed": 0.64,
|
||||
"k_yaw": 2.0,
|
||||
"k_lat": 4.0
|
||||
},
|
||||
"features": [
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"turnInds": [
|
||||
1,
|
||||
13
|
||||
],
|
||||
"mission": []
|
||||
},
|
||||
"geometry": {
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
30.800621004223,
|
||||
121.215415999929,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
30.800621004223,
|
||||
121.215478749384,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
30.800621368104,
|
||||
121.215481695811,
|
||||
0.285599332
|
||||
],
|
||||
[
|
||||
30.800622430267,
|
||||
121.215484403537,
|
||||
0.571198664
|
||||
],
|
||||
[
|
||||
30.800624104662,
|
||||
121.215486653196,
|
||||
0.856797996
|
||||
],
|
||||
[
|
||||
30.80062625564,
|
||||
121.215488262536,
|
||||
1.142397329
|
||||
],
|
||||
[
|
||||
30.80062870894,
|
||||
121.215489101177,
|
||||
1.427996661
|
||||
],
|
||||
[
|
||||
30.800631265812,
|
||||
121.215489101177,
|
||||
1.713595993
|
||||
],
|
||||
[
|
||||
30.800633719113,
|
||||
121.215488262536,
|
||||
1.999195325
|
||||
],
|
||||
[
|
||||
30.80063587009,
|
||||
121.215486653196,
|
||||
2.284794657
|
||||
],
|
||||
[
|
||||
30.800637544485,
|
||||
121.215484403537,
|
||||
2.570393989
|
||||
],
|
||||
[
|
||||
30.800638606648,
|
||||
121.215481695811,
|
||||
2.855993321
|
||||
],
|
||||
[
|
||||
30.800638970529,
|
||||
121.215478749384,
|
||||
3.141592654
|
||||
],
|
||||
[
|
||||
30.800638970529,
|
||||
121.215395083444,
|
||||
3.141592654
|
||||
],
|
||||
[
|
||||
30.800639276622,
|
||||
121.215392376652,
|
||||
2.879793266
|
||||
],
|
||||
[
|
||||
30.800640174043,
|
||||
121.215389854323,
|
||||
2.617993878
|
||||
],
|
||||
[
|
||||
30.800641601633,
|
||||
121.21538768835,
|
||||
2.35619449
|
||||
],
|
||||
[
|
||||
30.800643462105,
|
||||
121.21538602634,
|
||||
2.094395102
|
||||
],
|
||||
[
|
||||
30.800645628671,
|
||||
121.215384981558,
|
||||
1.832595715
|
||||
],
|
||||
[
|
||||
30.800647953682,
|
||||
121.215384625202,
|
||||
1.570796327
|
||||
],
|
||||
[
|
||||
30.800692869446,
|
||||
121.215384625202,
|
||||
1.570796327
|
||||
]
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user