"""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()