551 lines
23 KiB
Python
551 lines
23 KiB
Python
"""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()
|