224 lines
5.1 KiB
Python
224 lines
5.1 KiB
Python
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() |