迁移到dhxdl
This commit is contained in:
王闯
@@ -0,0 +1,607 @@
|
||||
"""Reeds-Shepp 曲线计算。
|
||||
|
||||
参照 Reeds & Shepp (1990) 论文公式 8.1-8.11 实现的纯 Python 版本。
|
||||
与常见实现（只返回最短路径）不同，这里保留全部 48 条候选 word，
|
||||
每条都带端点校验：只有正向积分能命中终点位姿的才标记为有效。
|
||||
|
||||
另外提供 include_dubins 选项：RS 的公式把弧长归一到 [-pi, pi) 并要求
|
||||
各段非负，结构上不可能输出超过 pi 的弧（RS 引理：最优路径不含 > pi 的
|
||||
弧，因为总能换一个带 cusp 的走法把它变短）。代价是绕远的纯前进解会
|
||||
直接从候选里消失。开启后用 [0, 2pi) 把这 6 条 Dubins 解重解出来，
|
||||
其中 L+R+L+ / R+L+R+ 不在标准 48 word 内。
|
||||
|
||||
约定（归一化坐标，转弯半径 = 1）：
|
||||
steering: +1 左转(L) / 0 直行(S) / -1 右转(R)
|
||||
gear: +1 前进(+) / -1 倒车(-)
|
||||
length: 段长度，>= 0（曲线段为转过的弧度，直线段为距离）
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Callable
|
||||
|
||||
PI = math.pi
|
||||
|
||||
|
||||
@dataclass
|
||||
class Segment:
|
||||
steering: int # +1 L, 0 S, -1 R
|
||||
gear: int # +1 forward, -1 backward
|
||||
length: float # >= 0
|
||||
|
||||
|
||||
def _make(length: float, steering: int, gear: int) -> Segment:
|
||||
"""构造段，若 length 为负则翻转 gear 并取绝对值。"""
|
||||
if length < 0:
|
||||
return Segment(steering, -gear, -length)
|
||||
return Segment(steering, gear, length)
|
||||
|
||||
|
||||
def _polar(x: float, y: float):
|
||||
return math.hypot(x, y), math.atan2(y, x)
|
||||
|
||||
|
||||
def _mod2pi(theta: float) -> float:
|
||||
"""归一化到 [-pi, pi)。"""
|
||||
v = theta % (2 * PI)
|
||||
if v >= PI:
|
||||
v -= 2 * PI
|
||||
return v
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 基础公式：每个函数尝试用某一类 word 连接 (0,0,0) 到 (x,y,phi)。
|
||||
# phi 为弧度。成功返回段列表，失败返回 None。
|
||||
# 这些是论文 8.1-8.11 的标准解析解。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _LpSpLp(x, y, phi):
|
||||
"""CSC，曲线同向：L+ S+ L+ (公式 8.1)。"""
|
||||
u, t = _polar(x - math.sin(phi), y - 1 + math.cos(phi))
|
||||
if t >= 0:
|
||||
v = _mod2pi(phi - t)
|
||||
if v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(0, +1, u), Segment(+1, +1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpSpRp(x, y, phi):
|
||||
"""CSC，曲线反向：L+ S+ R+ (公式 8.2)。"""
|
||||
u1, t1 = _polar(x + math.sin(phi), y - 1 - math.cos(phi))
|
||||
if u1 ** 2 < 4:
|
||||
return None
|
||||
u = math.sqrt(u1 ** 2 - 4)
|
||||
_, theta = _polar(u, 2.0)
|
||||
t = _mod2pi(t1 + theta)
|
||||
v = _mod2pi(t - phi)
|
||||
if t >= 0 and v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(0, +1, u), Segment(-1, +1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpRnLp(x, y, phi):
|
||||
"""CCC：L+ R- L+ (公式 8.3)。"""
|
||||
xi = x - math.sin(phi)
|
||||
eta = y - 1 + math.cos(phi)
|
||||
u1, theta = _polar(xi, eta)
|
||||
if u1 > 4:
|
||||
return None
|
||||
A = math.acos(u1 / 4.0)
|
||||
t = _mod2pi(theta + PI / 2 + A)
|
||||
u = _mod2pi(PI - 2 * A)
|
||||
v = _mod2pi(phi - t - u)
|
||||
if t >= 0 and u >= 0 and v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(-1, -1, u), Segment(+1, +1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpRnLn(x, y, phi):
|
||||
"""CCC：L+ R- L- (公式 8.4 变体)。"""
|
||||
xi = x - math.sin(phi)
|
||||
eta = y - 1 + math.cos(phi)
|
||||
u1, theta = _polar(xi, eta)
|
||||
if u1 > 4:
|
||||
return None
|
||||
A = math.acos(u1 / 4.0)
|
||||
t = _mod2pi(theta + PI / 2 + A)
|
||||
u = _mod2pi(PI - 2 * A)
|
||||
v = _mod2pi(t + u - phi)
|
||||
if t >= 0 and u >= 0 and v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(-1, -1, u), Segment(+1, -1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpRnSnLn(x, y, phi):
|
||||
"""CCSC：L+ R- S- L- (公式 8.9)。OMPL 修正形式，长度带符号。"""
|
||||
xi = x - math.sin(phi)
|
||||
eta = y - 1 + math.cos(phi)
|
||||
rho, theta = _polar(xi, eta)
|
||||
if rho < 2:
|
||||
return None
|
||||
r = math.sqrt(rho ** 2 - 4)
|
||||
u = 2 - r
|
||||
t = _mod2pi(theta + math.atan2(r, -2))
|
||||
v = _mod2pi(phi - PI / 2 - t)
|
||||
return [_make(t, +1, +1), _make(-PI / 2, -1, +1),
|
||||
_make(u, 0, +1), _make(v, +1, +1)]
|
||||
|
||||
|
||||
def _LpRnSnRn(x, y, phi):
|
||||
"""CCSC：L+ R- S- R- (公式 8.10)。OMPL 修正形式，长度带符号。"""
|
||||
xi = x + math.sin(phi)
|
||||
eta = y - 1 - math.cos(phi)
|
||||
rho, theta = _polar(-eta, xi)
|
||||
if rho < 2:
|
||||
return None
|
||||
t = theta
|
||||
u = 2 - rho
|
||||
v = _mod2pi(t + PI / 2 - phi)
|
||||
return [_make(t, +1, +1), _make(-PI / 2, -1, +1),
|
||||
_make(u, 0, +1), _make(v, -1, +1)]
|
||||
|
||||
|
||||
def _LpRnSnLnRp(x, y, phi):
|
||||
"""CCSCC：L+ R- S- L- R+ (公式 8.11)。OMPL 修正形式，长度带符号。"""
|
||||
xi = x + math.sin(phi)
|
||||
eta = y - 1 - math.cos(phi)
|
||||
rho, _ = _polar(xi, eta)
|
||||
if rho < 2:
|
||||
return None
|
||||
u = 4 - math.sqrt(rho ** 2 - 4)
|
||||
if u > 0:
|
||||
return None
|
||||
t = _mod2pi(math.atan2((4 - u) * xi - 2 * eta,
|
||||
-2 * xi + (u - 4) * eta))
|
||||
v = _mod2pi(t - phi)
|
||||
return [_make(t, +1, +1), _make(-PI / 2, -1, +1),
|
||||
_make(u, 0, +1), _make(-PI / 2, +1, +1),
|
||||
_make(v, -1, +1)]
|
||||
|
||||
|
||||
def _LpRupLunRn(x, y, phi):
|
||||
"""CCCC：L+ R+u L-u R- (公式 8.7)，两中段弧度相等。"""
|
||||
xi = x + math.sin(phi)
|
||||
eta = y - 1 - math.cos(phi)
|
||||
rho = (2 + math.hypot(xi, eta)) / 4.0
|
||||
if rho < 0 or rho > 1:
|
||||
return None
|
||||
u = math.acos(rho)
|
||||
t_ok, t, v = _calc_tauOmega(u, -u, xi, eta, phi)
|
||||
if t >= 0 and v <= 0:
|
||||
return [Segment(+1, +1, t), Segment(-1, +1, u),
|
||||
Segment(+1, -1, u), Segment(-1, -1, -v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpRunLunRp(x, y, phi):
|
||||
"""CCCC：L+ R-u L-u R+ (公式 8.8)。"""
|
||||
xi = x + math.sin(phi)
|
||||
eta = y - 1 - math.cos(phi)
|
||||
rho = (20 - xi ** 2 - eta ** 2) / 16.0
|
||||
if rho < 0 or rho > 1:
|
||||
return None
|
||||
u = -math.acos(rho)
|
||||
t_ok, t, v = _calc_tauOmega(u, u, xi, eta, phi)
|
||||
if t >= 0 and v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(-1, -1, -u),
|
||||
Segment(+1, -1, -u), Segment(-1, +1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _calc_tauOmega(u, v, xi, eta, phi):
|
||||
"""CCCC 族的 tau/omega 辅助计算（OMPL 同名函数）。"""
|
||||
delta = _mod2pi(u - v)
|
||||
A = math.sin(u) - math.sin(delta)
|
||||
B = math.cos(u) - math.cos(delta) - 1.0
|
||||
t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)
|
||||
t2 = 2 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0
|
||||
tau = _mod2pi(t1 + PI) if t2 < 0 else _mod2pi(t1)
|
||||
omega = _mod2pi(tau - u + v - phi)
|
||||
return True, tau, omega
|
||||
|
||||
|
||||
# 基础公式集合（含 word 标签，便于显示/调试）
|
||||
BASE_FORMULAS: list[tuple[str, Callable]] = [
|
||||
("LpSpLp", _LpSpLp),
|
||||
("LpSpRp", _LpSpRp),
|
||||
("LpRnLp", _LpRnLp),
|
||||
("LpRnLn", _LpRnLn),
|
||||
("LpRupLunRn", _LpRupLunRn),
|
||||
("LpRunLunRp", _LpRunLunRp),
|
||||
("LpRnSnLn", _LpRnSnLn),
|
||||
("LpRnSnRn", _LpRnSnRn),
|
||||
("LpRnSnLnRp", _LpRnSnLnRp),
|
||||
]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 对称变换：把段列表做镜像。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _timeflip(path: list[Segment]) -> list[Segment]:
|
||||
"""时间翻转：前进 <-> 倒车。"""
|
||||
return [Segment(s.steering, -s.gear, s.length) for s in path]
|
||||
|
||||
|
||||
def _reflect(path: list[Segment]) -> list[Segment]:
|
||||
"""左右镜像：L <-> R。"""
|
||||
return [Segment(-s.steering, s.gear, s.length) for s in path]
|
||||
|
||||
|
||||
def _backwards(path: list[Segment]) -> list[Segment]:
|
||||
"""路径反向：倒着走（段顺序翻转）。"""
|
||||
return [Segment(s.steering, s.gear, s.length) for s in reversed(path)]
|
||||
|
||||
|
||||
_STEER_CHAR = {+1: "L", 0: "S", -1: "R"}
|
||||
_GEAR_CHAR = {+1: "+", -1: "-"}
|
||||
|
||||
|
||||
def word_label(path: list[Segment]) -> str:
|
||||
"""把段列表转成可读 word，如 'L+S+R-'。"""
|
||||
return "".join(_STEER_CHAR[s.steering] + _GEAR_CHAR[s.gear] for s in path)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 正向积分：从一个位姿出发，沿各段前进，得到采样点和终点位姿。
|
||||
# 归一化坐标（转弯半径 = 1）。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def integrate(path: list[Segment], start=(0.0, 0.0, 0.0), step=0.05):
|
||||
"""返回 (xs, ys, end_pose)。xs/ys 为采样点，end_pose=(x,y,theta)。"""
|
||||
x, y, theta = start
|
||||
xs, ys = [x], [y]
|
||||
for seg in path:
|
||||
n = max(1, int(math.ceil(seg.length / step)))
|
||||
ds = seg.length / n
|
||||
for _ in range(n):
|
||||
if seg.steering == 0:
|
||||
x += seg.gear * ds * math.cos(theta)
|
||||
y += seg.gear * ds * math.sin(theta)
|
||||
else:
|
||||
nt = theta + seg.gear * seg.steering * ds
|
||||
x += seg.steering * (math.sin(nt) - math.sin(theta))
|
||||
y -= seg.steering * (math.cos(nt) - math.cos(theta))
|
||||
theta = nt
|
||||
xs.append(x)
|
||||
ys.append(y)
|
||||
return xs, ys, (x, y, theta)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 候选路径 + 公开 API
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@dataclass
|
||||
class Candidate:
|
||||
word: str # 如 "L+S+R-"
|
||||
segments: list[Segment] # 归一化坐标下的段
|
||||
length: float # 总段长（归一化）
|
||||
valid: bool # 正向积分是否命中目标位姿
|
||||
kind: str = "RS" # "RS" 标准 48 word / "Dubins" 纯前进绕远解
|
||||
max_arc: float = 0.0 # 最长曲线段弧度（归一化，与半径无关）
|
||||
|
||||
@property
|
||||
def key(self) -> str:
|
||||
"""GUI 用的唯一键。Dubins 解可能与 RS 同名，故加前缀区分。"""
|
||||
return (DUBINS_KEY_PREFIX + self.word if self.kind == "Dubins"
|
||||
else self.word)
|
||||
|
||||
@property
|
||||
def is_detour(self) -> bool:
|
||||
"""是否含超过 pi 的弧，即 RS 会主动丢弃的「绕远」。"""
|
||||
return self.max_arc > PI + 1e-6
|
||||
|
||||
|
||||
def _path_length(path):
|
||||
return sum(s.length for s in path)
|
||||
|
||||
|
||||
def _solve_all(x, y, phi):
|
||||
"""对归一化目标位姿 (x,y,phi)，用全部基础公式 × 4 对称求解。
|
||||
|
||||
返回 {word: segments}，每个 word 取最短解。
|
||||
4 种对称：identity / timeflip / reflect / timeflip+reflect。
|
||||
"""
|
||||
results: dict[str, list[Segment]] = {}
|
||||
|
||||
def add(path):
|
||||
if path is None:
|
||||
return
|
||||
w = word_label(path)
|
||||
if w not in results or _path_length(path) < _path_length(results[w]):
|
||||
results[w] = path
|
||||
|
||||
for _, f in BASE_FORMULAS:
|
||||
# identity
|
||||
add(f(x, y, phi))
|
||||
# timeflip: 解 f(-x, y, -phi)，再翻转 gear
|
||||
p = f(-x, y, -phi)
|
||||
add(_timeflip(p) if p else None)
|
||||
# reflect: 解 f(x, -y, -phi)，再 L<->R
|
||||
p = f(x, -y, -phi)
|
||||
add(_reflect(p) if p else None)
|
||||
# timeflip + reflect
|
||||
p = f(-x, -y, phi)
|
||||
add(_timeflip(_reflect(p)) if p else None)
|
||||
|
||||
# backwards：在目标坐标系中表达起点，求解后反转段顺序。
|
||||
# 这扩展出 CSCC 等以倒车段起步的 word。
|
||||
xb = x * math.cos(phi) + y * math.sin(phi)
|
||||
yb = x * math.sin(phi) - y * math.cos(phi)
|
||||
for _, f in BASE_FORMULAS:
|
||||
p = f(xb, yb, phi)
|
||||
add(_backwards(p) if p else None)
|
||||
p = f(-xb, yb, -phi)
|
||||
add(_backwards(_timeflip(p)) if p else None)
|
||||
p = f(xb, -yb, -phi)
|
||||
add(_backwards(_reflect(p)) if p else None)
|
||||
p = f(-xb, -yb, phi)
|
||||
add(_backwards(_timeflip(_reflect(p))) if p else None)
|
||||
|
||||
return results
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Dubins（纯前进）解：把 RS 主动丢弃的「绕远」路径找回来。
|
||||
#
|
||||
# 上面每个 RS 公式都把弧长过 _mod2pi 归一到 [-pi, pi) 再要求各段 >= 0，
|
||||
# 所以结构上不可能输出弧长 > pi 的段。这正是 RS 定理的引理：最优路径
|
||||
# 不含超过 pi 的弧——因为总能换一个带 cusp 的走法把它变短。代价是：
|
||||
# 一旦出现绕远的苗头，那条绕远的纯前进解就直接从候选里消失，换挡解顶上。
|
||||
#
|
||||
# 纯前进的绕远解恰好就是 Dubins 的 6 个 word，所以这里把同样的相切几何
|
||||
# 用 _mod2pi_pos（[0, 2pi)）重解一遍，即可把它们保留下来。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _mod2pi_pos(theta: float) -> float:
|
||||
"""归一化到 [0, 2pi)。与 _mod2pi 的唯一区别就是「允许绕远」。"""
|
||||
return theta % (2 * PI)
|
||||
|
||||
|
||||
def _dubins_LSL(x, y, phi):
|
||||
"""L+ S+ L+，弧长可到 2pi。两个左转圆的外公切线，恒有解。"""
|
||||
u, theta = _polar(x - math.sin(phi), y - 1 + math.cos(phi))
|
||||
# 直线段方向即两圆心连线方向 theta，故首尾弧把朝向从 0 转到 theta、再到 phi
|
||||
return [Segment(+1, +1, _mod2pi_pos(theta)),
|
||||
Segment(0, +1, u),
|
||||
Segment(+1, +1, _mod2pi_pos(phi - theta))]
|
||||
|
||||
|
||||
def _dubins_LSR(x, y, phi):
|
||||
"""L+ S+ R+，弧长可到 2pi。圆心距 < 2 时无内公切线。"""
|
||||
d, theta = _polar(x + math.sin(phi), y - 1 - math.cos(phi))
|
||||
if d < 2:
|
||||
return None
|
||||
u = math.sqrt(d * d - 4)
|
||||
t = _mod2pi_pos(theta + math.atan2(2.0, u)) # 内公切线方向
|
||||
return [Segment(+1, +1, t),
|
||||
Segment(0, +1, u),
|
||||
Segment(-1, +1, _mod2pi_pos(t - phi))]
|
||||
|
||||
|
||||
def _dubins_LRL(x, y, phi):
|
||||
"""L+ R+ L+，中段绕远弧（>= pi）。返回候选列表（两个相切分支）。
|
||||
|
||||
与 _LpRnLp 完全相同的三圆几何：起点左转圆 C1、终点左转圆 C3，
|
||||
中间右转圆 C2 与两者相切（|C1C2| = |C2C3| = 2，故需 |C1C3| <= 4）。
|
||||
C2 在 C1C3 两侧各有一个，A = acos(u1/4) 为 C1 处的半张角。
|
||||
|
||||
C2 上两切点之间有两段弧：短的 pi - 2A、长的 pi + 2A。倒着走中段
|
||||
(R-) 取短弧，即 RS 的 L+R-L+；前进走中段 (R+) 只能取长弧，即
|
||||
Dubins 的 L+R+L+。同样三个圆，两种走法——这就是「绕远」的来源。
|
||||
"""
|
||||
xi = x - math.sin(phi)
|
||||
eta = y - 1 + math.cos(phi)
|
||||
u1, theta = _polar(xi, eta)
|
||||
if u1 > 4:
|
||||
return []
|
||||
A = math.acos(min(1.0, u1 / 4.0))
|
||||
out = []
|
||||
# 两个相切分支：C2 方向为 theta + A 或 theta - A
|
||||
for branch in (+1, -1):
|
||||
t = _mod2pi_pos(theta + branch * A + PI / 2)
|
||||
# 长短弧都试，由 compute_paths 的端点校验筛出几何自洽的那个
|
||||
for u in (PI + 2 * A, PI - 2 * A):
|
||||
v = _mod2pi_pos(phi - t + u)
|
||||
out.append([Segment(+1, +1, t), Segment(-1, +1, u),
|
||||
Segment(+1, +1, v)])
|
||||
return out
|
||||
|
||||
|
||||
# Dubins 基础公式。前两个返回单个解，LRL 返回候选列表，统一成列表处理。
|
||||
_DUBINS_FORMULAS: list[Callable] = [
|
||||
lambda x, y, p: [r] if (r := _dubins_LSL(x, y, p)) else [],
|
||||
lambda x, y, p: [r] if (r := _dubins_LSR(x, y, p)) else [],
|
||||
_dubins_LRL,
|
||||
]
|
||||
|
||||
# Dubins 的 6 个 word。前 4 个与 RS 的 CSC 同名（同一 word，RS 只在
|
||||
# 各段弧长都 <= pi 时才给解，绕远时返回 None）；后 2 个不在 RS 的 48 里。
|
||||
DUBINS_WORDS: list[str] = [
|
||||
"L+S+L+", "L+S+R+", "R+S+L+", "R+S+R+", "L+R+L+", "R+L+R+",
|
||||
]
|
||||
|
||||
# 只有这两个 word 是 RS 48 个 word 之外的，专属于 Dubins。
|
||||
DUBINS_ONLY_WORDS: list[str] = ["L+R+L+", "R+L+R+"]
|
||||
|
||||
# Dubins 候选在 word_map 里的键前缀（同名 word 与 RS 解共存时用于区分）
|
||||
DUBINS_KEY_PREFIX = "D:"
|
||||
|
||||
|
||||
def _solve_dubins(x, y, phi, pos_tol=1e-2, ang_tol=1e-2):
|
||||
"""纯前进（Dubins）解，允许弧长 > pi。返回 {word: segments}，每 word 取最短。
|
||||
|
||||
只用 reflect 对称（L<->R），不用 timeflip——翻转挡位就不是纯前进了。
|
||||
|
||||
注意：_dubins_LRL 会投机地给出 4 个分支（2 个相切圆 × 长/短中段弧），
|
||||
只有部分几何自洽。必须先做端点校验再比长度，否则「取最短」可能留下
|
||||
一个不可达的分支、把真解挤掉。
|
||||
"""
|
||||
results: dict[str, list[Segment]] = {}
|
||||
|
||||
def add(path):
|
||||
if path is None:
|
||||
return
|
||||
if any(s.length < -1e-9 for s in path):
|
||||
return
|
||||
_, _, (ex, ey, eth) = integrate(path, start=(0.0, 0.0, 0.0))
|
||||
if (math.hypot(ex - x, ey - y) > pos_tol
|
||||
or abs(_mod2pi(eth - phi)) > ang_tol):
|
||||
return
|
||||
w = word_label(path)
|
||||
if w not in results or _path_length(path) < _path_length(results[w]):
|
||||
results[w] = path
|
||||
|
||||
for f in _DUBINS_FORMULAS:
|
||||
for p in f(x, y, phi):
|
||||
add(p)
|
||||
# reflect: 解 f(x, -y, -phi) 再 L<->R，得到 RSR / RSL / R+L+R+
|
||||
for p in f(x, -y, -phi):
|
||||
add(_reflect(p))
|
||||
return results
|
||||
|
||||
|
||||
def _max_arc(segments) -> float:
|
||||
"""路径中最长的曲线段弧度（直线段不计）。用于判定是否「绕远」。"""
|
||||
arcs = [s.length for s in segments if s.steering != 0]
|
||||
return max(arcs) if arcs else 0.0
|
||||
|
||||
|
||||
def compute_paths(start, goal, turning_radius=1.0, pos_tol=1e-2, ang_tol=1e-2,
|
||||
include_dubins=False):
|
||||
"""计算从 start 到 goal 的全部 Reeds-Shepp 候选路径。
|
||||
|
||||
start, goal: (x, y, theta_rad)，世界坐标。
|
||||
返回 Candidate 列表，按总长度升序；带端点校验的 valid 标记。
|
||||
长度单位与输入坐标一致（已乘回 turning_radius）。
|
||||
"""
|
||||
sx, sy, sth = start
|
||||
gx, gy, gth = goal
|
||||
|
||||
# 变换到以 start 为原点、朝向为 +x、半径归一化的局部坐标
|
||||
dx, dy = gx - sx, gy - sy
|
||||
c, s = math.cos(sth), math.sin(sth)
|
||||
lx = (c * dx + s * dy) / turning_radius
|
||||
ly = (-s * dx + c * dy) / turning_radius
|
||||
lphi = _mod2pi(gth - sth)
|
||||
|
||||
solutions = _solve_all(lx, ly, lphi)
|
||||
|
||||
canonical = set(ALL_WORDS)
|
||||
candidates = []
|
||||
for word, segs in solutions.items():
|
||||
# 只保留标准 48 word。RS 定理保证最优解必在其中；
|
||||
# 对称展开偶尔会产出几何正确但非标准（恒次优）的 word，在此剔除。
|
||||
if word not in canonical:
|
||||
continue
|
||||
# 端点校验：在局部归一化坐标下正向积分，须命中 (lx, ly, lphi)
|
||||
_, _, (ex, ey, eth) = integrate(segs, start=(0.0, 0.0, 0.0))
|
||||
ok = (math.hypot(ex - lx, ey - ly) < pos_tol
|
||||
and abs(_mod2pi(eth - lphi)) < ang_tol)
|
||||
scaled = [Segment(s_.steering, s_.gear, s_.length * turning_radius)
|
||||
for s_ in segs]
|
||||
candidates.append(Candidate(word, scaled,
|
||||
_path_length(segs) * turning_radius, ok,
|
||||
kind="RS", max_arc=_max_arc(segs)))
|
||||
|
||||
if include_dubins:
|
||||
for word, segs in _solve_dubins(lx, ly, lphi, pos_tol, ang_tol).items():
|
||||
scaled = [Segment(s_.steering, s_.gear, s_.length * turning_radius)
|
||||
for s_ in segs]
|
||||
# _solve_dubins 内部已做端点校验，能出来的都是 valid
|
||||
candidates.append(
|
||||
Candidate(word, scaled,
|
||||
_path_length(segs) * turning_radius, True,
|
||||
kind="Dubins", max_arc=_max_arc(segs)))
|
||||
|
||||
candidates.sort(key=lambda cc: (not cc.valid, cc.length))
|
||||
return candidates
|
||||
|
||||
|
||||
def sample_path(candidate: "Candidate", start, turning_radius=1.0, step=0.05):
|
||||
"""把候选路径在世界坐标下采样为 (xs, ys)，供绘图。"""
|
||||
norm_segs = [Segment(s.steering, s.gear, s.length / turning_radius)
|
||||
for s in candidate.segments]
|
||||
xs, ys, _ = integrate(norm_segs, start=(0.0, 0.0, 0.0), step=step)
|
||||
sx, sy, sth = start
|
||||
c, s = math.cos(sth), math.sin(sth)
|
||||
wx = [sx + turning_radius * (c * x - s * y) for x, y in zip(xs, ys)]
|
||||
wy = [sy + turning_radius * (s * x + c * y) for x, y in zip(xs, ys)]
|
||||
return wx, wy
|
||||
|
||||
|
||||
def sample_path_segments(candidate: "Candidate", start,
|
||||
turning_radius=1.0, step=0.05):
|
||||
"""逐段采样，返回 [(gear, xs, ys), ...]。
|
||||
|
||||
gear=+1 前进 / -1 倒车。每段在世界坐标下，相邻段共享端点以保证连续。
|
||||
供 GUI 按前进/倒车分色绘制。
|
||||
"""
|
||||
sx, sy, sth = start
|
||||
c, s = math.cos(sth), math.sin(sth)
|
||||
|
||||
def to_world(lx, ly):
|
||||
return (sx + turning_radius * (c * lx - s * ly),
|
||||
sy + turning_radius * (s * lx + c * ly))
|
||||
|
||||
out = []
|
||||
pose = (0.0, 0.0, 0.0) # 归一化局部坐标
|
||||
for seg in candidate.segments:
|
||||
norm = Segment(seg.steering, seg.gear, seg.length / turning_radius)
|
||||
lxs, lys, pose = integrate([norm], start=pose, step=step)
|
||||
wx, wy = zip(*(to_world(x, y) for x, y in zip(lxs, lys)))
|
||||
out.append((seg.gear, list(wx), list(wy)))
|
||||
return out
|
||||
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 标准 48 个 word，按 Reeds-Shepp 路径族分组。
|
||||
# 顺序固定，供 GUI 的 48 个勾选框稳定布局使用。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
WORD_GROUPS: list[tuple[str, list[str]]] = [
|
||||
("CSC", [
|
||||
"L+S+L+", "L+S+R+", "L-S-L-", "L-S-R-",
|
||||
"R+S+L+", "R+S+R+", "R-S-L-", "R-S-R-",
|
||||
]),
|
||||
("CCC", [
|
||||
"L+R+L-", "L+R-L+", "L+R-L-", "L-R+L+", "L-R+L-", "L-R-L+",
|
||||
"R+L+R-", "R+L-R+", "R+L-R-", "R-L+R+", "R-L+R-", "R-L-R+",
|
||||
]),
|
||||
("CCCC", [
|
||||
"L+R+L-R-", "L+R-L-R+", "L-R+L+R-", "L-R-L+R+",
|
||||
"R+L+R-L-", "R+L-R-L+", "R-L+R+L-", "R-L-R+L+",
|
||||
]),
|
||||
("CCSC", [
|
||||
"L+R-S-L-", "L+R-S-R-", "L+S+L+R-", "L+S+R+L-",
|
||||
"L-R+S+L+", "L-R+S+R+", "L-S-L-R+", "L-S-R-L+",
|
||||
"R+L-S-L-", "R+L-S-R-", "R+S+L+R-", "R+S+R+L-",
|
||||
"R-L+S+L+", "R-L+S+R+", "R-S-L-R+", "R-S-R-L+",
|
||||
]),
|
||||
("CCSCC", [
|
||||
"L+R-S-L-R+", "L-R+S+L+R-", "R+L-S-R-L+", "R-L+S+R+L-",
|
||||
]),
|
||||
]
|
||||
|
||||
# 扁平化的 48 个 word（保持分组顺序）
|
||||
ALL_WORDS: list[str] = [w for _, group in WORD_GROUPS for w in group]
|
||||
|
||||
# Dubins（纯前进，允许绕远）分组。作为第 6 组附加在 GUI 里，键带 "D:" 前缀。
|
||||
DUBINS_GROUP: tuple[str, list[str]] = ("Dubins（纯前进/可绕远）", DUBINS_WORDS)
|
||||
|
||||
# GUI 用的完整槽位键列表：48 个 RS word + 6 个 Dubins word
|
||||
ALL_KEYS: list[str] = ALL_WORDS + [DUBINS_KEY_PREFIX + w for w in DUBINS_WORDS]
|
||||
|
||||
|
||||
def compute_word_map(start, goal, turning_radius=1.0, include_dubins=False):
|
||||
"""计算所有候选，返回 {key: Candidate}，仅含有效路径。
|
||||
|
||||
key 对 RS 解就是 word，对 Dubins 解带 "D:" 前缀（同名 word 可共存）。
|
||||
GUI 可用 ALL_KEYS 遍历槽位：在此映射中的为可达，否则置灰。
|
||||
"""
|
||||
cands = compute_paths(start, goal, turning_radius=turning_radius,
|
||||
include_dubins=include_dubins)
|
||||
return {c.key: c for c in cands if c.valid}
|
||||
|
||||
@@ -0,0 +1,550 @@
|
||||
"""Reeds-Shepp 曲线交互演示。
|
||||
|
||||
PySide6 + Matplotlib 界面：
|
||||
- 画布：两次点击设置起点/终点（第一下定位置，第二下定朝向）
|
||||
- 起点按钮 / 终点按钮：进入对应的点选模式
|
||||
- 48 个勾选框（按 RS 路径族分组）：勾选哪条就在画布上画哪条
|
||||
- 每次重设起/终点：重算全部路径，默认只勾选并显示最短的那条
|
||||
- 「保留 Dubins 绕远路径」开关：额外显示 6 条纯前进解（紫色虚线）。
|
||||
RS 的公式结构上不会输出超过 pi 的弧（一有绕远苗头就换成带 cusp 的
|
||||
解），勾上后把这些被丢弃的绕远路径找回来对比。
|
||||
|
||||
运行：
|
||||
python rs_demo.py
|
||||
"""
|
||||
|
||||
import sys
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
from PySide6 import QtCore, QtWidgets
|
||||
import matplotlib
|
||||
from matplotlib.backends.backend_qtagg import FigureCanvasQTAgg as FigureCanvas
|
||||
from matplotlib.figure import Figure
|
||||
from matplotlib.font_manager import findSystemFonts, FontProperties
|
||||
|
||||
import reeds_shepp as rs
|
||||
|
||||
TURNING_RADIUS = 1.5 # 默认转弯半径
|
||||
RADIUS_MIN = 1 # 滑块最小档（整数）
|
||||
RADIUS_MAX = 8 # 滑块最大档（整数）
|
||||
ARROW_LEN = 1.0 # 位姿朝向箭头长度（世界坐标）
|
||||
|
||||
# 线条配色：前进浅、倒车深；最短路径用红系，其余用蓝系。
|
||||
COLOR_SHORTEST_FWD = "#ff8a80" # 最短-前进（浅红）
|
||||
COLOR_SHORTEST_REV = "#b71c1c" # 最短-倒车（深红）
|
||||
COLOR_OTHER_FWD = "#90caf9" # 其它-前进（浅蓝）
|
||||
COLOR_OTHER_REV = "#0d47a1" # 其它-倒车（深蓝）
|
||||
RED_TEXT = "#c62828" # 最短路径勾选框文字色
|
||||
|
||||
# Dubins（纯前进，可绕远）路径用紫系虚线，与 RS 明显区分
|
||||
COLOR_DUBINS = "#ce93d8" # Dubins-普通（浅紫）
|
||||
COLOR_DUBINS_BEST = "#6a1b9a" # Dubins-最短（深紫）
|
||||
PURPLE_TEXT = "#6a1b9a" # Dubins 最短勾选框文字色
|
||||
|
||||
|
||||
def _setup_cjk_font():
|
||||
"""让 Matplotlib 画布能显示中文。找到 CJK 字体就用，否则返回 False。"""
|
||||
candidates = ["Noto Sans CJK SC", "Noto Sans CJK JP", "WenQuanYi Zen Hei",
|
||||
"WenQuanYi Micro Hei", "Microsoft YaHei", "SimHei",
|
||||
"Source Han Sans SC", "Droid Sans Fallback"]
|
||||
available = set()
|
||||
for f in findSystemFonts():
|
||||
try:
|
||||
available.add(FontProperties(fname=f).get_name())
|
||||
except (RuntimeError, OSError):
|
||||
# 跳过损坏或无法识别的字体文件
|
||||
continue
|
||||
for name in candidates:
|
||||
if name in available:
|
||||
matplotlib.rcParams["font.sans-serif"] = [name]
|
||||
matplotlib.rcParams["axes.unicode_minus"] = False
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
HAS_CJK = _setup_cjk_font()
|
||||
|
||||
|
||||
def _t(zh, en):
|
||||
"""画布文本：有中文字体用中文，否则退回英文。"""
|
||||
return zh if HAS_CJK else en
|
||||
|
||||
|
||||
class Canvas(FigureCanvas):
|
||||
"""承载 Matplotlib 绘图并捕获鼠标点击的画布。"""
|
||||
|
||||
pose_picked = QtCore.Signal(float, float, float) # x, y, theta
|
||||
|
||||
def __init__(self):
|
||||
self.fig = Figure(figsize=(6, 6))
|
||||
super().__init__(self.fig)
|
||||
self.ax = self.fig.add_subplot(111)
|
||||
# 当前视图范围，缩放时更新，使其在重画后保持
|
||||
self._xlim = (-10, 10)
|
||||
self._ylim = (-10, 10)
|
||||
self._reset_axes()
|
||||
# 两次点击的状态：第一次存位置，第二次定朝向
|
||||
self._pending_xy = None
|
||||
self._picking = False
|
||||
# 右键拖拽平移状态
|
||||
self._pan = None
|
||||
self.mpl_connect("button_press_event", self._on_click)
|
||||
self.mpl_connect("scroll_event", self._on_scroll)
|
||||
self.mpl_connect("motion_notify_event", self._on_motion)
|
||||
self.mpl_connect("button_release_event", self._on_release)
|
||||
|
||||
def _reset_axes(self):
|
||||
self.ax.set_xlim(*self._xlim)
|
||||
self.ax.set_ylim(*self._ylim)
|
||||
self.ax.set_aspect("equal")
|
||||
self.ax.grid(True, linestyle=":", alpha=0.5)
|
||||
self.ax.set_title(_t("点击「设置起点」或「设置终点」后，在画布上点两下",
|
||||
"Click a Set button, then click twice on canvas"))
|
||||
|
||||
def _on_scroll(self, event):
|
||||
"""滚轮缩放，以光标位置为中心。上滚放大，下滚缩小。"""
|
||||
if event.inaxes != self.ax or event.xdata is None:
|
||||
return
|
||||
scale = 0.83 if event.button == "up" else 1.2
|
||||
x0, x1 = self.ax.get_xlim()
|
||||
y0, y1 = self.ax.get_ylim()
|
||||
cx, cy = event.xdata, event.ydata
|
||||
self._xlim = (cx + (x0 - cx) * scale, cx + (x1 - cx) * scale)
|
||||
self._ylim = (cy + (y0 - cy) * scale, cy + (y1 - cy) * scale)
|
||||
self.ax.set_xlim(*self._xlim)
|
||||
self.ax.set_ylim(*self._ylim)
|
||||
self.draw()
|
||||
|
||||
def start_picking(self, color="orange"):
|
||||
"""进入点选模式，等待两次点击。color 为预览箭头颜色。"""
|
||||
self._picking = True
|
||||
self._pending_xy = None
|
||||
self._pick_color = color
|
||||
self._preview = None # 跟随鼠标的预览箭头 artist
|
||||
|
||||
def _clear_preview(self):
|
||||
if getattr(self, "_preview", None) is not None:
|
||||
self._preview.remove()
|
||||
self._preview = None
|
||||
|
||||
def _on_click(self, event):
|
||||
# 右键：开始拖拽平移（记录像素起点与当时的视图范围）
|
||||
if event.button == 3:
|
||||
self._pan = (event.x, event.y,
|
||||
self.ax.get_xlim(), self.ax.get_ylim())
|
||||
return
|
||||
if not self._picking or event.inaxes != self.ax or event.button != 1:
|
||||
return
|
||||
if self._pending_xy is None:
|
||||
# 第一次点击：记录位置，之后箭头跟随鼠标旋转
|
||||
self._pending_xy = (event.xdata, event.ydata)
|
||||
self.ax.plot(event.xdata, event.ydata, "o",
|
||||
color=self._pick_color, ms=6)
|
||||
self.draw()
|
||||
else:
|
||||
# 第二次点击：与第一点连线方向即朝向，固定箭头
|
||||
x0, y0 = self._pending_xy
|
||||
theta = math.atan2(event.ydata - y0, event.xdata - x0)
|
||||
self._picking = False
|
||||
self._pending_xy = None
|
||||
self._clear_preview()
|
||||
self.pose_picked.emit(x0, y0, theta)
|
||||
|
||||
def _draw_preview(self, x0, y0, theta):
|
||||
"""画/更新跟随鼠标的预览箭头。"""
|
||||
self._clear_preview()
|
||||
self._preview = self.ax.arrow(
|
||||
x0, y0, ARROW_LEN * math.cos(theta), ARROW_LEN * math.sin(theta),
|
||||
head_width=0.4, head_length=0.4, fc=self._pick_color,
|
||||
ec=self._pick_color, alpha=0.6, zorder=7,
|
||||
length_includes_head=True)
|
||||
self.draw()
|
||||
|
||||
def _on_motion(self, event):
|
||||
# 第一次点击后、第二次点击前：箭头跟随鼠标旋转
|
||||
if (self._picking and self._pending_xy is not None
|
||||
and event.inaxes == self.ax and event.xdata is not None):
|
||||
x0, y0 = self._pending_xy
|
||||
if event.xdata != x0 or event.ydata != y0:
|
||||
theta = math.atan2(event.ydata - y0, event.xdata - x0)
|
||||
self._draw_preview(x0, y0, theta)
|
||||
return
|
||||
# 右键拖拽：按像素位移平移视图
|
||||
if self._pan is None or event.x is None:
|
||||
return
|
||||
x0_px, y0_px, (xl0, xl1), (yl0, yl1) = self._pan
|
||||
# 像素 -> 数据坐标的缩放比例
|
||||
bbox = self.ax.get_window_extent()
|
||||
dx = (event.x - x0_px) / bbox.width * (xl1 - xl0)
|
||||
dy = (event.y - y0_px) / bbox.height * (yl1 - yl0)
|
||||
self._xlim = (xl0 - dx, xl1 - dx)
|
||||
self._ylim = (yl0 - dy, yl1 - dy)
|
||||
self.ax.set_xlim(*self._xlim)
|
||||
self.ax.set_ylim(*self._ylim)
|
||||
self.draw()
|
||||
|
||||
def _on_release(self, event):
|
||||
if event.button == 3:
|
||||
self._pan = None
|
||||
|
||||
def render(self, start, goal, paths):
|
||||
"""重画整幅图。
|
||||
|
||||
paths 为 [(word, segments, is_shortest, is_dubins, is_detour), ...]，
|
||||
segments 为 [(gear, xs, ys), ...]，gear=+1 前进 / -1 倒车。
|
||||
RS：前进浅、倒车深；最短用红系，其余蓝系。
|
||||
Dubins：紫系虚线（纯前进，无倒挡），绕远的加粗。
|
||||
"""
|
||||
self.ax.clear()
|
||||
self._reset_axes()
|
||||
if start is not None:
|
||||
self._draw_pose(start, "green", _t("起点", "Start"))
|
||||
if goal is not None:
|
||||
self._draw_pose(goal, "red", _t("终点", "Goal"))
|
||||
for word, segments, is_shortest, is_dubins, is_detour in paths:
|
||||
self._draw_path(word, segments, is_shortest, is_dubins, is_detour)
|
||||
if paths:
|
||||
self.ax.legend(loc="upper left", fontsize=8)
|
||||
self.draw()
|
||||
|
||||
def _draw_path(self, word, segments, is_shortest, is_dubins=False,
|
||||
is_detour=False):
|
||||
if is_dubins:
|
||||
# Dubins 纯前进，不存在倒挡段，故 fwd/rev 同色；用虚线区分
|
||||
color = COLOR_DUBINS_BEST if is_shortest else COLOR_DUBINS
|
||||
fwd = rev = color
|
||||
lw = 2.8 if is_shortest else (2.0 if is_detour else 1.6)
|
||||
z = 4
|
||||
style = "--"
|
||||
tag = _t("(Dubins", "(Dubins")
|
||||
tag += _t("·绕远", "·detour") if is_detour else ""
|
||||
tag += _t("·最短)", "·shortest)") if is_shortest else ")"
|
||||
label = f"{word} {tag}"
|
||||
elif is_shortest:
|
||||
fwd, rev, lw, z = (COLOR_SHORTEST_FWD, COLOR_SHORTEST_REV, 2.8, 5)
|
||||
style = "-"
|
||||
# 高亮路径含倒挡段则标「倒挡最短」，否则为「总路径最短」
|
||||
has_rev = any(gear < 0 for gear, _, _ in segments)
|
||||
tag = (_t("(倒挡最短)", "(shortest w/ reverse)") if has_rev
|
||||
else _t("(总路径最短)", "(shortest overall)"))
|
||||
label = f"{word} " + tag
|
||||
else:
|
||||
fwd, rev, lw, z = (COLOR_OTHER_FWD, COLOR_OTHER_REV, 1.6, 3)
|
||||
style = "-"
|
||||
label = word
|
||||
labeled = False
|
||||
for gear, xs, ys in segments:
|
||||
color = fwd if gear > 0 else rev
|
||||
# 每条路径只给一段贴标签，避免图例重复
|
||||
self.ax.plot(xs, ys, style, color=color, lw=lw, zorder=z,
|
||||
label=(None if labeled else label))
|
||||
labeled = True
|
||||
|
||||
def _draw_pose(self, pose, color, label):
|
||||
x, y, th = pose
|
||||
self.ax.plot(x, y, "o", color=color, ms=9, zorder=6)
|
||||
self.ax.arrow(x, y, ARROW_LEN * math.cos(th), ARROW_LEN * math.sin(th),
|
||||
head_width=0.4, head_length=0.4, fc=color, ec=color,
|
||||
zorder=6, length_includes_head=True)
|
||||
self.ax.annotate(label, (x, y), textcoords="offset points",
|
||||
xytext=(8, 8), color=color, fontsize=9)
|
||||
|
||||
|
||||
class MainWindow(QtWidgets.QMainWindow):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.setWindowTitle("Reeds-Shepp 曲线演示")
|
||||
self.resize(1100, 760)
|
||||
|
||||
self.start = None
|
||||
self.goal = None
|
||||
self.word_map = {} # key -> Candidate（仅有效）
|
||||
self.shortest_word = None # RS 高亮键
|
||||
self.shortest_is_reverse = False
|
||||
self.dubins_best = None # Dubins 里最短的键（紫色高亮）
|
||||
self.checks = {} # key -> QCheckBox
|
||||
self.group_boxes = {} # family -> QGroupBox
|
||||
self._picking_target = None # "start" / "goal" / None
|
||||
self.turning_radius = float(TURNING_RADIUS)
|
||||
|
||||
self._build_ui()
|
||||
|
||||
# ---- UI 搭建 ----
|
||||
def _build_ui(self):
|
||||
central = QtWidgets.QWidget()
|
||||
self.setCentralWidget(central)
|
||||
layout = QtWidgets.QHBoxLayout(central)
|
||||
|
||||
self.canvas = Canvas()
|
||||
self.canvas.pose_picked.connect(self._on_pose_picked)
|
||||
layout.addWidget(self.canvas, stretch=3)
|
||||
|
||||
# 右侧控制面板
|
||||
panel = QtWidgets.QVBoxLayout()
|
||||
layout.addLayout(panel, stretch=1)
|
||||
|
||||
self.btn_start = QtWidgets.QPushButton("设置起点")
|
||||
self.btn_goal = QtWidgets.QPushButton("设置终点")
|
||||
self.btn_start.clicked.connect(lambda: self._begin_pick("start"))
|
||||
self.btn_goal.clicked.connect(lambda: self._begin_pick("goal"))
|
||||
panel.addWidget(self.btn_start)
|
||||
panel.addWidget(self.btn_goal)
|
||||
|
||||
# 转弯半径滑块（整数档位）
|
||||
self.radius_label = QtWidgets.QLabel()
|
||||
panel.addWidget(self.radius_label)
|
||||
self.radius_slider = QtWidgets.QSlider(QtCore.Qt.Horizontal)
|
||||
self.radius_slider.setMinimum(RADIUS_MIN)
|
||||
self.radius_slider.setMaximum(RADIUS_MAX)
|
||||
self.radius_slider.setSingleStep(1)
|
||||
self.radius_slider.setPageStep(1)
|
||||
self.radius_slider.setTickInterval(1)
|
||||
self.radius_slider.setTickPosition(QtWidgets.QSlider.TicksBelow)
|
||||
self.radius_slider.setValue(int(round(self.turning_radius)))
|
||||
self.turning_radius = float(self.radius_slider.value())
|
||||
self.radius_slider.valueChanged.connect(self._on_radius_changed)
|
||||
panel.addWidget(self.radius_slider)
|
||||
self._update_radius_label()
|
||||
|
||||
# Dubins 开关：保留 RS 主动丢弃的「绕远」纯前进路径
|
||||
self.chk_dubins = QtWidgets.QCheckBox("保留 Dubins 绕远路径（纯前进）")
|
||||
self.chk_dubins.setToolTip(
|
||||
"RS 公式把弧长归一到 [-pi, pi) 并要求各段非负，结构上不可能\n"
|
||||
"输出超过 pi 的弧——一有绕远苗头就换成带 cusp 的解。\n"
|
||||
"勾上后额外用 [0, 2pi) 重解纯前进几何，把绕远路径找回来。")
|
||||
self.chk_dubins.toggled.connect(self._on_dubins_toggled)
|
||||
panel.addWidget(self.chk_dubins)
|
||||
|
||||
# 显示模式：单选组（仅显示最短 / 全选所有可达）
|
||||
self.radio_shortest = QtWidgets.QRadioButton("仅显示倒挡最短路径")
|
||||
self.radio_all = QtWidgets.QRadioButton("全选所有可达路径")
|
||||
self.mode_group = QtWidgets.QButtonGroup(self)
|
||||
self.mode_group.addButton(self.radio_shortest)
|
||||
self.mode_group.addButton(self.radio_all)
|
||||
self.radio_shortest.setChecked(True)
|
||||
self.radio_shortest.setEnabled(False)
|
||||
self.radio_all.setEnabled(False)
|
||||
self.radio_shortest.toggled.connect(self._on_mode_changed)
|
||||
panel.addWidget(self.radio_shortest)
|
||||
panel.addWidget(self.radio_all)
|
||||
|
||||
self.status = QtWidgets.QLabel("请先设置起点和终点")
|
||||
self.status.setWordWrap(True)
|
||||
panel.addWidget(self.status)
|
||||
|
||||
panel.addWidget(self._build_checkbox_area())
|
||||
|
||||
def _build_checkbox_area(self):
|
||||
"""48 个 RS 勾选框 + 6 个 Dubins 勾选框，按族分组，放进可滚动区域。"""
|
||||
scroll = QtWidgets.QScrollArea()
|
||||
scroll.setWidgetResizable(True)
|
||||
container = QtWidgets.QWidget()
|
||||
vbox = QtWidgets.QVBoxLayout(container)
|
||||
|
||||
# RS 的 5 个族，键就是 word 本身
|
||||
groups = [(fam, [(w, w) for w in words])
|
||||
for fam, words in rs.WORD_GROUPS]
|
||||
# 附加 Dubins 族，键带 "D:" 前缀（word 可能与 RS 的 CSC 同名）
|
||||
dub_family, dub_words = rs.DUBINS_GROUP
|
||||
groups.append((dub_family,
|
||||
[(rs.DUBINS_KEY_PREFIX + w, w) for w in dub_words]))
|
||||
|
||||
for family, entries in groups:
|
||||
box = QtWidgets.QGroupBox(f"{family}（{len(entries)}）")
|
||||
self.group_boxes[family] = box
|
||||
grid = QtWidgets.QGridLayout(box)
|
||||
for i, (key, text) in enumerate(entries):
|
||||
cb = QtWidgets.QCheckBox(text)
|
||||
cb.setEnabled(False) # 未计算前禁用
|
||||
cb.toggled.connect(self._on_check_toggled)
|
||||
self.checks[key] = cb
|
||||
grid.addWidget(cb, i // 2, i % 2)
|
||||
vbox.addWidget(box)
|
||||
# Dubins 族默认隐藏，勾上开关后才出现
|
||||
self.dubins_box = self.group_boxes[dub_family]
|
||||
self.dubins_box.setVisible(False)
|
||||
vbox.addStretch()
|
||||
scroll.setWidget(container)
|
||||
return scroll
|
||||
|
||||
# ---- 交互逻辑 ----
|
||||
def _begin_pick(self, target):
|
||||
self._picking_target = target
|
||||
name = "起点" if target == "start" else "终点"
|
||||
self.status.setText(f"点选{name}：先点位置，移动鼠标转箭头，再点一下固定朝向")
|
||||
self.canvas.start_picking("green" if target == "start" else "red")
|
||||
|
||||
def _on_pose_picked(self, x, y, theta):
|
||||
if self._picking_target == "start":
|
||||
self.start = (x, y, theta)
|
||||
elif self._picking_target == "goal":
|
||||
self.goal = (x, y, theta)
|
||||
self._picking_target = None
|
||||
self._recompute()
|
||||
|
||||
def _update_radius_label(self):
|
||||
self.radius_label.setText(f"转弯半径：{int(self.turning_radius)}")
|
||||
|
||||
def _on_radius_changed(self, value):
|
||||
"""滑块改变转弯半径，重算路径；保持当前显示模式（不动 radio）。"""
|
||||
self.turning_radius = float(value)
|
||||
self._update_radius_label()
|
||||
self._recompute(reset_mode=False)
|
||||
|
||||
def _recompute(self, reset_mode=True):
|
||||
"""重算全部路径。
|
||||
|
||||
reset_mode=True：默认回到「仅显示最短」；
|
||||
reset_mode=False：保持当前显示模式（供半径滑块使用）。
|
||||
"""
|
||||
if self.start is None or self.goal is None:
|
||||
self.canvas.render(self.start, self.goal, [])
|
||||
return
|
||||
|
||||
want_dubins = self.chk_dubins.isChecked()
|
||||
self.word_map = rs.compute_word_map(
|
||||
self.start, self.goal, turning_radius=self.turning_radius,
|
||||
include_dubins=want_dubins)
|
||||
|
||||
# RS 与 Dubins 分别选高亮，互不干扰
|
||||
rs_keys = [k for k, c in self.word_map.items() if c.kind == "RS"]
|
||||
dub_keys = [k for k, c in self.word_map.items() if c.kind == "Dubins"]
|
||||
|
||||
# RS 高亮：取「倒车里程最短」的那条。倒车里程 = gear<0 段累计长度；
|
||||
# 纯前进路径为 0（即最小）。相同时再按总长度取短。
|
||||
self.shortest_word = None
|
||||
self.shortest_is_reverse = False
|
||||
|
||||
def reverse_len(cand):
|
||||
return sum(s.length for s in cand.segments if s.gear < 0)
|
||||
|
||||
if rs_keys:
|
||||
self.shortest_word = min(
|
||||
rs_keys,
|
||||
key=lambda w: (reverse_len(self.word_map[w]),
|
||||
self.word_map[w].length))
|
||||
self.shortest_is_reverse = (
|
||||
reverse_len(self.word_map[self.shortest_word]) > 0)
|
||||
|
||||
# Dubins 高亮：纯前进里最短的那条
|
||||
self.dubins_best = (min(dub_keys, key=lambda k: self.word_map[k].length)
|
||||
if dub_keys else None)
|
||||
|
||||
has = bool(self.word_map)
|
||||
|
||||
# 决定本次默认勾选模式：
|
||||
# - 保持模式时跟随当前 radio；
|
||||
# - reset 时回到「仅勾最短」（倒挡最短，无倒挡时为全局最短）。
|
||||
keep_all = self.radio_all.isChecked() if not reset_mode else False
|
||||
|
||||
# 更新勾选框：可达的启用，不可达的置灰
|
||||
for key, cb in self.checks.items():
|
||||
cb.blockSignals(True)
|
||||
reachable = key in self.word_map
|
||||
cb.setEnabled(reachable)
|
||||
word = self._key_word(key)
|
||||
if reachable:
|
||||
cand = self.word_map[key]
|
||||
# 绕远路径（含 > pi 的弧）加 ↻ 标记并注明最长弧
|
||||
mark = f" ↻{cand.max_arc:.2f}" if cand.is_detour else ""
|
||||
cb.setText(f"{word} ({cand.length:.2f}){mark}")
|
||||
else:
|
||||
cb.setText(word)
|
||||
# 默认勾选：全选模式勾所有可达，否则只勾两条高亮
|
||||
cb.setChecked(reachable and (keep_all or key == self.shortest_word
|
||||
or key == self.dubins_best))
|
||||
if key == self.shortest_word:
|
||||
cb.setStyleSheet(f"color: {RED_TEXT}; font-weight: bold;")
|
||||
elif key == self.dubins_best:
|
||||
cb.setStyleSheet(f"color: {PURPLE_TEXT}; font-weight: bold;")
|
||||
else:
|
||||
cb.setStyleSheet("")
|
||||
cb.blockSignals(False)
|
||||
|
||||
# 显示模式：reset_mode 时回到「仅显示最短」，否则保持当前选择
|
||||
self.radio_shortest.blockSignals(True)
|
||||
self.radio_all.blockSignals(True)
|
||||
self.radio_shortest.setEnabled(has)
|
||||
self.radio_all.setEnabled(has)
|
||||
if reset_mode and has:
|
||||
self.radio_shortest.setChecked(True)
|
||||
self.radio_shortest.blockSignals(False)
|
||||
self.radio_all.blockSignals(False)
|
||||
|
||||
n_rs = len(rs_keys)
|
||||
if not has:
|
||||
self.status.setText("无可达路径")
|
||||
else:
|
||||
kind = "倒挡最短" if self.shortest_is_reverse else "总路径最短"
|
||||
msg = (f"RS 可达 {n_rs}/48。{kind}：{self.shortest_word} "
|
||||
f"(长度 {self.word_map[self.shortest_word].length:.2f})")
|
||||
if self.dubins_best:
|
||||
bd = self.word_map[self.dubins_best]
|
||||
n_det = sum(1 for k in dub_keys if self.word_map[k].is_detour)
|
||||
extra = (f" 最长弧 {bd.max_arc:.2f} rad > π，RS 会丢弃"
|
||||
if bd.is_detour else "")
|
||||
msg += (f"\nDubins 可达 {len(dub_keys)}/6（绕远 {n_det} 条）。"
|
||||
f"最短：{bd.word} (长度 {bd.length:.2f}){extra}")
|
||||
self.status.setText(msg)
|
||||
self._redraw_paths()
|
||||
|
||||
@staticmethod
|
||||
def _key_word(key):
|
||||
"""去掉 Dubins 键前缀，得到显示用的 word。"""
|
||||
return (key[len(rs.DUBINS_KEY_PREFIX):]
|
||||
if key.startswith(rs.DUBINS_KEY_PREFIX) else key)
|
||||
|
||||
def _on_dubins_toggled(self, checked):
|
||||
"""开关 Dubins：显示/隐藏该分组并重算（保持当前显示模式）。"""
|
||||
self.dubins_box.setVisible(checked)
|
||||
if not checked:
|
||||
# 关掉时清掉该组勾选，免得残留在图上
|
||||
for key, cb in self.checks.items():
|
||||
if key.startswith(rs.DUBINS_KEY_PREFIX):
|
||||
cb.blockSignals(True)
|
||||
cb.setChecked(False)
|
||||
cb.blockSignals(False)
|
||||
self._recompute(reset_mode=False)
|
||||
|
||||
def _on_check_toggled(self, _checked):
|
||||
self._redraw_paths()
|
||||
|
||||
def _on_mode_changed(self, _checked):
|
||||
"""显示模式切换：全选所有可达 / 仅勾最短，批量应用到勾选框。"""
|
||||
# toggled 会对两个 radio 各触发一次，只在切到「最短」时处理一次即可
|
||||
select_all = self.radio_all.isChecked()
|
||||
for key, cb in self.checks.items():
|
||||
if key not in self.word_map:
|
||||
continue
|
||||
cb.blockSignals(True)
|
||||
cb.setChecked(select_all or key == self.shortest_word
|
||||
or key == self.dubins_best)
|
||||
cb.blockSignals(False)
|
||||
self._redraw_paths()
|
||||
|
||||
def _redraw_paths(self):
|
||||
"""按当前勾选状态重画曲线。"""
|
||||
paths = []
|
||||
for key, cb in self.checks.items():
|
||||
if cb.isChecked() and key in self.word_map:
|
||||
cand = self.word_map[key]
|
||||
segs = rs.sample_path_segments(
|
||||
cand, self.start, turning_radius=self.turning_radius)
|
||||
is_dubins = cand.kind == "Dubins"
|
||||
highlight = (key == self.dubins_best if is_dubins
|
||||
else key == self.shortest_word)
|
||||
paths.append((self._key_word(key), segs, highlight,
|
||||
is_dubins, cand.is_detour))
|
||||
self.canvas.render(self.start, self.goal, paths)
|
||||
|
||||
|
||||
def main():
|
||||
app = QtWidgets.QApplication(sys.argv)
|
||||
win = MainWindow()
|
||||
win.show()
|
||||
sys.exit(app.exec())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user