chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -54,3 +54,6 @@ appsettings.Development.json
|
||||
# OS files
|
||||
Thumbs.db
|
||||
.DS_Store
|
||||
|
||||
#Unfinished Traplanner
|
||||
TrajPlanner
|
||||
@@ -0,0 +1 @@
|
||||
52380
|
||||
@@ -0,0 +1 @@
|
||||
c619697a72212b481e509ebd1c9023d987d76f98e8d335c1a428bd2f2e641248
|
||||
@@ -0,0 +1,63 @@
|
||||
<style>
|
||||
.layouts { display:grid; grid-template-columns:repeat(3,minmax(300px,1fr)); gap:16px; margin-top:18px; }
|
||||
.layout-card { border:1px solid #d6dde7; border-radius:12px; background:#fff; overflow:hidden; cursor:pointer; }
|
||||
.layout-card:hover { border-color:#2b6fbf; box-shadow:0 8px 24px rgba(32,73,120,.12); }
|
||||
.layout-card.selected { outline:3px solid rgba(43,111,191,.22); border-color:#2b6fbf; }
|
||||
.card-head { padding:13px 14px 8px; }
|
||||
.card-head p { margin:5px 0 0; color:#5d6875; font-size:13px; min-height:40px; }
|
||||
.tag { display:inline-block; padding:2px 8px; border-radius:999px; background:#eaf3fc; color:#0f5da8; font-size:11px; }
|
||||
.screen { margin:8px 12px 14px; height:330px; border:1px solid #aeb8c5; background:#f7f9fb; display:grid; gap:5px; padding:6px; font:10px/1.2 sans-serif; color:#35404c; }
|
||||
.bar { background:#fff; border:1px solid #d9e0e8; padding:5px; display:flex; justify-content:space-between; }
|
||||
.map { position:relative; background:linear-gradient(#edf1f5 1px,transparent 1px),linear-gradient(90deg,#edf1f5 1px,transparent 1px),#fff; background-size:18px 18px; border:1px solid #d7dee7; overflow:hidden; }
|
||||
.map:before { content:""; position:absolute; left:8%; right:8%; top:55%; height:4px; background:#adb8c4; transform:rotate(-7deg); opacity:.45; }
|
||||
.map:after { content:"当前完整轨迹"; position:absolute; left:14%; right:12%; top:50%; height:3px; color:#1769aa; background:#1769aa; transform:rotate(-7deg); box-shadow:0 -8px 0 rgba(102,164,219,.35); }
|
||||
.chart { position:relative; background:linear-gradient(#edf1f5 1px,transparent 1px),linear-gradient(90deg,#edf1f5 1px,transparent 1px),#fff; background-size:28px 28px; border:1px solid #d7dee7; padding:6px; }
|
||||
.chart:after { content:""; position:absolute; left:12%; right:8%; top:58%; height:3px; background:#1769aa; transform:skewY(-10deg); }
|
||||
.status { background:#fff; border:1px solid #d7dee7; padding:7px; display:grid; gap:4px; }
|
||||
.chip { background:#f1f5f9; border-left:3px solid #1769aa; padding:4px; }
|
||||
.tools { position:absolute; right:5px; top:5px; background:#fff; border:1px solid #b8c2ce; padding:3px 5px; z-index:2; }
|
||||
.tabs { background:#fff; border:1px solid #d7dee7; padding:5px; word-spacing:7px; }
|
||||
.a { grid-template-columns:2fr 1fr; grid-template-rows:30px 1fr 110px 26px; }
|
||||
.a .bar { grid-column:1/3; }.a .map { grid-row:2/3; }.a .status { grid-row:2/3; }.a .chart { grid-column:1/3; }.a .tabs{grid-column:1/3;}
|
||||
.b { grid-template-columns:3fr 2fr; grid-template-rows:30px 1fr 26px; }
|
||||
.b .bar{grid-column:1/3}.b .map{grid-row:2/3}.b .right{display:grid;grid-template-rows:1fr 1fr;gap:5px}.b .tabs{grid-column:1/3}
|
||||
.c { grid-template-columns:1fr 1fr 1fr; grid-template-rows:30px 130px 1fr 26px; }
|
||||
.c .bar{grid-column:1/4}.c .map{grid-column:1/4}.c .tabs{grid-column:1/4}
|
||||
@media(max-width:1050px){.layouts{grid-template-columns:1fr}.screen{height:360px}}
|
||||
</style>
|
||||
|
||||
<h2>网页主界面采用哪种观察布局？</h2>
|
||||
<p class="subtitle">所有方案都修复页签串页、增加 X/Y 数值刻度与单位、图例、图层显隐、框选缩放、滚轮缩放、复位和全屏查看。</p>
|
||||
|
||||
<div class="layouts">
|
||||
<div class="layout-card" data-choice="a-map-status" onclick="toggleSelect(this)">
|
||||
<div class="card-head"><h3>A · 地图主视图</h3><p>路径图最大，状态在右侧；图表作为底部工作区。适合先看全局几何。</p></div>
|
||||
<div class="screen a">
|
||||
<div class="bar"><b>FullDirectionSegment · OBSERVE_ONLY</b><span>图层 ▣ 重置视图</span></div>
|
||||
<div class="map"><span class="tools">＋ − ⛶ 图层</span></div>
|
||||
<div class="status"><div class="chip">终点误差 0.8 cm / 1.2°</div><div class="chip">v=0 · a=0</div><div class="chip">制动起点 S=6.4 m</div></div>
|
||||
<div class="chart"><span class="tools">框选放大 复位 全屏</span>ST：t (s) / PathS (m)</div>
|
||||
<div class="tabs">路径总览 LS/ST 运动学 诊断与配置</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="layout-card" data-choice="b-balanced" onclick="toggleSelect(this)">
|
||||
<div class="card-head"><span class="tag">推荐</span><h3>B · 地图与分析并列</h3><p>左侧完整路径，右侧同时看当前选中的 ST/速度图与关键状态，减少来回切页。</p></div>
|
||||
<div class="screen b">
|
||||
<div class="bar"><b>方向段 0 · Forward · 完整规划成功</b><span>导出快照 图层</span></div>
|
||||
<div class="map"><span class="tools">＋ − ⛶</span></div>
|
||||
<div class="right"><div class="chart"><span class="tools">⛶</span>ST：完整起步—巡航—停车</div><div class="status"><div class="chip">巡航目标 1.0 m/s</div><div class="chip">终点 3 cm / 5° 内</div><div class="chip">无零进度异常</div></div></div>
|
||||
<div class="tabs">路径总览 LS/ST 曲率 速度/加速度/jerk 周期诊断</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="layout-card" data-choice="c-chart-wall" onclick="toggleSelect(this)">
|
||||
<div class="card-head"><h3>C · 多图分析墙</h3><p>地图在上方，LS/ST/速度三图并排；适合桌面大屏同时比较数据。</p></div>
|
||||
<div class="screen c">
|
||||
<div class="bar"><b>完整方向段分析</b><span>同步缩放 复位全部</span></div>
|
||||
<div class="map"><span class="tools">＋ − ⛶ 图层</span></div>
|
||||
<div class="chart"><span class="tools">⛶</span>LS</div><div class="chart"><span class="tools">⛶</span>ST</div><div class="chart"><span class="tools">⛶</span>速度</div>
|
||||
<div class="tabs">基础图组 运动学图组 终点验证 配置</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1,56 @@
|
||||
<style>
|
||||
.profile-grid { display:grid; grid-template-columns:repeat(3,minmax(260px,1fr)); gap:16px; margin-top:18px; }
|
||||
.profile-card { border:1px solid #d7dee8; border-radius:12px; padding:16px; background:#fff; cursor:pointer; }
|
||||
.profile-card:hover { border-color:#3678c8; box-shadow:0 8px 24px rgba(40,80,130,.10); }
|
||||
.profile-card.selected { outline:3px solid rgba(43,111,191,.24); border-color:#2b6fbf; }
|
||||
.recommended { display:inline-block; color:#0f5da8; background:#eaf3fc; border-radius:999px; padding:3px 8px; font-size:12px; }
|
||||
.mini { width:100%; height:170px; margin:10px 0 4px; }
|
||||
.axis { stroke:#7d8794; stroke-width:1; }
|
||||
.gridline { stroke:#e5eaf0; stroke-width:1; }
|
||||
.speed { fill:none; stroke:#1769aa; stroke-width:4; }
|
||||
.stopline { stroke:#cf5b39; stroke-width:2; stroke-dasharray:5 4; }
|
||||
.phase { font:12px sans-serif; fill:#4f5965; }
|
||||
.caption { font-size:13px; color:#56616d; min-height:58px; }
|
||||
.formula { margin-top:18px; padding:14px 16px; background:#f6f8fb; border-left:4px solid #1769aa; font-family:ui-monospace,Consolas,monospace; }
|
||||
@media(max-width:900px){.profile-grid{grid-template-columns:1fr}}
|
||||
</style>
|
||||
|
||||
<h2>每个方向段应如何规划“起步—巡航—停车”？</h2>
|
||||
<p class="subtitle">三种实现方式都保持 OBSERVE_ONLY。点击选择；推荐 C，它既表达完整方向段，又保留实时修正能力。</p>
|
||||
|
||||
<div class="profile-grid">
|
||||
<div class="profile-card" data-choice="a-fixed" onclick="toggleSelect(this)">
|
||||
<h3>A · 固定距离开始减速</h3>
|
||||
<svg class="mini" viewBox="0 0 320 170">
|
||||
<path class="gridline" d="M35 30H305M35 75H305M35 120H305"/><path class="axis" d="M35 20V140H310"/>
|
||||
<path class="speed" d="M35 135 C55 90 75 55 110 48 L230 48 Q250 48 265 135 L305 135"/>
|
||||
<path class="stopline" d="M250 20V140"/><text class="phase" x="45" y="158">起步</text><text class="phase" x="145" y="158">巡航</text><text class="phase" x="250" y="158">固定点制动</text>
|
||||
</svg>
|
||||
<p class="caption">实现简单，但速度、载荷或当前加速度变化后，固定制动点可能过早或来不及停车。</p>
|
||||
</div>
|
||||
|
||||
<div class="profile-card" data-choice="b-one-shot" onclick="toggleSelect(this)">
|
||||
<h3>B · 整段一次性联合优化</h3>
|
||||
<svg class="mini" viewBox="0 0 320 170">
|
||||
<path class="gridline" d="M35 30H305M35 75H305M35 120H305"/><path class="axis" d="M35 20V140H310"/>
|
||||
<path class="speed" d="M35 135 C60 120 70 58 120 48 C180 36 220 50 245 78 C270 108 280 135 305 135"/>
|
||||
<text class="phase" x="45" y="158">整段时间与速度共同求解</text>
|
||||
</svg>
|
||||
<p class="caption">完整性最好，但长方向段会产生很大的优化问题；现场参数变化时，整段重新求解可能很慢。</p>
|
||||
</div>
|
||||
|
||||
<div class="profile-card" data-choice="c-hybrid" onclick="toggleSelect(this)">
|
||||
<span class="recommended">推荐</span>
|
||||
<h3>C · 全局速度包络 + 滚动精化</h3>
|
||||
<svg class="mini" viewBox="0 0 320 170">
|
||||
<path class="gridline" d="M35 30H305M35 75H305M35 120H305"/><path class="axis" d="M35 20V140H310"/>
|
||||
<path class="speed" d="M35 135 C55 90 82 48 125 48 L205 48 C232 48 245 63 258 88 C272 114 282 135 305 135"/>
|
||||
<path class="stopline" d="M225 20V140"/><text class="phase" x="43" y="158">jerk 起步</text><text class="phase" x="135" y="158">受限巡航</text><text class="phase" x="225" y="158">动态停车包络</text>
|
||||
</svg>
|
||||
<p class="caption">先为完整方向段计算速度上限与停车包络，再按短周期重算当前轨迹；能主动起步，也能随实时状态调整制动点。</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="formula">
|
||||
开始减速条件：剩余距离 ≤ jerk 受限停车距离(当前 v, 当前 a, 最大减速度, 最大 jerk) + 重规划安全余量
|
||||
</div>
|
||||
@@ -0,0 +1,6 @@
|
||||
<div style="display:flex;align-items:center;justify-content:center;min-height:60vh">
|
||||
<div style="text-align:center;max-width:720px">
|
||||
<h2>正在确认完整方向段规划语义</h2>
|
||||
<p class="subtitle">主产物：一次优化得到完整当前方向段的 LS 与 ST；网页显示完整轨迹，而不是短窗口片段。</p>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1,2 @@
|
||||
{"type":"click","text":"推荐\n C · 全局速度包络 + 滚动精化\n \n \n \n jerk 起步受限巡航动态停车包络\n \n 先为完整方向段计算速度上限与停车包络，再按短周期重算当前轨迹；能主动起步，也能随实时状态调整制动点。","choice":"c-hybrid","id":null,"timestamp":1786007937984}
|
||||
{"type":"click","text":"推荐\n C · 全局速度包络 + 滚动精化\n \n \n \n jerk 起步受限巡航动态停车包络\n \n 先为完整方向段计算速度上限与停车包络，再按短周期重算当前轨迹；能主动起步，也能随实时状态调整制动点。","choice":"c-hybrid","id":null,"timestamp":1786007938998}
|
||||
@@ -0,0 +1 @@
|
||||
{"type":"server-started","port":52380,"host":"127.0.0.1","url_host":"localhost","url":"http://localhost:52380/?key=c619697a72212b481e509ebd1c9023d987d76f98e8d335c1a428bd2f2e641248","screen_dir":"D:\\Users\\Desktop\\项目\\prakrobot\\ParkingRobot\\.superpowers\\brainstorm\\1212-1786007808\\content","state_dir":"D:\\Users\\Desktop\\项目\\prakrobot\\ParkingRobot\\.superpowers\\brainstorm\\1212-1786007808\\state","idle_timeout_ms":14400000}
|
||||
@@ -0,0 +1 @@
|
||||
823e5463adedce4ebfd4c6a032a5fb950de021e730acb9aa
|
||||
@@ -0,0 +1 @@
|
||||
1224
|
||||
@@ -0,0 +1 @@
|
||||
{"type":"server-started","port":52380,"host":"127.0.0.1","url_host":"localhost","url":"http://localhost:52380/?key=c619697a72212b481e509ebd1c9023d987d76f98e8d335c1a428bd2f2e641248","screen_dir":"D:\\Users\\Desktop\\项目\\prakrobot\\ParkingRobot\\.superpowers\\brainstorm\\1515-1785381825\\content","state_dir":"D:\\Users\\Desktop\\项目\\prakrobot\\ParkingRobot\\.superpowers\\brainstorm\\1515-1785381825\\state","idle_timeout_ms":14400000}
|
||||
@@ -0,0 +1 @@
|
||||
ee4120676fb587be5e6f7996276b30b3e88f37f9781d30a6
|
||||
@@ -0,0 +1 @@
|
||||
1527
|
||||
@@ -0,0 +1,37 @@
|
||||
<h2>六张点图报告：布局草图</h2>
|
||||
<p class="subtitle">所有轨迹均以完整采样点绘制，不使用任何连线。每张俯瞰图按路径包围盒加小边距取景，并保持 X/Y 等比例。</p>
|
||||
|
||||
<div style="display:grid;grid-template-columns:repeat(3,minmax(220px,1fr));gap:16px;margin-top:20px">
|
||||
<div class="mockup"><div class="mockup-header">1. 原始粗路径 · 俯瞰</div><div class="mockup-body">
|
||||
<svg viewBox="0 0 240 160" width="100%" aria-label="原始粗路径点图"><rect x="28" y="12" width="190" height="120" fill="#fff" stroke="#222"/><rect x="108" y="62" width="36" height="45" fill="#d9d9d9" stroke="#777"/>
|
||||
<g fill="#4D4D4D"><circle cx="43" cy="113" r="2"/><circle cx="50" cy="108" r="2"/><circle cx="57" cy="102" r="2"/><circle cx="64" cy="95" r="2"/><circle cx="71" cy="86" r="2"/><circle cx="78" cy="78" r="2"/><circle cx="85" cy="72" r="2"/><circle cx="93" cy="68" r="2"/><circle cx="101" cy="67" r="2"/><circle cx="151" cy="61" r="2"/><circle cx="160" cy="65" r="2"/><circle cx="169" cy="74" r="2"/><circle cx="178" cy="84" r="2"/><circle cx="188" cy="91" r="2"/><circle cx="199" cy="96" r="2"/></g>
|
||||
<circle cx="43" cy="113" r="4" fill="#F0E442" stroke="#111"/><path d="M199 90 l6 6 l-6 6 l-6-6z" fill="#CC79A7" stroke="#111"/><text x="113" y="152" font-size="9">X (m)</text><text x="7" y="78" font-size="9" transform="rotate(-90 7 78)">Y (m)</text></svg>
|
||||
<p>保留障碍物、起终点与灰色粗路径点。</p></div></div>
|
||||
|
||||
<div class="mockup"><div class="mockup-header">2. 四方法 · 点集对比</div><div class="mockup-body">
|
||||
<svg viewBox="0 0 240 160" width="100%" aria-label="四种方法点集对比"><rect x="28" y="12" width="190" height="120" fill="#fff" stroke="#222"/>
|
||||
<g fill="#4D4D4D"><circle cx="43" cy="113" r="1.4"/><circle cx="54" cy="103" r="1.4"/><circle cx="65" cy="91" r="1.4"/><circle cx="76" cy="78" r="1.4"/><circle cx="88" cy="70" r="1.4"/><circle cx="100" cy="67" r="1.4"/><circle cx="155" cy="61" r="1.4"/><circle cx="171" cy="76" r="1.4"/><circle cx="188" cy="91" r="1.4"/><circle cx="199" cy="96" r="1.4"/></g>
|
||||
<g fill="#0072B2"><circle cx="43" cy="112" r="1.8"/><circle cx="55" cy="99" r="1.8"/><circle cx="67" cy="84" r="1.8"/><circle cx="80" cy="73" r="1.8"/><circle cx="94" cy="68" r="1.8"/><circle cx="154" cy="63" r="1.8"/><circle cx="171" cy="78" r="1.8"/><circle cx="188" cy="91" r="1.8"/></g>
|
||||
<g fill="#D55E00"><circle cx="43" cy="114" r="1.8"/><circle cx="56" cy="104" r="1.8"/><circle cx="69" cy="89" r="1.8"/><circle cx="82" cy="75" r="1.8"/><circle cx="96" cy="68" r="1.8"/><circle cx="155" cy="61" r="1.8"/><circle cx="173" cy="77" r="1.8"/><circle cx="190" cy="92" r="1.8"/></g>
|
||||
<g fill="#009E73"><circle cx="43" cy="111" r="1.8"/><circle cx="55" cy="98" r="1.8"/><circle cx="68" cy="83" r="1.8"/><circle cx="81" cy="72" r="1.8"/><circle cx="95" cy="67" r="1.8"/><circle cx="156" cy="62" r="1.8"/><circle cx="172" cy="77" r="1.8"/><circle cx="189" cy="91" r="1.8"/></g>
|
||||
<text x="35" y="145" font-size="8" fill="#4D4D4D">● 粗路径</text><text x="87" y="145" font-size="8" fill="#0072B2">● B样条</text><text x="139" y="145" font-size="8" fill="#D55E00">● Bézier</text><text x="195" y="145" font-size="8" fill="#009E73">● 五次</text></svg>
|
||||
<p>仅四组路径点、坐标轴和图例；无障碍物、无起终点。</p></div></div>
|
||||
|
||||
<div class="mockup"><div class="mockup-header">3. B 样条 · 俯瞰</div><div class="mockup-body">
|
||||
<svg viewBox="0 0 240 160" width="100%"><rect x="28" y="12" width="190" height="120" fill="#fff" stroke="#222"/><rect x="108" y="62" width="36" height="45" fill="#d9d9d9" stroke="#777"/><g fill="#4D4D4D" opacity=".35"><circle cx="43" cy="113" r="1.2"/><circle cx="58" cy="100" r="1.2"/><circle cx="73" cy="83" r="1.2"/><circle cx="88" cy="70" r="1.2"/><circle cx="101" cy="67" r="1.2"/><circle cx="155" cy="61" r="1.2"/><circle cx="177" cy="84" r="1.2"/><circle cx="199" cy="96" r="1.2"/></g><g fill="#0072B2"><circle cx="43" cy="112" r="2"/><circle cx="54" cy="100" r="2"/><circle cx="66" cy="85" r="2"/><circle cx="80" cy="73" r="2"/><circle cx="94" cy="68" r="2"/><circle cx="154" cy="63" r="2"/><circle cx="171" cy="78" r="2"/><circle cx="188" cy="91" r="2"/></g><text x="113" y="152" font-size="9">X (m)</text><text x="7" y="78" font-size="9" transform="rotate(-90 7 78)">Y (m)</text></svg>
|
||||
<p>灰色粗路径点作参照；蓝色为全部 B 样条输出点。</p></div></div>
|
||||
|
||||
<div class="mockup"><div class="mockup-header">4. Bézier · 俯瞰</div><div class="mockup-body">
|
||||
<svg viewBox="0 0 240 160" width="100%"><rect x="28" y="12" width="190" height="120" fill="#fff" stroke="#222"/><rect x="108" y="62" width="36" height="45" fill="#d9d9d9" stroke="#777"/><g fill="#4D4D4D" opacity=".35"><circle cx="43" cy="113" r="1.2"/><circle cx="58" cy="100" r="1.2"/><circle cx="73" cy="83" r="1.2"/><circle cx="88" cy="70" r="1.2"/><circle cx="101" cy="67" r="1.2"/><circle cx="155" cy="61" r="1.2"/><circle cx="177" cy="84" r="1.2"/><circle cx="199" cy="96" r="1.2"/></g><g fill="#D55E00"><circle cx="43" cy="114" r="2"/><circle cx="56" cy="104" r="2"/><circle cx="69" cy="89" r="2"/><circle cx="82" cy="75" r="2"/><circle cx="96" cy="68" r="2"/><circle cx="155" cy="61" r="2"/><circle cx="173" cy="77" r="2"/><circle cx="190" cy="92" r="2"/></g><text x="113" y="152" font-size="9">X (m)</text><text x="7" y="78" font-size="9" transform="rotate(-90 7 78)">Y (m)</text></svg>
|
||||
<p>灰色粗路径点作参照；橙色为全部 Bézier 输出点。</p></div></div>
|
||||
|
||||
<div class="mockup"><div class="mockup-header">5. 五次 · 俯瞰</div><div class="mockup-body">
|
||||
<svg viewBox="0 0 240 160" width="100%"><rect x="28" y="12" width="190" height="120" fill="#fff" stroke="#222"/><rect x="108" y="62" width="36" height="45" fill="#d9d9d9" stroke="#777"/><g fill="#4D4D4D" opacity=".35"><circle cx="43" cy="113" r="1.2"/><circle cx="58" cy="100" r="1.2"/><circle cx="73" cy="83" r="1.2"/><circle cx="88" cy="70" r="1.2"/><circle cx="101" cy="67" r="1.2"/><circle cx="155" cy="61" r="1.2"/><circle cx="177" cy="84" r="1.2"/><circle cx="199" cy="96" r="1.2"/></g><g fill="#009E73"><circle cx="43" cy="111" r="2"/><circle cx="55" cy="98" r="2"/><circle cx="68" cy="83" r="2"/><circle cx="81" cy="72" r="2"/><circle cx="95" cy="67" r="2"/><circle cx="156" cy="62" r="2"/><circle cx="172" cy="77" r="2"/><circle cx="189" cy="91" r="2"/></g><text x="113" y="152" font-size="9">X (m)</text><text x="7" y="78" font-size="9" transform="rotate(-90 7 78)">Y (m)</text></svg>
|
||||
<p>灰色粗路径点作参照；绿色为全部五次输出点。</p></div></div>
|
||||
|
||||
<div class="mockup"><div class="mockup-header">6. 曲率 · 全方法点图</div><div class="mockup-body">
|
||||
<svg viewBox="0 0 240 160" width="100%"><rect x="34" y="12" width="184" height="120" fill="#fff" stroke="#222"/><line x1="34" y1="72" x2="218" y2="72" stroke="#222"/><g fill="#4D4D4D"><circle cx="47" cy="72" r="1.4"/><circle cx="61" cy="47" r="1.4"/><circle cx="75" cy="47" r="1.4"/><circle cx="89" cy="72" r="1.4"/><circle cx="103" cy="97" r="1.4"/><circle cx="117" cy="72" r="1.4"/><circle cx="131" cy="72" r="1.4"/><circle cx="145" cy="47" r="1.4"/><circle cx="159" cy="72" r="1.4"/><circle cx="173" cy="97" r="1.4"/><circle cx="187" cy="72" r="1.4"/></g><g fill="#0072B2"><circle cx="47" cy="72" r="1.8"/><circle cx="61" cy="54" r="1.8"/><circle cx="75" cy="51" r="1.8"/><circle cx="89" cy="70" r="1.8"/><circle cx="103" cy="90" r="1.8"/><circle cx="117" cy="73" r="1.8"/><circle cx="131" cy="68" r="1.8"/><circle cx="145" cy="52" r="1.8"/><circle cx="159" cy="71" r="1.8"/><circle cx="173" cy="91" r="1.8"/><circle cx="187" cy="71" r="1.8"/></g><text x="114" y="152" font-size="9">s (m)</text><text x="9" y="82" font-size="9" transform="rotate(-90 9 82)">κ (m⁻¹)</text><text x="42" y="145" font-size="8" fill="#4D4D4D">● 粗路径</text><text x="97" y="145" font-size="8" fill="#0072B2">● B样条</text><text x="148" y="145" font-size="8" fill="#D55E00">● Bézier</text><text x="199" y="145" font-size="8" fill="#009E73">● 五次</text></svg>
|
||||
<p>四组曲率采样点、统一坐标轴和图例；不绘制曲线。</p></div></div>
|
||||
</div>
|
||||
|
||||
<div class="section" style="margin-top:20px"><p><strong>请确认：</strong>这正是将被导出的六张独立 PNG + SVG 图的内容划分。看完后直接回复“确认”或指出需要调整的那一张。</p></div>
|
||||
@@ -0,0 +1,3 @@
|
||||
<div style="display:flex;align-items:center;justify-content:center;min-height:60vh">
|
||||
<p class="subtitle">布局已确认，继续在终端中准备实现。</p>
|
||||
</div>
|
||||
@@ -0,0 +1 @@
|
||||
fe7e537ea1c8795202b14034148de7c503a50e4319e76d42
|
||||
@@ -0,0 +1 @@
|
||||
{"reason":"idle timeout","timestamp":1785396457111}
|
||||
@@ -0,0 +1 @@
|
||||
1540
|
||||
@@ -0,0 +1,73 @@
|
||||
<h2>实时轨迹规划网页看板：布局选择</h2>
|
||||
<p class="subtitle">三种方案都保持 OBSERVE_ONLY。蓝线表示当前规划，灰线表示上一轮规划；红点专门标记滚动交接处。</p>
|
||||
|
||||
<style>
|
||||
.dash { background:#0d1725; color:#d9e8f5; border-radius:10px; padding:9px; min-height:245px; font-size:9px; }
|
||||
.bar { display:flex; justify-content:space-between; align-items:center; background:#15263a; padding:5px 7px; border-radius:6px; margin-bottom:6px; }
|
||||
.ok { color:#72e6a6; } .warn { color:#ffcb6b; } .bad { color:#ff7070; }
|
||||
.grid2 { display:grid; grid-template-columns:1.45fr 1fr; gap:6px; }
|
||||
.grid3 { display:grid; grid-template-columns:repeat(3,1fr); gap:5px; }
|
||||
.panel { background:#132235; border:1px solid #29405a; border-radius:5px; padding:4px; min-height:48px; }
|
||||
.panel.big { min-height:132px; }
|
||||
.tabs { display:flex; gap:4px; margin:5px 0; }
|
||||
.tab { padding:3px 6px; background:#243b55; border-radius:4px; }
|
||||
.tab.active { background:#168aad; color:white; }
|
||||
.plot { position:relative; height:36px; border-left:1px solid #68839c; border-bottom:1px solid #68839c; margin:5px 3px 1px 9px; overflow:hidden; }
|
||||
.plot.tall { height:105px; }
|
||||
.line { position:absolute; left:2%; right:2%; height:2px; background:#3bc9db; top:48%; transform:rotate(-7deg); transform-origin:left; box-shadow:32px -7px 0 #3bc9db,64px 1px 0 #3bc9db; }
|
||||
.previous { background:#8494a4; top:58%; opacity:.55; }
|
||||
.handoff { position:absolute; left:64%; top:39%; width:6px; height:6px; background:#ff7070; border-radius:50%; box-shadow:0 0 0 2px rgba(255,112,112,.25); }
|
||||
.metrics { display:grid; grid-template-columns:repeat(3,1fr); gap:4px; margin-top:5px; }
|
||||
.metric { background:#1a3047; border-radius:4px; padding:4px; text-align:center; }
|
||||
.focus-list { display:grid; grid-template-columns:1fr 1fr; gap:4px; margin-top:6px; }
|
||||
.spark { height:18px; background:linear-gradient(165deg,transparent 42%,#3bc9db 44%,#3bc9db 48%,transparent 50%); border-bottom:1px solid #526b81; }
|
||||
</style>
|
||||
|
||||
<div class="cards">
|
||||
<div class="card" data-choice="a" onclick="toggleSelect(this)">
|
||||
<div class="card-image">
|
||||
<div class="dash">
|
||||
<div class="bar"><b>A · 总览 + 分析页签</b><span class="ok">● cycle 128 · Rolling</span></div>
|
||||
<div class="grid2">
|
||||
<div class="panel big"><b>世界路径 / 当前与上一轮</b><div class="plot tall"><div class="line previous"></div><div class="line"></div><div class="handoff"></div></div></div>
|
||||
<div>
|
||||
<div class="panel"><b>关键状态</b><div class="metrics"><span class="metric">v<br>0.18</span><span class="metric">a<br>0.04</span><span class="metric">j<br>-0.12</span></div></div>
|
||||
<div class="panel" style="margin-top:6px"><b>交接检查</b><br><span class="ok">速度连续 ✓</span><br><span class="ok">非零滚动终点 ✓</span><br><span class="ok">jerk 合规 ✓</span></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="tabs"><span class="tab active">LS / ST</span><span class="tab">曲率</span><span class="tab">v-a-j</span><span class="tab">历史</span></div>
|
||||
<div class="grid3"><div class="panel">L-S<div class="spark"></div></div><div class="panel">T-S<div class="spark"></div></div><div class="panel">T-V<div class="spark"></div></div></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card-body"><h3>A · 分层混合（推荐）</h3><p>首屏保留路径、状态和异常；曲线按页签切换。信息完整，浏览器与车辆端的内存/重绘压力较平衡。</p></div>
|
||||
</div>
|
||||
|
||||
<div class="card" data-choice="b" onclick="toggleSelect(this)">
|
||||
<div class="card-image">
|
||||
<div class="dash">
|
||||
<div class="bar"><b>B · 六图同时显示</b><span class="ok">● LIVE 20 Hz</span></div>
|
||||
<div class="grid3">
|
||||
<div class="panel">世界路径<div class="plot"><div class="line"></div></div></div><div class="panel">L-S<div class="spark"></div></div><div class="panel">T-S<div class="spark"></div></div>
|
||||
<div class="panel">曲率-S<div class="spark"></div></div><div class="panel">速度-T<div class="spark"></div></div><div class="panel">加速度-T<div class="spark"></div></div>
|
||||
<div class="panel">加加速度-T<div class="spark"></div></div><div class="panel">规划耗时<div class="spark"></div></div><div class="panel">交接误差<div class="spark"></div></div>
|
||||
</div>
|
||||
<div class="metrics"><span class="metric">cycle 128</span><span class="metric">OSQP 42 ms</span><span class="metric ok">0 violations</span></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card-body"><h3>B · 密集工程屏</h3><p>一次看到所有图，适合大显示器持续监控；小屏可读性较差，浏览器渲染负载最高。</p></div>
|
||||
</div>
|
||||
|
||||
<div class="card" data-choice="c" onclick="toggleSelect(this)">
|
||||
<div class="card-image">
|
||||
<div class="dash">
|
||||
<div class="bar"><b>C · 单图聚焦</b><span class="warn">Rolling handoff #128</span></div>
|
||||
<div class="tabs"><span class="tab">路径</span><span class="tab">LS</span><span class="tab">ST</span><span class="tab active">Jerk-T</span></div>
|
||||
<div class="panel big"><b>加加速度 / 限值带</b><div class="plot tall"><div class="line"></div><div class="handoff"></div></div></div>
|
||||
<div class="focus-list"><div class="panel">当前轮<br><span class="ok">max |j| 0.31</span></div><div class="panel">交接点<br><span class="ok">Δa 0.02</span></div><div class="panel">上一轮末端<br>v 0.17</div><div class="panel">下一轮首点<br>v 0.18</div></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card-body"><h3>C · 单图诊断</h3><p>一次聚焦一条曲线，最轻量、适合笔记本；跨图对照需要切换，实时总览能力较弱。</p></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<p class="subtitle">请点击一个布局。我的建议是 A：默认总览轻量，发生异常时再切到曲率或 v-a-j 细节。</p>
|
||||
@@ -0,0 +1,70 @@
|
||||
<h2>科研绘图风 · A 布局精修</h2>
|
||||
<p class="subtitle">以论文图表为基准：细线、白底、单位完整、图例克制；红色只表示真正越界。</p>
|
||||
|
||||
<style>
|
||||
.scientific { background:#fff; color:#20252b; border:1px solid #cfd4da; border-radius:3px; padding:12px; font-family:Arial,"Microsoft YaHei",sans-serif; }
|
||||
.s-head { display:flex; justify-content:space-between; align-items:flex-end; border-bottom:1px solid #60666d; padding-bottom:6px; margin-bottom:10px; }
|
||||
.s-title { font-family:"Times New Roman","SimSun",serif; font-size:15px; font-weight:600; }
|
||||
.s-meta { color:#59636e; font-size:9px; }
|
||||
.s-grid { display:grid; grid-template-columns:1.55fr .85fr; gap:10px; }
|
||||
.s-panel { border:1px solid #b8bec5; padding:7px; background:#fff; }
|
||||
.s-panel-title { font-family:"Times New Roman","SimSun",serif; font-size:10px; font-weight:600; display:flex; justify-content:space-between; }
|
||||
.s-plot { width:100%; height:132px; display:block; margin-top:3px; }
|
||||
.s-side { display:flex; flex-direction:column; gap:8px; }
|
||||
.s-table { width:100%; border-collapse:collapse; font-size:9px; }
|
||||
.s-table td { padding:4px 3px; border-bottom:1px solid #e2e5e8; }
|
||||
.s-table td:last-child { text-align:right; font-family:Consolas,monospace; }
|
||||
.pass { color:#237a57; }.note { color:#b86514; }.fail { color:#b42318; }
|
||||
.s-tabs { display:flex; gap:0; margin-top:10px; border-bottom:1px solid #777e86; }
|
||||
.s-tab { padding:5px 11px 4px; border:1px solid transparent; font-size:9px; color:#545d66; }
|
||||
.s-tab.active { color:#174c80; border:1px solid #777e86; border-bottom:1px solid white; margin-bottom:-1px; background:#fff; }
|
||||
.s-multipanel { display:grid; grid-template-columns:repeat(3,1fr); gap:9px; padding-top:9px; }
|
||||
.s-small { border:1px solid #c8cdd2; padding:5px; }
|
||||
.s-small svg { width:100%; height:72px; display:block; }
|
||||
.caption { color:#56606a; font-size:9px; margin-top:8px; line-height:1.45; }
|
||||
.limit { stroke:#b42318; stroke-width:.7; stroke-dasharray:3 3; fill:none; }
|
||||
.gridline { stroke:#d9dde1; stroke-width:.55; }.axis { stroke:#343a40; stroke-width:.8; }.tick { fill:#555d65; font:5px Arial; }
|
||||
.current { stroke:#1769aa; stroke-width:1.15; fill:none; }.previous { stroke:#8d959d; stroke-width:.8; stroke-dasharray:4 2; fill:none; }
|
||||
.reference { stroke:#333; stroke-width:.75; fill:none; }.handoff { fill:#d87918; stroke:#fff; stroke-width:.7; }
|
||||
</style>
|
||||
|
||||
<div class="mockup">
|
||||
<div class="mockup-header">Preview: Scientific trajectory dashboard</div>
|
||||
<div class="mockup-body">
|
||||
<div class="scientific">
|
||||
<div class="s-head">
|
||||
<div><div class="s-title">EM Trajectory Planning Observation</div><div class="s-meta">OBSERVE_ONLY · local session · coordinate units: m / s / rad</div></div>
|
||||
<div class="s-meta"><span class="pass">● LIVE</span> Cycle 128 RollingContinuation 42 ms</div>
|
||||
</div>
|
||||
<div class="s-grid">
|
||||
<div class="s-panel">
|
||||
<div class="s-panel-title"><span>(a) World path and rolling handoff</span><span style="font:8px Arial"><span style="color:#1769aa">━ current</span> <span style="color:#8d959d">┄ previous</span> <span style="color:#d87918">● handoff</span></span></div>
|
||||
<svg class="s-plot" viewBox="0 0 360 132" preserveAspectRatio="none">
|
||||
<g><line class="gridline" x1="38" y1="12" x2="38" y2="112"/><line class="gridline" x1="101" y1="12" x2="101" y2="112"/><line class="gridline" x1="164" y1="12" x2="164" y2="112"/><line class="gridline" x1="227" y1="12" x2="227" y2="112"/><line class="gridline" x1="290" y1="12" x2="290" y2="112"/><line class="gridline" x1="38" y1="32" x2="344" y2="32"/><line class="gridline" x1="38" y1="58" x2="344" y2="58"/><line class="gridline" x1="38" y1="84" x2="344" y2="84"/></g>
|
||||
<line class="axis" x1="38" y1="112" x2="344" y2="112"/><line class="axis" x1="38" y1="12" x2="38" y2="112"/>
|
||||
<path class="reference" d="M42,92 C92,88 116,70 151,67 S226,66 268,41 S322,25 341,22"/>
|
||||
<path class="previous" d="M43,95 C91,90 116,73 152,69 S226,68 270,44 S323,29 339,27"/>
|
||||
<path class="current" d="M43,93 C92,89 116,71 152,68 S226,67 269,42 S323,26 341,23"/>
|
||||
<circle class="handoff" cx="269" cy="42" r="3.2"/>
|
||||
<text class="tick" x="174" y="128">World X (m)</text><text class="tick" transform="rotate(-90 8 69)" x="8" y="69">World Y (m)</text>
|
||||
</svg>
|
||||
</div>
|
||||
<div class="s-side">
|
||||
<div class="s-panel"><div class="s-panel-title">Current sample</div><table class="s-table"><tr><td>t</td><td>0.35 s</td></tr><tr><td>v</td><td>0.182 m/s</td></tr><tr><td>a</td><td>0.041 m/s²</td></tr><tr><td>j</td><td>−0.118 m/s³</td></tr><tr><td>κ</td><td>0.224 m⁻¹</td></tr></table></div>
|
||||
<div class="s-panel"><div class="s-panel-title">Rolling continuity</div><table class="s-table"><tr><td>terminal speed ≠ 0</td><td class="pass">PASS</td></tr><tr><td>|Δv| ≤ tol</td><td class="pass">0.003</td></tr><tr><td>|Δa| ≤ tol</td><td class="pass">0.018</td></tr><tr><td>|j| ≤ limit</td><td class="pass">PASS</td></tr><tr><td>trajectory age</td><td class="note">61 ms</td></tr></table></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="s-tabs"><span class="s-tab active">LS / ST</span><span class="s-tab">Curvature</span><span class="s-tab">Velocity / Acceleration / Jerk</span><span class="s-tab">Cycle history</span></div>
|
||||
<div class="s-multipanel">
|
||||
<div class="s-small"><div class="s-panel-title">(b) Lateral offset</div><svg viewBox="0 0 120 72" preserveAspectRatio="none"><line class="axis" x1="18" y1="60" x2="115" y2="60"/><line class="axis" x1="18" y1="8" x2="18" y2="60"/><line class="gridline" x1="18" y1="34" x2="115" y2="34"/><path class="current" d="M18,34 C38,25 51,30 65,35 S92,39 115,33"/><text class="tick" x="52" y="70">s (m)</text><text class="tick" x="1" y="9">l (m)</text></svg></div>
|
||||
<div class="s-small"><div class="s-panel-title">(c) Longitudinal progress</div><svg viewBox="0 0 120 72" preserveAspectRatio="none"><line class="axis" x1="18" y1="60" x2="115" y2="60"/><line class="axis" x1="18" y1="8" x2="18" y2="60"/><path class="previous" d="M18,58 L42,52 L66,41 L90,27 L115,13"/><path class="current" d="M18,58 L42,51 L66,39 L90,25 L115,11"/><circle class="handoff" cx="90" cy="25" r="2.5"/><text class="tick" x="52" y="70">t (s)</text><text class="tick" x="2" y="9">s (m)</text></svg></div>
|
||||
<div class="s-small"><div class="s-panel-title">(d) Signed velocity</div><svg viewBox="0 0 120 72" preserveAspectRatio="none"><line class="axis" x1="18" y1="60" x2="115" y2="60"/><line class="axis" x1="18" y1="8" x2="18" y2="60"/><line class="limit" x1="18" y1="14" x2="115" y2="14"/><path class="previous" d="M18,51 C36,43 48,30 67,26 S96,27 115,24"/><path class="current" d="M18,49 C36,41 48,29 67,25 S96,26 115,23"/><circle class="handoff" cx="67" cy="25" r="2.5"/><text class="tick" x="52" y="70">t (s)</text><text class="tick" x="1" y="9">v (m/s)</text></svg></div>
|
||||
</div>
|
||||
<div class="caption">Refresh: 10 Hz · latest-snapshot transport · history: 60 cycles (bounded) · browser clients: 1 · no planner backpressure</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="options">
|
||||
<div class="option" data-choice="approve-scientific" onclick="toggleSelect(this)"><div class="letter">✓</div><div class="content"><h3>采用这个科研风格</h3><p>后续页面都沿用细线、白底、论文式多子图与单位标注。</p></div></div>
|
||||
</div>
|
||||
@@ -0,0 +1,74 @@
|
||||
<h2>A 布局：选择视觉风格</h2>
|
||||
<p class="subtitle">图表颜色始终语义一致：当前轨迹为蓝色、上一轮为灰色、交接点为橙色、越界才使用红色。</p>
|
||||
|
||||
<style>
|
||||
.preview { border-radius:10px; padding:10px; min-height:245px; font-size:9px; }
|
||||
.top { display:flex; justify-content:space-between; align-items:center; padding:6px 8px; border-radius:6px; margin-bottom:7px; }
|
||||
.layout { display:grid; grid-template-columns:1.45fr 1fr; gap:7px; }
|
||||
.box { border-radius:6px; padding:6px; }
|
||||
.big { min-height:125px; }
|
||||
.chart { height:100px; margin:7px 3px 0 12px; border-left:1px solid; border-bottom:1px solid; position:relative; overflow:hidden; }
|
||||
.path-old,.path-new { position:absolute; height:3px; left:4%; width:82%; top:57%; transform:rotate(-7deg); transform-origin:left; border-radius:2px; }
|
||||
.path-new { top:49%; }
|
||||
.dot { position:absolute; left:65%; top:38%; width:8px; height:8px; border-radius:50%; }
|
||||
.stats { display:grid; grid-template-columns:repeat(3,1fr); gap:4px; margin:5px 0 7px; }
|
||||
.stat { border-radius:4px; padding:5px 2px; text-align:center; }
|
||||
.checks { line-height:1.8; }
|
||||
.tabs { display:flex; gap:5px; margin-top:7px; }
|
||||
.pill { border-radius:10px; padding:4px 8px; }
|
||||
.mini { display:grid; grid-template-columns:repeat(3,1fr); gap:5px; margin-top:6px; }
|
||||
.mini > div { border-radius:5px; padding:5px; height:33px; }
|
||||
|
||||
.light { background:#f7f9fc; color:#27364a; border:1px solid #dce3ec; }
|
||||
.light .top { background:#ffffff; border:1px solid #e1e7ee; }
|
||||
.light .box,.light .mini>div { background:#fff; border:1px solid #dfe5ec; }
|
||||
.light .chart { border-color:#9baabd; background:linear-gradient(#f2f5f8 1px,transparent 1px),linear-gradient(90deg,#f2f5f8 1px,transparent 1px); background-size:22px 22px; }
|
||||
.light .path-old { background:#a6b0bc; }.light .path-new { background:#1668dc; }.light .dot { background:#f08c2e; }
|
||||
.light .stat { background:#eef3f8; }.light .pill { background:#e8edf3; }.light .pill.on { background:#1668dc;color:#fff; }
|
||||
|
||||
.ink { background:#fbfaf6; color:#272521; border:1px solid #d5d0c5; }
|
||||
.ink .top { background:#efede6; border-bottom:2px solid #34312d; }
|
||||
.ink .box,.ink .mini>div { background:#fffef9; border:1px solid #cfc9bd; }
|
||||
.ink .chart { border-color:#4d4a45; background:linear-gradient(#ece8df 1px,transparent 1px),linear-gradient(90deg,#ece8df 1px,transparent 1px); background-size:22px 22px; }
|
||||
.ink .path-old { background:#9c978d; }.ink .path-new { background:#126782; }.ink .dot { background:#d97706; }
|
||||
.ink .stat { background:#f0ede6; }.ink .pill { background:#e8e4dc; }.ink .pill.on { background:#126782;color:#fff; }
|
||||
|
||||
.night { background:#101419; color:#d8dee8; border:1px solid #303944; }
|
||||
.night .top { background:#191f27; border-bottom:1px solid #3b4653; }
|
||||
.night .box,.night .mini>div { background:#171c23; border:1px solid #343e49; }
|
||||
.night .chart { border-color:#657382; background:linear-gradient(#222a33 1px,transparent 1px),linear-gradient(90deg,#222a33 1px,transparent 1px); background-size:22px 22px; }
|
||||
.night .path-old { background:#6e7883; }.night .path-new { background:#58a6ff; }.night .dot { background:#f2a65a; }
|
||||
.night .stat { background:#222a33; }.night .pill { background:#252e38; }.night .pill.on { background:#2f81f7;color:#fff; }
|
||||
.good { color:#16815d; }.night .good { color:#56d49b; }
|
||||
</style>
|
||||
|
||||
<div class="cards">
|
||||
<div class="card" data-choice="light" onclick="toggleSelect(this)">
|
||||
<div class="card-image"><div class="preview light">
|
||||
<div class="top"><b>轨迹规划观测台</b><span class="good">● 实时 · Cycle 128</span></div>
|
||||
<div class="layout"><div class="box big"><b>世界路径</b><div class="chart"><div class="path-old"></div><div class="path-new"></div><div class="dot"></div></div></div>
|
||||
<div><div class="box"><b>当前状态</b><div class="stats"><div class="stat">v<br>0.18</div><div class="stat">a<br>0.04</div><div class="stat">j<br>-0.12</div></div></div><div class="box checks" style="margin-top:7px"><b>交接检查</b><br><span class="good">✓ 非零滚动终点</span><br><span class="good">✓ 速度/加速度连续</span><br><span class="good">✓ Jerk 合规</span></div></div></div>
|
||||
<div class="tabs"><span class="pill on">LS / ST</span><span class="pill">曲率</span><span class="pill">v-a-j</span><span class="pill">历史</span></div><div class="mini"><div>L-S</div><div>T-S</div><div>T-V</div></div>
|
||||
</div></div><div class="card-body"><h3>1 · 清晰实验室（推荐）</h3><p>浅色、低饱和、高可读性，适合长时间观察、截图和报告。</p></div>
|
||||
</div>
|
||||
|
||||
<div class="card" data-choice="paper" onclick="toggleSelect(this)">
|
||||
<div class="card-image"><div class="preview ink">
|
||||
<div class="top"><b>TRAJECTORY OBSERVATION</b><span class="good">LIVE / 128</span></div>
|
||||
<div class="layout"><div class="box big"><b>WORLD PATH</b><div class="chart"><div class="path-old"></div><div class="path-new"></div><div class="dot"></div></div></div>
|
||||
<div><div class="box"><b>STATE</b><div class="stats"><div class="stat">v<br>0.18</div><div class="stat">a<br>0.04</div><div class="stat">j<br>-0.12</div></div></div><div class="box checks" style="margin-top:7px"><b>HANDOFF</b><br><span class="good">PASS / rolling</span><br><span class="good">PASS / continuity</span><br><span class="good">PASS / jerk</span></div></div></div>
|
||||
<div class="tabs"><span class="pill on">LS / ST</span><span class="pill">CURVATURE</span><span class="pill">V-A-J</span><span class="pill">HISTORY</span></div><div class="mini"><div>L-S</div><div>T-S</div><div>T-V</div></div>
|
||||
</div></div><div class="card-body"><h3>2 · 工程图纸</h3><p>米白与墨色，接近论文/仪器输出，克制但稍显传统。</p></div>
|
||||
</div>
|
||||
|
||||
<div class="card" data-choice="night" onclick="toggleSelect(this)">
|
||||
<div class="card-image"><div class="preview night">
|
||||
<div class="top"><b>轨迹规划观测台</b><span class="good">● 实时 · Cycle 128</span></div>
|
||||
<div class="layout"><div class="box big"><b>世界路径</b><div class="chart"><div class="path-old"></div><div class="path-new"></div><div class="dot"></div></div></div>
|
||||
<div><div class="box"><b>当前状态</b><div class="stats"><div class="stat">v<br>0.18</div><div class="stat">a<br>0.04</div><div class="stat">j<br>-0.12</div></div></div><div class="box checks" style="margin-top:7px"><b>交接检查</b><br><span class="good">✓ 非零滚动终点</span><br><span class="good">✓ 速度/加速度连续</span><br><span class="good">✓ Jerk 合规</span></div></div></div>
|
||||
<div class="tabs"><span class="pill on">LS / ST</span><span class="pill">曲率</span><span class="pill">v-a-j</span><span class="pill">历史</span></div><div class="mini"><div>L-S</div><div>T-S</div><div>T-V</div></div>
|
||||
</div></div><div class="card-body"><h3>3 · 石墨夜间</h3><p>中性深灰、低炫光；比上一版更克制，适合暗光车间。</p></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<p class="subtitle">请点击一种风格；也可以回复“以 1 为基础，但……”。</p>
|
||||
@@ -0,0 +1,6 @@
|
||||
<div style="display:flex;align-items:center;justify-content:center;min-height:60vh;text-align:center">
|
||||
<div>
|
||||
<h2>科研风格已确认</h2>
|
||||
<p class="subtitle">界面说明使用中文；坐标轴、单位和数学符号保留英文规范。继续在终端讨论数据服务架构。</p>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1 @@
|
||||
{"reason":"stop-server.sh","timestamp":1785940779}
|
||||
@@ -0,0 +1,54 @@
|
||||
<h2>不同算法应该怎样被可视化？</h2>
|
||||
<p class="subtitle">请选择生成策略。重点考虑：既要自动化，也要让不同算法的关键机制真正容易理解。</p>
|
||||
|
||||
<div class="cards">
|
||||
<div class="card" data-choice="adaptive-visuals" onclick="toggleSelect(this)">
|
||||
<div class="card-image">
|
||||
<div class="label">A · 推荐：自适应图形族</div>
|
||||
<div style="padding:12px;display:grid;grid-template-columns:repeat(2,minmax(0,1fr));gap:9px;background:#f6faf9">
|
||||
<div style="padding:9px;background:white;border-top:3px solid #2b8872"><b>数据处理函数</b><br><small>输入 → 变换 → 校验 → 输出</small></div>
|
||||
<div style="padding:9px;background:white;border-top:3px solid #497fa3"><b>搜索/规划算法</b><br><small>起点 ⇢ 扩展 ⇢ 代价 ⇢ 目标</small></div>
|
||||
<div style="padding:9px;background:white;border-top:3px solid #9a68a8"><b>状态机</b><br><small>Idle ⇄ Running → Error</small></div>
|
||||
<div style="padding:9px;background:white;border-top:3px solid #d18a26"><b>数值/几何算法</b><br><small>曲线、约束区间、异常峰值</small></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card-body">
|
||||
<h3>根据算法特征自动选择主视图</h3>
|
||||
<p>所有类型共享同一诊断叠加协议，但主图可以是流程、搜索路径、状态转移或数值曲线。表达力最强，同时保留统一点击体验。</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="card" data-choice="universal-flowchart" onclick="toggleSelect(this)">
|
||||
<div class="card-image">
|
||||
<div class="label">B · 通用流程图</div>
|
||||
<div style="padding:28px 12px;display:flex;align-items:center;justify-content:center;gap:7px;background:#faf8f4;flex-wrap:wrap">
|
||||
<span style="padding:9px;border:1px solid #888;border-radius:6px;background:white">输入</span><b>→</b>
|
||||
<span style="padding:9px;border:1px solid #888;border-radius:6px;background:white">步骤 A</span><b>→</b>
|
||||
<span style="padding:9px;border:1px solid #c8463a;border-radius:6px;background:#fff0ee">步骤 B ⚠</span><b>→</b>
|
||||
<span style="padding:9px;border:1px solid #888;border-radius:6px;background:white">输出</span>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card-body">
|
||||
<h3>所有函数统一画成节点流程</h3>
|
||||
<p>生成稳定、实现简单、读法一致；但数值曲线、状态跳转和搜索空间等算法特征会被压扁，可能仍停留在“文字方框”。</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="card" data-choice="manual-visual-spec" onclick="toggleSelect(this)">
|
||||
<div class="card-image">
|
||||
<div class="label">C · 人工指定视图</div>
|
||||
<div style="padding:16px;background:#f7f8fb">
|
||||
<div class="mock-nav">日报输入中指定：visual_type = curve</div>
|
||||
<div class="placeholder" style="height:115px;margin-top:8px">自定义节点、坐标、连线和交互说明</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card-body">
|
||||
<h3>由 Agent 或用户提供专用图形描述</h3>
|
||||
<p>定制能力最高，但生成成本大、输入要求高，也更依赖当次 Agent 是否完整理解算法，难以保证每份日报都有图。</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="section">
|
||||
<p><strong>建议 A：</strong>使用“统一诊断协议 + 自适应主视图”。不管图形类型如何变化，点击节点后都固定展示源码、当前状态、原因、后果、修正方案和预期结果。</p>
|
||||
</div>
|
||||
@@ -0,0 +1,89 @@
|
||||
<h2>修订方向：展示算法的真实领域效果</h2>
|
||||
<p class="subtitle">下面以泊车路径规划为例。主画面直接呈现规划场景与算法结果，流程和文字只负责解释“为什么”。</p>
|
||||
|
||||
<div class="mockup">
|
||||
<div class="mockup-header">路径规划诊断 · Parking Scenario #08</div>
|
||||
<div class="mockup-body" style="padding:0">
|
||||
<div class="mock-nav" style="display:flex;gap:8px;flex-wrap:wrap">
|
||||
<span>正常机制</span>
|
||||
<strong style="color:#b7352c">● 当前问题</strong>
|
||||
<span>○ 候选修正预演</span>
|
||||
<span>✓ 已验证结果</span>
|
||||
<span style="margin-left:auto">播放搜索 逐步执行 重置视角</span>
|
||||
</div>
|
||||
|
||||
<div style="display:grid;grid-template-columns:minmax(0,1.8fr) minmax(250px,1fr);gap:0">
|
||||
<div style="padding:12px;background:#edf1f2;border-right:1px solid #cbd5d8">
|
||||
<svg viewBox="0 0 560 310" role="img" aria-label="泊车路径规划实际效果示意" style="width:100%;height:auto;background:#2c3438;border-radius:8px">
|
||||
<defs>
|
||||
<pattern id="grid" width="28" height="28" patternUnits="userSpaceOnUse"><path d="M 28 0 L 0 0 0 28" fill="none" stroke="#3b464b" stroke-width="1"/></pattern>
|
||||
<marker id="arrow-red" markerWidth="8" markerHeight="8" refX="6" refY="3" orient="auto"><path d="M0,0 L0,6 L7,3 z" fill="#ff7065"/></marker>
|
||||
<marker id="arrow-green" markerWidth="8" markerHeight="8" refX="6" refY="3" orient="auto"><path d="M0,0 L0,6 L7,3 z" fill="#5ed5a5"/></marker>
|
||||
</defs>
|
||||
<rect width="560" height="310" fill="url(#grid)"/>
|
||||
<g stroke="#839096" stroke-width="2" fill="none">
|
||||
<path d="M360 18V110H520V18"/><path d="M400 18V110"/><path d="M440 18V110"/><path d="M480 18V110"/>
|
||||
</g>
|
||||
<g fill="#77848a" stroke="#aeb8bc">
|
||||
<rect x="365" y="28" width="30" height="68" rx="5"/><rect x="445" y="28" width="30" height="68" rx="5"/>
|
||||
</g>
|
||||
<rect x="405" y="24" width="30" height="76" rx="5" fill="rgba(94,213,165,.12)" stroke="#5ed5a5" stroke-dasharray="5 4"/>
|
||||
<text x="420" y="119" fill="#b9f3da" font-size="11" text-anchor="middle">目标车位</text>
|
||||
|
||||
<g opacity=".38" fill="#9eb2ba">
|
||||
<circle cx="95" cy="235" r="3"/><circle cx="140" cy="222" r="3"/><circle cx="185" cy="206" r="3"/><circle cx="230" cy="183" r="3"/><circle cx="275" cy="154" r="3"/><circle cx="320" cy="125" r="3"/><circle cx="355" cy="92" r="3"/>
|
||||
</g>
|
||||
|
||||
<g transform="translate(72 220) rotate(-3)"><rect x="-28" y="-15" width="56" height="30" rx="7" fill="#4e9ec5" stroke="#bce8fa" stroke-width="2"/><path d="M10 0H32" stroke="#bce8fa" stroke-width="2"/><text x="0" y="-23" fill="#d8f2ff" font-size="11" text-anchor="middle">起始姿态</text></g>
|
||||
<g transform="translate(420 61) rotate(-90)"><rect x="-28" y="-15" width="56" height="30" rx="7" fill="#319470" stroke="#b9f3da" stroke-width="2"/><path d="M10 0H32" stroke="#b9f3da" stroke-width="2"/></g>
|
||||
|
||||
<path d="M72 220 C185 226 278 196 341 138 C376 106 388 85 420 61" fill="none" stroke="#ff7065" stroke-width="5" marker-end="url(#arrow-red)"/>
|
||||
<path d="M72 220 C175 245 295 225 350 170 C397 124 392 88 420 61" fill="none" stroke="#5ed5a5" stroke-width="4" stroke-dasharray="9 7" marker-end="url(#arrow-green)"/>
|
||||
|
||||
<circle cx="347" cy="132" r="17" fill="rgba(255,112,101,.18)" stroke="#ff7065" stroke-width="3"/>
|
||||
<path d="M347 115V149M330 132H364" stroke="#ff7065" stroke-width="2"/>
|
||||
<path d="M350 130 L282 82" stroke="#ffb2aa" stroke-width="1.5"/>
|
||||
<rect x="150" y="48" width="142" height="48" rx="6" fill="#fff7f5" stroke="#ff7065"/>
|
||||
<text x="160" y="67" fill="#8f2f29" font-size="12" font-weight="700">最小间距失效</text>
|
||||
<text x="160" y="85" fill="#5b4542" font-size="11">车体包络进入障碍膨胀区</text>
|
||||
|
||||
<g transform="translate(22 18)">
|
||||
<rect width="185" height="28" rx="5" fill="rgba(20,25,28,.82)"/>
|
||||
<line x1="10" y1="10" x2="35" y2="10" stroke="#ff7065" stroke-width="4"/><text x="42" y="14" fill="#eee" font-size="10">当前轨迹</text>
|
||||
<line x1="102" y1="10" x2="127" y2="10" stroke="#5ed5a5" stroke-width="4" stroke-dasharray="6 4"/><text x="134" y="14" fill="#eee" font-size="10">修正预演</text>
|
||||
</g>
|
||||
</svg>
|
||||
|
||||
<div style="display:grid;grid-template-columns:repeat(5,minmax(0,1fr));gap:6px;margin-top:9px;font-size:12px;text-align:center">
|
||||
<div style="padding:7px;background:white;border-top:3px solid #4289aa">环境建模</div>
|
||||
<div style="padding:7px;background:white;border-top:3px solid #4289aa">状态扩展</div>
|
||||
<div style="padding:7px;background:#fff1ef;border-top:3px solid #c8463a">碰撞检查 ⚠</div>
|
||||
<div style="padding:7px;background:white;border-top:3px solid #d18a26">路径平滑</div>
|
||||
<div style="padding:7px;background:white;border-top:3px solid #777">安全验证</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<aside style="padding:14px;background:white">
|
||||
<div class="label">点击异常位置后的诊断面板</div>
|
||||
<h3 style="margin-top:7px">碰撞检查 · clearance gate</h3>
|
||||
<div style="display:grid;gap:9px;font-size:13px">
|
||||
<div style="padding:8px;border-left:3px solid #c8463a;background:#fff7f5"><b>目前状况</b><br>规划轨迹在倒车转向段进入障碍物膨胀边界。</div>
|
||||
<div style="padding:8px;border-left:3px solid #d18a26;background:#fffaf0"><b>出错原因</b><br>搜索代价采用点模型间距，未覆盖完整车体包络。</div>
|
||||
<div style="padding:8px;border-left:3px solid #9f624e;background:#fff8f3"><b>导致结果</b><br>轨迹数值可达，但实际车辆存在擦碰风险，安全门应拒绝。</div>
|
||||
<div style="padding:8px;border-left:3px solid #3b8d75;background:#f2fbf7"><b>纠正方案</b><br>扩展节点时使用车辆多边形包络，并对障碍物增加安全裕量。</div>
|
||||
<div style="padding:8px;border-left:3px dashed #3b8d75;background:#f7fcfa"><b>预期结果 · 尚未验证</b><br>候选轨迹绕开膨胀区，最小间距满足阈值后再进入平滑阶段。</div>
|
||||
</div>
|
||||
<p style="margin-top:10px;font-size:12px;color:#68777d"><b>证据下钻：</b>点击可查看函数、源码行、输入样本、约束值、测试输出与证据等级。</p>
|
||||
</aside>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="options">
|
||||
<div class="option" data-choice="confirm-domain-visual" onclick="toggleSelect(this)">
|
||||
<div class="letter">✓</div><div class="content"><h3>就是这个方向</h3><p>算法主区域展示真实业务对象和运行结果，诊断信息与源码证据随点击联动。</p></div>
|
||||
</div>
|
||||
<div class="option" data-choice="adjust-domain-visual" onclick="toggleSelect(this)">
|
||||
<div class="letter">↺</div><div class="content"><h3>还需要调整</h3><p>保留领域可视化方向，但需要改变内容层次或交互方式。</p></div>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1,72 @@
|
||||
<h2>交互设计：从算法效果一路追到验证结果</h2>
|
||||
<p class="subtitle">示意的是所有领域共用的操作方式；中央主画面会根据算法自动变成路径、曲线、搜索树、状态机或其他实际效果图。</p>
|
||||
|
||||
<div class="mockup">
|
||||
<div class="mockup-header" style="display:flex;justify-content:space-between;gap:8px;flex-wrap:wrap">
|
||||
<span>算法诊断报告</span><span>算法：路径规划器 ▾ 问题：最小间距失效 ▾</span>
|
||||
</div>
|
||||
<div class="mockup-body" style="padding:0">
|
||||
<div style="display:grid;grid-template-columns:170px minmax(0,1.5fr) minmax(245px,.8fr);min-height:390px">
|
||||
<nav class="mock-sidebar" style="padding:12px">
|
||||
<div class="label">查看模式</div>
|
||||
<div style="display:grid;gap:8px;margin-top:10px">
|
||||
<div style="padding:9px;background:#eef6f8;border-left:4px solid #4b8fa8"><b>① 正常机制</b><br><small>算法原本怎样工作</small></div>
|
||||
<div style="padding:9px;background:#fff1ef;border-left:4px solid #c8463a"><b>② 当前问题</b><br><small>异常位置与实际状态</small></div>
|
||||
<div style="padding:9px;background:#fff9ed;border-left:4px dashed #d18a26"><b>③ 修正预演</b><br><small>方案尚未验证</small></div>
|
||||
<div style="padding:9px;background:#f0faf5;border-left:4px solid #31836b"><b>④ 验证结果</b><br><small>修正后的真实证据</small></div>
|
||||
</div>
|
||||
<div class="label" style="margin-top:18px">显示层</div>
|
||||
<p style="font-size:12px">☑ 输入输出<br>☑ 约束边界<br>☑ 当前异常<br>☑ 影响传播<br>☐ 源码位置</p>
|
||||
</nav>
|
||||
|
||||
<main style="padding:14px;background:#edf1f2">
|
||||
<div style="display:flex;justify-content:space-between;align-items:center;gap:8px;margin-bottom:9px">
|
||||
<b>领域效果主画面</b><small>播放 暂停 单步 缩放 重置</small>
|
||||
</div>
|
||||
<div style="height:238px;position:relative;border-radius:8px;background:linear-gradient(135deg,#334047,#222b30);overflow:hidden;color:white">
|
||||
<div style="position:absolute;inset:18px;border:1px dashed #718087;border-radius:7px"></div>
|
||||
<div style="position:absolute;left:9%;bottom:18%;padding:7px 10px;border-radius:6px;background:#4b97b8">输入/起点</div>
|
||||
<div style="position:absolute;right:10%;top:17%;padding:7px 10px;border-radius:6px;background:#328b70">输出/目标</div>
|
||||
<svg viewBox="0 0 500 220" style="position:absolute;inset:0;width:100%;height:100%">
|
||||
<path d="M80 175 C190 185 245 145 310 105 C350 80 385 63 423 55" fill="none" stroke="#ff7065" stroke-width="6"/>
|
||||
<circle cx="308" cy="106" r="19" fill="rgba(255,112,101,.25)" stroke="#ffb0aa" stroke-width="3"/>
|
||||
<path d="M308 87V125M289 106H327" stroke="#fff" stroke-width="2"/>
|
||||
<path d="M80 175 C170 206 280 188 335 137 C372 103 390 72 423 55" fill="none" stroke="#65d9aa" stroke-width="4" stroke-dasharray="10 7"/>
|
||||
</svg>
|
||||
<div style="position:absolute;left:44%;top:12%;padding:7px;background:#fff4f2;color:#8f2e28;border-radius:5px"><b>点击异常对象</b><br><small>主画面与右侧诊断联动</small></div>
|
||||
</div>
|
||||
<div style="display:grid;grid-template-columns:repeat(4,minmax(0,1fr));gap:6px;margin-top:9px;font-size:11px;text-align:center">
|
||||
<div style="padding:6px;background:white">输入阶段</div><div style="padding:6px;background:white">核心计算</div><div style="padding:6px;background:#fff0ee">异常阶段 ⚠</div><div style="padding:6px;background:white">输出验证</div>
|
||||
</div>
|
||||
</main>
|
||||
|
||||
<aside style="padding:14px;background:white;border-left:1px solid #ccd5d8">
|
||||
<div class="label">对象诊断卡</div>
|
||||
<h3>异常对象 / 阶段名称</h3>
|
||||
<div style="font-size:12px;display:grid;gap:8px">
|
||||
<div><b style="color:#b83b32">目前状况</b><br>显示当前实际值、位置或状态。</div>
|
||||
<div><b>出错原因</b><br>根因或明确标记的待验证假设。</div>
|
||||
<div><b style="color:#a36325">导致结果</b><br>在主图中高亮影响传播路径。</div>
|
||||
<div><b style="color:#26745f">纠正方案</b><br>点击步骤可在主图预演对应变化。</div>
|
||||
<div><b style="color:#26745f">预期结果</b><br>候选预测与真实验证结果分开显示。</div>
|
||||
</div>
|
||||
<hr>
|
||||
<p style="font-size:11px"><b>证据：</b>函数名、源码位置、输入样本、测试命令和证据等级，可继续展开。</p>
|
||||
</aside>
|
||||
</div>
|
||||
|
||||
<div style="padding:12px;border-top:1px solid #ccd5d8;background:#fafbfb">
|
||||
<div class="label">一次完整点击路径</div>
|
||||
<p style="margin:6px 0 0;text-align:center"><b>选择算法</b> → <b>查看正常机制</b> → <b>点击异常对象</b> → <b>播放影响传播</b> → <b>逐步预演纠正方案</b> → <b>切换已验证结果</b></p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="options">
|
||||
<div class="option" data-choice="approve-interaction" onclick="toggleSelect(this)">
|
||||
<div class="letter">✓</div><div class="content"><h3>确认这套交互</h3><p>领域主画面、四种状态、对象诊断卡和证据下钻共同组成报告页面。</p></div>
|
||||
</div>
|
||||
<div class="option" data-choice="revise-interaction" onclick="toggleSelect(this)">
|
||||
<div class="letter">↺</div><div class="content"><h3>需要调整</h3><p>说明希望改变的交互顺序、页面区域或信息层次。</p></div>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1,69 @@
|
||||
<h2>选择算法诊断页的交互结构</h2>
|
||||
<p class="subtitle">三种方案都能展示“正常功能 → 当前问题 → 原因 → 后果 → 纠正方案 → 预期结果”。请重点比较：第一眼是否容易理解，以及点击后能否逐层深入。</p>
|
||||
|
||||
<div class="cards">
|
||||
<div class="card" data-choice="b-layered-diagnostic" onclick="toggleSelect(this)">
|
||||
<div class="card-image">
|
||||
<div class="label">B · 推荐：双层算法地图</div>
|
||||
<div class="mock-nav">算法基线 ｜ 问题叠加 ｜ 修正预演 [当前：问题叠加]</div>
|
||||
<div style="padding:14px;display:grid;gap:12px;background:#f7faf9">
|
||||
<div style="display:flex;align-items:center;gap:7px;justify-content:center;flex-wrap:wrap">
|
||||
<span style="padding:8px 11px;border:1px solid #83b6ad;border-radius:7px;background:white">输入路径</span><b>→</b>
|
||||
<span style="padding:8px 11px;border:2px solid #c8463a;border-radius:7px;background:#fff1ef">曲率计算 ⚠</span><b>→</b>
|
||||
<span style="padding:8px 11px;border:1px solid #83b6ad;border-radius:7px;background:white">平滑约束</span><b>→</b>
|
||||
<span style="padding:8px 11px;border:1px solid #83b6ad;border-radius:7px;background:white">输出轨迹</span>
|
||||
</div>
|
||||
<div style="display:grid;grid-template-columns:repeat(3,minmax(0,1fr));gap:8px">
|
||||
<div style="padding:9px;border-top:3px solid #c8463a;background:white"><b>原因</b><br><small>边界导数不连续</small></div>
|
||||
<div style="padding:9px;border-top:3px solid #d18a26;background:white"><b>影响传播</b><br><small>曲率峰值 → 控制抖动</small></div>
|
||||
<div style="padding:9px;border-top:3px solid #23856d;background:white"><b>纠正预演</b><br><small>新约束 → 预期稳定</small></div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card-body">
|
||||
<h3>基线流程 + 故障/修正叠加层</h3>
|
||||
<p>先固定展示算法正常机制；点击问题后在同一流程上高亮故障节点、原因和影响，再切换到修正方案预演。上下文不丢失，最适合函数与算法诊断。</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="card" data-choice="a-linear-story" onclick="toggleSelect(this)">
|
||||
<div class="card-image">
|
||||
<div class="label">A · 线性故事链</div>
|
||||
<div style="padding:14px;display:grid;gap:7px;background:#faf8f4">
|
||||
<div style="padding:8px;border-left:4px solid #427f9d;background:white"><b>1. 原算法功能</b> 输入如何变成输出</div>
|
||||
<div style="text-align:center">↓ 点击继续</div>
|
||||
<div style="padding:8px;border-left:4px solid #c8463a;background:white"><b>2. 当前问题</b> 异常发生在哪里</div>
|
||||
<div style="text-align:center">↓</div>
|
||||
<div style="padding:8px;border-left:4px solid #d18a26;background:white"><b>3. 原因与后果</b> 为什么错、会导致什么</div>
|
||||
<div style="text-align:center">↓</div>
|
||||
<div style="padding:8px;border-left:4px solid #23856d;background:white"><b>4. 纠正与预期</b> 改什么、预计变成什么</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card-body">
|
||||
<h3>从上到下的诊断故事</h3>
|
||||
<p>阅读顺序最直观，适合汇报展示；但算法复杂或问题较多时页面会变长，比较多个问题时容易失去原流程位置。</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="card" data-choice="c-explorer-dashboard" onclick="toggleSelect(this)">
|
||||
<div class="card-image">
|
||||
<div class="label">C · 自由探索工作台</div>
|
||||
<div style="display:grid;grid-template-columns:110px 1fr 145px;min-height:190px;background:#f7f8fb">
|
||||
<div class="mock-sidebar">函数列表<br><br>calculate()<br><br>smooth()<br><br>validate()</div>
|
||||
<div style="padding:12px">
|
||||
<div class="placeholder" style="height:110px">可缩放算法关系图<br>节点可自由点击</div>
|
||||
<div style="display:flex;gap:6px;margin-top:8px"><button class="mock-button">问题</button><button class="mock-button">方案</button><button class="mock-button">前后对比</button></div>
|
||||
</div>
|
||||
<div style="padding:10px;background:white;border-left:1px solid #ddd"><b>节点详情</b><hr><small>输入<br>源码位置<br>问题<br>根因<br>影响<br>验证</small></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="card-body">
|
||||
<h3>图谱式诊断工作台</h3>
|
||||
<p>探索能力最强，适合大型算法和多个函数；但实现与维护成本最高，日报第一次打开时也更容易让读者迷失。</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="section">
|
||||
<p><strong>我的建议：</strong>选择 B。它保留原算法作为稳定“地图”，问题和解决方案都是同一张地图上的可切换叠加层，最符合你强调的“点击就能理解目前状况和修正后结果”。</p>
|
||||
</div>
|
||||
@@ -0,0 +1,6 @@
|
||||
<div style="display:flex;align-items:center;justify-content:center;min-height:60vh;text-align:center">
|
||||
<div>
|
||||
<h2>领域算法可视化方向已确认</h2>
|
||||
<p class="subtitle">正在终端中比较可复用的实现架构……</p>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1,6 @@
|
||||
<div style="display:flex;align-items:center;justify-content:center;min-height:60vh;text-align:center">
|
||||
<div>
|
||||
<h2>已确定：混合粒度算法图</h2>
|
||||
<p class="subtitle">主图展示算法阶段，节点可下钻至函数与源码。正在终端中确认修正预演的证据状态……</p>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1,6 @@
|
||||
<div style="display:flex;align-items:center;justify-content:center;min-height:60vh;text-align:center">
|
||||
<div>
|
||||
<h2>已选择：B · 双层算法地图</h2>
|
||||
<p class="subtitle">正在终端中确认算法节点的组织粒度……</p>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1,6 @@
|
||||
<div style="display:flex;align-items:center;justify-content:center;min-height:60vh;text-align:center">
|
||||
<div>
|
||||
<h2>交互设计已确认</h2>
|
||||
<p class="subtitle">正在终端中确认降级规则与验收标准……</p>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1 @@
|
||||
{"reason":"stop-server.sh","timestamp":1785737557}
|
||||
@@ -0,0 +1,10 @@
|
||||
<style>
|
||||
.layouts{display:grid;grid-template-columns:repeat(3,minmax(280px,1fr));gap:16px;margin-top:18px}.choice{border:1px solid #d6dde7;border-radius:12px;padding:14px;background:#fff;cursor:pointer}.choice:hover{border-color:#1769aa;box-shadow:0 8px 24px rgba(30,80,130,.12)}.choice.selected{outline:3px solid rgba(23,105,170,.22)}.tag{display:inline-block;background:#e8f2fb;color:#1769aa;border-radius:99px;padding:2px 8px;font-size:11px}.screen{height:330px;margin-top:12px;padding:6px;display:grid;gap:5px;border:1px solid #aeb8c5;background:#f7f9fb;font:10px sans-serif}.bar,.tabs,.status{background:#fff;border:1px solid #d7dee7;padding:6px}.map,.chart{position:relative;border:1px solid #d7dee7;background:linear-gradient(#e9eef3 1px,transparent 1px),linear-gradient(90deg,#e9eef3 1px,transparent 1px),#fff;background-size:20px 20px;overflow:hidden}.map:after{content:'当前完整轨迹';position:absolute;left:12%;right:9%;top:54%;height:4px;color:#1769aa;background:#1769aa;transform:rotate(-7deg)}.chart:after{content:'';position:absolute;left:10%;right:8%;top:58%;height:3px;background:#1769aa;transform:skewY(-9deg)}.tools{position:absolute;right:5px;top:5px;z-index:2;background:#fff;border:1px solid #b9c3ce;padding:3px}.a{grid-template-columns:2fr 1fr;grid-template-rows:30px 1fr 110px 26px}.a .bar,.a .chart,.a .tabs{grid-column:1/3}.b{grid-template-columns:3fr 2fr;grid-template-rows:30px 1fr 26px}.b .bar,.b .tabs{grid-column:1/3}.right{display:grid;grid-template-rows:1fr 1fr;gap:5px}.c{grid-template-columns:repeat(3,1fr);grid-template-rows:30px 130px 1fr 26px}.c .bar,.c .map,.c .tabs{grid-column:1/4}.chip{margin:4px 0;padding:4px;background:#f1f5f9;border-left:3px solid #1769aa}@media(max-width:1000px){.layouts{grid-template-columns:1fr}}
|
||||
</style>
|
||||
<h2>网页主界面布局选择</h2>
|
||||
<p class="subtitle">三种方案都包含独立页签、X/Y 数值刻度与单位、图例、图层显隐、框选/滚轮缩放、复位及单图全屏。</p>
|
||||
<div class="layouts">
|
||||
<div class="choice" data-choice="a-map-first" onclick="toggleSelect(this)"><h3>A · 地图优先</h3><p>大地图、右侧状态、底部单张分析图。</p><div class="screen a"><div class="bar"><b>完整方向段 · OBSERVE_ONLY</b><span>图层 复位</span></div><div class="map"><span class="tools">＋ − ⛶</span></div><div class="status"><div class="chip">终点 3 cm / 5°</div><div class="chip">v=0 · a=0</div><div class="chip">制动点 S=6.4m</div></div><div class="chart"><span class="tools">框选 复位 ⛶</span>完整 ST</div><div class="tabs">路径 LS/ST 运动学 诊断</div></div></div>
|
||||
<div class="choice" data-choice="b-balanced" onclick="toggleSelect(this)"><span class="tag">推荐</span><h3>B · 地图与分析并列</h3><p>左侧完整路径，右侧同时看选中图表和终点状态。</p><div class="screen b"><div class="bar"><b>段 0 · Forward · 完整规划成功</b><span>图层 导出</span></div><div class="map"><span class="tools">＋ − ⛶</span></div><div class="right"><div class="chart"><span class="tools">框选 ⛶</span>完整 ST：起步—巡航—停车</div><div class="status"><div class="chip">目标速度 1.0m/s</div><div class="chip">终点误差合格</div><div class="chip">无零进度异常</div></div></div><div class="tabs">路径 LS/ST 曲率 速度/加速度/jerk 诊断</div></div></div>
|
||||
<div class="choice" data-choice="c-chart-wall" onclick="toggleSelect(this)"><h3>C · 多图分析墙</h3><p>地图在上，LS、ST、速度三图始终并排。</p><div class="screen c"><div class="bar"><b>完整方向段分析</b><span>同步缩放 复位全部</span></div><div class="map"><span class="tools">＋ − ⛶</span></div><div class="chart">LS</div><div class="chart">ST</div><div class="chart">速度</div><div class="tabs">基础图组 运动学 终点验证 配置</div></div></div>
|
||||
</div>
|
||||
@@ -0,0 +1,23 @@
|
||||
<style>
|
||||
.paper{background:#fff;color:#20252b;border:1px solid #c8cdd2;padding:12px;font-family:Arial,"Microsoft YaHei",sans-serif}.head{display:flex;justify-content:space-between;align-items:flex-end;border-bottom:.6px solid #50565c;padding-bottom:6px}.title{font:600 15px "Times New Roman","SimSun",serif}.meta{font-size:9px;color:#626b74}.live{color:#237a57}.topgrid{display:grid;grid-template-columns:1.65fr .75fr;gap:9px;margin-top:9px}.panel{border:.6px solid #b8bec5;padding:6px;background:#fff}.ptitle{display:flex;justify-content:space-between;font:600 10px "Times New Roman","SimSun",serif}.tools{font:8px Arial;color:#59636d;word-spacing:6px}.plot{width:100%;height:160px;display:block}.side{display:grid;gap:7px}.table{width:100%;border-collapse:collapse;font-size:9px}.table td{padding:4px 2px;border-bottom:.5px solid #e1e5e8}.table td:last-child{text-align:right;font-family:Consolas,monospace}.pass{color:#237a57}.warn{color:#b86514}.tabs{display:flex;border-bottom:.6px solid #777e86;margin-top:9px}.tab{padding:5px 10px 4px;font-size:9px;color:#545d66}.tab.active{color:#174c80;border:.6px solid #777e86;border-bottom:.6px solid #fff;margin-bottom:-.6px}.charts{display:grid;grid-template-columns:repeat(3,1fr);gap:8px;padding-top:8px}.chart{border:.6px solid #c8cdd2;padding:5px}.chart svg{width:100%;height:92px;display:block}.caption{margin-top:7px;color:#616a73;font-size:8px}.grid{stroke:#d9dde1;stroke-width:.35}.axis{stroke:#343a40;stroke-width:.55}.tick{fill:#555d65;font:5px Arial}.reference{fill:none;stroke:#4f555b;stroke-width:.5}.coarse{fill:none;stroke:#8d959d;stroke-width:.45;stroke-dasharray:3 2;opacity:.55}.current{fill:none;stroke:#1769aa;stroke-width:.7}.previous{fill:none;stroke:#8d959d;stroke-width:.55;stroke-dasharray:3 2}.limit{fill:none;stroke:#b42318;stroke-width:.45;stroke-dasharray:2 2}.marker{fill:#d87918;stroke:#fff;stroke-width:.4}.brake{stroke:#d87918;stroke-width:.45;stroke-dasharray:2 2}.approve{margin-top:14px}
|
||||
</style>
|
||||
<h2>论文风格 · 细线版</h2>
|
||||
<p class="subtitle">恢复之前的科研绘图语言，并进一步减小所有线宽。蓝色只表示完整规划轨迹，红色只表示真实越界。</p>
|
||||
<div class="mockup"><div class="mockup-header">Preview: Full-direction-segment trajectory observation</div><div class="mockup-body">
|
||||
<div class="paper">
|
||||
<div class="head"><div><div class="title">EM Full-Direction-Segment Trajectory Observation</div><div class="meta">OBSERVE_ONLY · segment 0 / Forward · units: m, s, rad</div></div><div class="meta"><span class="live">● VALID</span> FullDirectionSegment solve 186 ms</div></div>
|
||||
<div class="topgrid">
|
||||
<div class="panel"><div class="ptitle"><span>(a) World path and complete planned trajectory</span><span class="tools">Layers Box zoom Reset Full screen</span></div>
|
||||
<svg class="plot" viewBox="0 0 520 160" preserveAspectRatio="none"><g><path class="grid" d="M42 14V139M115 14V139M188 14V139M261 14V139M334 14V139M407 14V139M480 14V139M42 35H505M42 61H505M42 87H505M42 113H505"/></g><path class="axis" d="M42 14V139H505"/><path class="coarse" d="M48 123C115 116 154 91 209 86S322 82 384 49S465 26 498 22"/><path class="reference" d="M48 121C115 114 154 89 209 84S322 80 384 47S465 24 498 20"/><path class="current" d="M48 122C115 115 154 90 209 85S322 81 384 48S465 25 498 21"/><line class="brake" x1="396" y1="17" x2="396" y2="139"/><circle class="marker" cx="396" cy="43" r="2.4"/><text class="tick" x="260" y="153">World X (m)</text><text class="tick" transform="rotate(-90 9 80)" x="9" y="80">World Y (m)</text><text class="tick" x="399" y="37">brake start</text></svg>
|
||||
</div>
|
||||
<div class="side"><div class="panel"><div class="ptitle">Terminal boundary</div><table class="table"><tr><td>Δ position</td><td class="pass">0.012 m</td></tr><tr><td>Δ yaw</td><td class="pass">1.8°</td></tr><tr><td>v terminal</td><td class="pass">0.000 m/s</td></tr><tr><td>a terminal</td><td class="pass">0.000 m/s²</td></tr><tr><td>yaw rate</td><td class="pass">0.000 rad/s</td></tr></table></div><div class="panel"><div class="ptitle">Profile summary</div><table class="table"><tr><td>segment length</td><td>8.42 m</td></tr><tr><td>planned duration</td><td>12.80 s</td></tr><tr><td>cruise target</td><td>1.00 m/s</td></tr><tr><td>brake start</td><td>6.31 m</td></tr><tr><td>zero progress</td><td class="pass">NO</td></tr></table></div></div>
|
||||
</div>
|
||||
<div class="tabs"><span class="tab active">LS / ST</span><span class="tab">Curvature</span><span class="tab">Velocity / Acceleration / Jerk</span><span class="tab">Terminal validation</span><span class="tab">Configuration</span></div>
|
||||
<div class="charts">
|
||||
<div class="chart"><div class="ptitle"><span>(b) Lateral offset</span><span class="tools">Zoom Reset ⛶</span></div><svg viewBox="0 0 160 92" preserveAspectRatio="none"><path class="grid" d="M23 12V75M65 12V75M107 12V75M149 12V75M23 28H154M23 51H154"/><path class="axis" d="M23 10V75H155"/><path class="current" d="M23 48C48 41 65 44 83 48S123 52 154 46"/><text class="tick" x="75" y="88">ReferenceS (m)</text><text class="tick" x="2" y="10">l (m)</text></svg></div>
|
||||
<div class="chart"><div class="ptitle"><span>(c) Complete ST</span><span class="tools">Zoom Reset ⛶</span></div><svg viewBox="0 0 160 92" preserveAspectRatio="none"><path class="grid" d="M23 12V75M65 12V75M107 12V75M149 12V75M23 28H154M23 51H154"/><path class="axis" d="M23 10V75H155"/><path class="current" d="M23 74C40 72 52 66 66 56L108 25C122 15 135 11 154 11"/><text class="tick" x="82" y="88">t (s)</text><text class="tick" x="2" y="10">PathS (m)</text></svg></div>
|
||||
<div class="chart"><div class="ptitle"><span>(d) Signed velocity</span><span class="tools">Zoom Reset ⛶</span></div><svg viewBox="0 0 160 92" preserveAspectRatio="none"><path class="grid" d="M23 12V75M65 12V75M107 12V75M149 12V75M23 28H154M23 51H154"/><path class="axis" d="M23 10V75H155"/><path class="limit" d="M23 14H155"/><path class="current" d="M23 74C33 58 43 28 62 20H105C121 21 134 51 154 74"/><text class="tick" x="82" y="88">t (s)</text><text class="tick" x="2" y="10">v (m/s)</text></svg></div>
|
||||
</div>
|
||||
<div class="caption">Thin scientific rendering · current trajectory 0.70 px · reference 0.50 px · grid 0.35 px · all axes include numeric ticks and units · per-plot zoom does not alter source data</div>
|
||||
</div></div></div>
|
||||
<div class="options approve"><div class="option" data-choice="approve-paper-thin" onclick="toggleSelect(this)"><div class="letter">✓</div><div class="content"><h3>采用论文风格细线版</h3><p>后续页面沿用这套排版、线宽、颜色和交互密度。</p></div></div></div>
|
||||
@@ -0,0 +1,6 @@
|
||||
<div style="display:flex;align-items:center;justify-content:center;min-height:60vh">
|
||||
<div style="max-width:760px;text-align:center">
|
||||
<h2>保留原页面与论文风格</h2>
|
||||
<p class="subtitle">当前设计已收紧为：修复空白、页签、数据、刻度和图层缺陷；补充 s_end、换向点、车辆位置与路径对比，不重做布局。</p>
|
||||
</div>
|
||||
</div>
|
||||
@@ -0,0 +1 @@
|
||||
df42eac72602791b7f917cb1b2703cca549757ea1ad9eca5
|
||||
@@ -0,0 +1 @@
|
||||
{"reason":"idle timeout","timestamp":1786050319918}
|
||||
@@ -0,0 +1 @@
|
||||
997
|
||||
@@ -0,0 +1,534 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Sensors;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.AgvInterfaces;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using Newtonsoft.Json;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Net.Http;
|
||||
using System.Numerics;
|
||||
using System.Security.Cryptography;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using static ClumsyCore.DTools.Painter;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class SetLocationRes
|
||||
{
|
||||
public float x, y, th;
|
||||
public int l_step;
|
||||
public long tick;
|
||||
public string error;
|
||||
}
|
||||
public class AGV : MultiWheelInterface
|
||||
{
|
||||
public override AbstractGeometricController GetController()
|
||||
{
|
||||
return new ChassisController().Get();
|
||||
}
|
||||
|
||||
public override MultiWheelMagTracker GetMagController()
|
||||
{
|
||||
return new MultiWheelMagTracker();
|
||||
}
|
||||
|
||||
public override NaiveMagnetController GetNaiveMagnetController()
|
||||
{
|
||||
return new NaiveMagnetController();
|
||||
}
|
||||
|
||||
public void Sleep(float s)
|
||||
{
|
||||
new DriveTask(new Sleep() { Second = s }.Get()).Wait();
|
||||
}
|
||||
|
||||
public void ControlChargePort(bool open)
|
||||
{
|
||||
DLog.Log($"call ControlChargePort({open})");
|
||||
PilotDefinition.Self.OpenChargeByClumsy = open;
|
||||
}
|
||||
|
||||
public void SwitchLidarArea(int area)
|
||||
{
|
||||
DLog.Log($"call SwitchLidarArea({area})");
|
||||
PilotDefinition.Self.AreaChoose = area;
|
||||
}
|
||||
|
||||
public void SwitchIoArea(int area)
|
||||
{
|
||||
if (area != -1)
|
||||
{
|
||||
PilotDefinition.Self.IOObstacleArea = area;
|
||||
}
|
||||
}
|
||||
public void RotateToTarget(float target)
|
||||
{
|
||||
//if (!needrotate) return;
|
||||
var dl = new DriveTask(new MultiWheelRotateInPlace()
|
||||
{
|
||||
AngleTarget = target,
|
||||
PidparamsRead = () => new PIDParams()
|
||||
{
|
||||
Kp = PilotDefinition.Conf.TireFollowingThkp,
|
||||
Ki = PilotDefinition.Conf.TireFollowingThki,
|
||||
Kd = PilotDefinition.Conf.TireFollowingThkd,
|
||||
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
|
||||
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
|
||||
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
|
||||
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
|
||||
}
|
||||
}.Get());
|
||||
dl.Wait();
|
||||
}
|
||||
|
||||
//参数1：tireNum 需要钻过的轮胎对数量
|
||||
//参数2：frontLidarDetect true:前雷达识别 false:后雷达识别
|
||||
public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||
var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
|
||||
DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
|
||||
if (tireNum != 1 && tireNum != 2)
|
||||
{
|
||||
DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
|
||||
return;
|
||||
}
|
||||
|
||||
if (PilotDefinition.Self.GhostMode)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Console.WriteLine("等待锁取货点中...");
|
||||
Thread.Sleep(200);
|
||||
}
|
||||
Console.WriteLine($"锁点{dstId}完成");
|
||||
Thread.Sleep(1000);
|
||||
Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
|
||||
Thread.Sleep(1000);
|
||||
Leave(srcId);
|
||||
Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
|
||||
Thread.Sleep(2000);
|
||||
//Leave(dstId);
|
||||
//Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
|
||||
Thread.Sleep(2000);
|
||||
Console.WriteLine($"结束钻车动作");
|
||||
return;
|
||||
}
|
||||
|
||||
var detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
|
||||
StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0),
|
||||
LeaveSrcFunction = Leave,
|
||||
SrcId = srcId,
|
||||
DstId = dstId,
|
||||
},
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
|
||||
StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
};
|
||||
DLog.Log($"钻胎为{tireNum}", "TireFollowing");
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = detectors,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
TireNum = tireNum,
|
||||
CarDirection = frontLidarDetect ? 0f : 180f,
|
||||
WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
|
||||
};
|
||||
var _dt = new DriveTask(following.Get());
|
||||
_dt.Wait();
|
||||
DLog.Log("钻车动作结束", "TireFollowing");
|
||||
}
|
||||
|
||||
//离车一定是后雷达识别一个轮胎
|
||||
public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||
DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||||
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0),
|
||||
LeaveSrcFunction = Leave,
|
||||
SrcId = srcId,
|
||||
DstId = dstId,
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = 0.25f,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200f,
|
||||
HandoverSpeed = 0.3f,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
|
||||
IEnumerable<bool> LeaveThenFollow()
|
||||
{
|
||||
foreach (var running in following.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
|
||||
DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
|
||||
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = new Vector2(srcX, srcY),
|
||||
Dst = new Vector2(dstX, dstY),
|
||||
CarDirectionBias = 180f,
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
var _dt = new DriveTask(LeaveThenFollow());
|
||||
_dt.Wait();
|
||||
DLog.Log("离车动作1结束", "TireFollowing");
|
||||
}
|
||||
|
||||
public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||
IEnumerable<bool> TrackThenFollow()
|
||||
{
|
||||
foreach (var running in new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
LeaveSrcFunction = Leave,
|
||||
SrcId = srcId,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200,
|
||||
HandoverSpeed = 0.3f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
|
||||
DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
|
||||
while (!TryLock(426))
|
||||
{
|
||||
Thread.Sleep(20);
|
||||
}
|
||||
Leave(dstId);
|
||||
DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = new Vector2(srcX, srcY),
|
||||
Dst = new Vector2(dstX, dstY),
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
var _dt = new DriveTask(TrackThenFollow());
|
||||
_dt.Wait();
|
||||
DLog.Log("离车动作2结束", "TireFollowing");
|
||||
}
|
||||
|
||||
//驱动器上使能
|
||||
public void DriverAble()
|
||||
{
|
||||
var dl = new DriveTask(new DriverAble() { }.Get());
|
||||
dl.Wait();
|
||||
DLog.Log("驱动器上使能完成", "TireFollowing");
|
||||
}
|
||||
|
||||
//驱动器下使能
|
||||
public void DriverDisable()
|
||||
{
|
||||
var dl = new DriveTask(new DriverDisable() { }.Get());
|
||||
dl.Wait();
|
||||
DLog.Log("驱动器下使能完成", "TireFollowing");
|
||||
}
|
||||
|
||||
// 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
|
||||
public void ClamptoTarget(bool close)
|
||||
{
|
||||
if (PilotDefinition.Self.GhostMode)
|
||||
{
|
||||
Thread.Sleep(2000);
|
||||
Console.WriteLine("夹抱完成");
|
||||
return;
|
||||
}
|
||||
new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
|
||||
RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
|
||||
}.Get()).Wait();
|
||||
}
|
||||
// Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
|
||||
public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
|
||||
float speed)
|
||||
{
|
||||
var dx = dstX - srcX;
|
||||
var dy = dstY - srcY;
|
||||
var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (pathLength <= 1f)
|
||||
{
|
||||
DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
|
||||
return;
|
||||
}
|
||||
|
||||
var self = PilotDefinition.Self;
|
||||
if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
|
||||
!self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
|
||||
Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
|
||||
return;
|
||||
}
|
||||
|
||||
var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
|
||||
var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
|
||||
var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
|
||||
var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
|
||||
DLog.Log(
|
||||
$"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
|
||||
$"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
|
||||
$"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
|
||||
$"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
|
||||
"FleetCrabDbg");
|
||||
|
||||
if (dstId != -1)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
|
||||
}
|
||||
|
||||
var action = new MultiWheelC.FleetCrabWalk
|
||||
{
|
||||
CrabAngleDeg = crabAngle,
|
||||
BodyToPathAngleDeg = bodyToPathAngle,
|
||||
CrabLengthMm = pathLength,
|
||||
CrabSpeed = speed,
|
||||
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
|
||||
FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
|
||||
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
|
||||
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
|
||||
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
|
||||
FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
|
||||
};
|
||||
|
||||
try
|
||||
{
|
||||
new DriveTask(action.Get()).Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (srcId != -1)
|
||||
{
|
||||
Leave(srcId);
|
||||
DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
|
||||
float speed, params float[] trackTypeInfo)
|
||||
{
|
||||
if (trackTypeInfo == null || trackTypeInfo.Length < 2)
|
||||
{
|
||||
DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var trackType = (int)trackTypeInfo[0];
|
||||
if (trackType != 2)
|
||||
{
|
||||
DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var controlPointNum = (int)trackTypeInfo[1];
|
||||
var expectedLength = 2 + controlPointNum * 2;
|
||||
if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
|
||||
{
|
||||
DLog.Log(
|
||||
$"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
|
||||
$"length={trackTypeInfo.Length}, expected>={expectedLength}.",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
BezierTrack track;
|
||||
try
|
||||
{
|
||||
track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
if (track == null)
|
||||
{
|
||||
DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
track.Speed = speed;
|
||||
track.CarDirectionBias = 0f;
|
||||
|
||||
DLog.Log(
|
||||
$"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
|
||||
$"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
|
||||
$"carDirectionBias=0.0)",
|
||||
"FleetCurveDbg");
|
||||
|
||||
if (dstId != -1)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
|
||||
}
|
||||
|
||||
var action = new MultiWheelC.FleetCurveWalk
|
||||
{
|
||||
Track = track,
|
||||
CurveSpeed = speed,
|
||||
CarDirectionBias = 0f,
|
||||
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
|
||||
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
|
||||
SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
||||
StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
|
||||
};
|
||||
|
||||
try
|
||||
{
|
||||
new DriveTask(action.Get()).Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (srcId != -1)
|
||||
{
|
||||
Leave(srcId);
|
||||
DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
|
||||
{
|
||||
DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
|
||||
PilotDefinition.Self.SlowDistance = slowDistance;
|
||||
PilotDefinition.Self.StopDistance = stopDistance;
|
||||
}
|
||||
public void ChangeAvoidanceParam(float length = -1, float width = -1)
|
||||
{
|
||||
|
||||
PilotDefinition.Self.CarLength = length;
|
||||
PilotDefinition.Self.CarWidth = width;
|
||||
|
||||
}
|
||||
public void SetLocation(float x, float y, float th)
|
||||
{
|
||||
DLog.Log($"call SetLocation({x},{y},{th})");
|
||||
Console.WriteLine($"call SetLocation({x},{y},{th})");
|
||||
Queue(() =>
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var str1 = new HttpClient()
|
||||
.GetStringAsync(
|
||||
$"http://127.0.0.1:4321/setLocation?x={x}&y={y}&th={th}")
|
||||
.Result;
|
||||
Thread.Sleep(500);
|
||||
Console.WriteLine($"SetLocation str={str1}");
|
||||
var setLocationRes = JsonConvert.DeserializeObject<SetLocationRes>(str1);
|
||||
Console.WriteLine(setLocationRes.l_step);
|
||||
if (setLocationRes != null && setLocationRes.l_step == 2) break;
|
||||
}
|
||||
});
|
||||
}
|
||||
public float baseSpeed = 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class ChassisController : MovementDefinition<MultiWheelGeometricController>
|
||||
{
|
||||
public float BaseSpeed = Configuration.conf.basicSpeed;
|
||||
|
||||
private DateTime _sendMotionDbgLast = DateTime.MinValue;
|
||||
|
||||
public override MultiWheelGeometricController Get()
|
||||
{
|
||||
return new MultiWheelGeometricController
|
||||
{
|
||||
Chassis = BasicPilotBase.Chassis,
|
||||
BaseSpeed = BaseSpeed,
|
||||
SlowDistance = PilotDefinition.Conf.SlowDistance,
|
||||
SlowingPow = PilotDefinition.Conf.SlowingPow,
|
||||
FinishDistance = PilotDefinition.Conf.FinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FinishSpeed,
|
||||
FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
|
||||
FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
|
||||
FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
|
||||
NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
|
||||
DebugMode = PilotDefinition.Conf.MotionDebugPrint,
|
||||
DebugCurvature = PilotDefinition.Conf.DebugCurvature,
|
||||
PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
|
||||
SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
|
||||
SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
|
||||
SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
|
||||
SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
|
||||
SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
|
||||
MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
|
||||
MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
|
||||
DthLinearFac = PilotDefinition.Conf.DthLinearFac,
|
||||
DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
|
||||
BiasFac = PilotDefinition.Conf.BiasFac,
|
||||
BiasThreshold = PilotDefinition.Conf.BiasThreshold,
|
||||
|
||||
MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
// A: 用固定锁对象（不再锁会被替换的字段引用）。
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock)
|
||||
fleetCnt = self.MultiVehicleFleet.Count;
|
||||
|
||||
// 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
|
||||
if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
|
||||
{
|
||||
_sendMotionDbgLast = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
|
||||
$"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
|
||||
$"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
|
||||
$"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
|
||||
if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
|
||||
return;
|
||||
|
||||
self.MultiVehicleAutoVx = speed;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
// D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
|
||||
self.MultiVehicleAutoIdealX = idealPos.X;
|
||||
self.MultiVehicleAutoIdealY = idealPos.Y;
|
||||
self.MultiVehicleAutoIdealTh = idealAngle;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
// B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
},
|
||||
|
||||
// G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
|
||||
MultiVehicleGetFleetPos = () =>
|
||||
{
|
||||
var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
|
||||
return new Location
|
||||
{
|
||||
x = snap.X,
|
||||
y = snap.Y,
|
||||
th = snap.Th,
|
||||
l_step = 1,
|
||||
tick = DateTime.Now.Ticks
|
||||
};
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<LangVersion>10</LangVersion>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
|
||||
<PackageReference Include="System.Drawing.Common" Version="10.0.10"
|
||||
GeneratePathProperty="true" />
|
||||
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
||||
<PackageReference Include="StbImageWriteSharp" Version="1.16.7"
|
||||
GeneratePathProperty="true" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Compile Remove="tests\PathSmoothingPngVerificationHost\**\*.cs" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>ref\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="LessokajiWeaverUtilities">
|
||||
<HintPath>ref\LessokajiWeaverUtilities.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>ref\MDCSToolBox.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="ClumsyCore">
|
||||
<HintPath>ref\RefClumsyCore.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="ClumsyDance">
|
||||
<HintPath>ref\RefClumsyDance.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>ref\RefFundamentalLib.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
<Target Name="DeployManagedPngRuntime" AfterTargets="Build">
|
||||
<Copy SourceFiles="$(PkgStbImageWriteSharp)\lib\netstandard2.0\StbImageWriteSharp.dll"
|
||||
DestinationFiles="$(TargetDir)StbImageWriteSharp.dll" />
|
||||
</Target>
|
||||
|
||||
<Target Name="DeploySystemDrawingRuntime" AfterTargets="Build">
|
||||
<Copy SourceFiles="$(PkgSystem_Drawing_Common)\lib\netstandard2.0\System.Drawing.Common.dll"
|
||||
DestinationFiles="$(TargetDir)System.Drawing.Common.dll" />
|
||||
</Target>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,526 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
// ===== 车队联动-自动蟹行动作 =====
|
||||
// 以当前车队中心为起点，构造指定方向和长度的直线路径；
|
||||
// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
|
||||
//
|
||||
// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
|
||||
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
|
||||
// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
|
||||
// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
|
||||
//
|
||||
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
|
||||
public class FleetCrabWalk : MovementDefinition
|
||||
{
|
||||
/// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
|
||||
public float CrabAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
|
||||
public float BodyToPathAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径长度(mm)。</summary>
|
||||
public float CrabLengthMm = 2000f;
|
||||
|
||||
/// <summary>行驶速度(m/s)。</summary>
|
||||
public float CrabSpeed = 0.2f;
|
||||
|
||||
/// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
|
||||
public float FleetCrabAccel = 0.2f;
|
||||
|
||||
/// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
|
||||
public float FleetCrabStartAccel = 0.01f;
|
||||
|
||||
/// <summary>末端开始减速距离(mm)。</summary>
|
||||
public float FleetCrabSlowDistance = 2000f;
|
||||
|
||||
/// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
|
||||
public float FleetCrabFinishDistance = 20f;
|
||||
|
||||
/// <summary>末端最低速度(m/s)。</summary>
|
||||
public float FleetCrabFinishSpeed = 0.02f;
|
||||
|
||||
/// <summary>末端减速曲线指数。</summary>
|
||||
public float FleetCrabSlowingPow = 0.8f;
|
||||
|
||||
/// <summary>前后 GCP 舵角修正上限(deg)。</summary>
|
||||
public float GcpThetaThreshold = 95f;
|
||||
|
||||
private bool _stopping;
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = 0;
|
||||
self.MultiVehicleAutoRearTh = 0;
|
||||
self.MultiVehicleAutoHasIdeal = false;
|
||||
self.MultiVehicleAutoEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_stopping = true;
|
||||
Cleanup();
|
||||
}
|
||||
|
||||
private static float Clamp(float value, float min, float max)
|
||||
{
|
||||
if (value < min) return min;
|
||||
if (value > max) return max;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static float ClampAbs(float value, float limit)
|
||||
{
|
||||
var absLimit = Math.Abs(limit);
|
||||
if (absLimit <= 0) return value;
|
||||
if (value > absLimit) return absLimit;
|
||||
if (value < -absLimit) return -absLimit;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static float Slew(float current, float target, float maxDelta)
|
||||
{
|
||||
if (maxDelta <= 0) return target;
|
||||
if (target > current + maxDelta) return current + maxDelta;
|
||||
if (target < current - maxDelta) return current - maxDelta;
|
||||
return target;
|
||||
}
|
||||
|
||||
private static float AverageAngle(float frontTh, float rearTh)
|
||||
{
|
||||
var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
|
||||
return (float)CommonMath.RoundTh(rearTh + diff / 2f);
|
||||
}
|
||||
|
||||
private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
|
||||
out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
|
||||
{
|
||||
var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
|
||||
rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
|
||||
driveSpeed = speed;
|
||||
frontTh = rawFrontTh;
|
||||
rearTh = rawRearTh;
|
||||
reverseEquivalent = false;
|
||||
|
||||
if (rawBaseTh > limit)
|
||||
{
|
||||
frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
|
||||
rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
|
||||
driveSpeed = -driveSpeed;
|
||||
reverseEquivalent = true;
|
||||
}
|
||||
else if (rawBaseTh < -limit)
|
||||
{
|
||||
frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
|
||||
rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
|
||||
driveSpeed = -driveSpeed;
|
||||
reverseEquivalent = true;
|
||||
}
|
||||
|
||||
frontTh = ClampAbs(frontTh, limit);
|
||||
rearTh = ClampAbs(rearTh, limit);
|
||||
}
|
||||
|
||||
private static float ProbeSpeed(float speed)
|
||||
{
|
||||
return Math.Abs(speed) > 1e-4f ? speed : 1f;
|
||||
}
|
||||
|
||||
private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
|
||||
out float yawSign)
|
||||
{
|
||||
yawSign = 0f;
|
||||
if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
|
||||
return false;
|
||||
|
||||
var radius = Math.Max(1f, Math.Abs(controlRadius));
|
||||
Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
|
||||
normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
|
||||
normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
|
||||
var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
|
||||
if (!intersect)
|
||||
return false;
|
||||
|
||||
// Match MultiWheelChassis.SendMotion: the tangent side is selected by
|
||||
// rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
|
||||
var tangentSign = center.Y > 1f ? 1f : -1f;
|
||||
var speedSign = driveSpeed >= 0f ? 1f : -1f;
|
||||
yawSign = speedSign * tangentSign;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
|
||||
{
|
||||
const float probeDth = 1f;
|
||||
ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
|
||||
out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
|
||||
return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
|
||||
? yawSign
|
||||
: 1f;
|
||||
}
|
||||
|
||||
private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
|
||||
Vector2 pathLeft)
|
||||
{
|
||||
var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
|
||||
var rad = motionTh / 180f * Math.PI;
|
||||
var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
|
||||
if (driveSpeed < 0f)
|
||||
dir = -dir;
|
||||
return Vector2.Dot(dir, pathLeft);
|
||||
}
|
||||
|
||||
private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
|
||||
float lateral, float biasProbe)
|
||||
{
|
||||
ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
|
||||
out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
|
||||
var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
|
||||
return -Math.Sign(lateral) * lateralVelocity;
|
||||
}
|
||||
|
||||
private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
|
||||
float lateral)
|
||||
{
|
||||
if (Math.Abs(lateral) <= 1e-3f)
|
||||
return 1f;
|
||||
|
||||
const float probeBias = 1f;
|
||||
var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
|
||||
var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
|
||||
return positiveScore >= negativeScore ? 1f : -1f;
|
||||
}
|
||||
|
||||
private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
|
||||
out float centerTh, out string source)
|
||||
{
|
||||
if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "fleet";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "slam";
|
||||
return true;
|
||||
}
|
||||
|
||||
source = "none";
|
||||
return false;
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
|
||||
yield break;
|
||||
}
|
||||
_stopping = false;
|
||||
|
||||
DLog.Log(
|
||||
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
|
||||
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
|
||||
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
|
||||
$"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
|
||||
$"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
|
||||
$"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
|
||||
// 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
|
||||
DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
|
||||
if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
|
||||
{
|
||||
DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||||
|
||||
DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||||
|
||||
var pathStart = new Vector2(x0, y0);
|
||||
var pathLengthMm = CrabLengthMm;
|
||||
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||||
var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
|
||||
var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
|
||||
var phiRad = phi / 180.0 * Math.PI;
|
||||
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||||
var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||||
|
||||
DLog.Log(
|
||||
$"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
|
||||
$"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
|
||||
$"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
|
||||
$"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
|
||||
$"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
|
||||
$"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
|
||||
var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
|
||||
ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
|
||||
(float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
|
||||
out _, out _);
|
||||
var warmStart = DateTime.Now;
|
||||
var warmSeqBaseline = self.BeginFleetMotionWarmup();
|
||||
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
DLog.Log(
|
||||
$"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
|
||||
$"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
|
||||
"FleetCrabDbg");
|
||||
|
||||
var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
|
||||
var warmIter = 0;
|
||||
var warmReady = false;
|
||||
var warmDetail = "";
|
||||
while (!_stopping && DateTime.Now < warmEnd)
|
||||
{
|
||||
warmIter++;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int cnt;
|
||||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||||
if (warmIter % 5 == 0)
|
||||
DLog.Log(
|
||||
$"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
|
||||
$"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
|
||||
$"detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
|
||||
conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
|
||||
{
|
||||
warmReady = true;
|
||||
DLog.Log(
|
||||
$"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (!warmReady)
|
||||
{
|
||||
DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
|
||||
Cleanup();
|
||||
yield break;
|
||||
}
|
||||
|
||||
Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
|
||||
|
||||
if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
|
||||
{
|
||||
x0 = warmX;
|
||||
y0 = warmY;
|
||||
theta = warmTh;
|
||||
pathStart = new Vector2(x0, y0);
|
||||
phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||||
dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
|
||||
targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
|
||||
phiRad = phi / 180.0 * Math.PI;
|
||||
pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||||
pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
|
||||
var iter = 0;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
|
||||
var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
|
||||
var baseSpeed = Math.Abs(CrabSpeed);
|
||||
var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
|
||||
var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
|
||||
var accel = Math.Abs(FleetCrabAccel);
|
||||
var startAccel = Math.Abs(FleetCrabStartAccel);
|
||||
var cmdSpeed = 0f;
|
||||
var lastTick = DateTime.Now;
|
||||
var speedRampStart = DateTime.Now;
|
||||
var stopReason = "done";
|
||||
|
||||
while (!_stopping)
|
||||
{
|
||||
iter++;
|
||||
|
||||
if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
|
||||
{
|
||||
stopReason = "fleet center invalid";
|
||||
DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
|
||||
break;
|
||||
}
|
||||
var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
|
||||
var along = Vector2.Dot(delta, pathDir);
|
||||
var lateral = Vector2.Dot(delta, pathLeft);
|
||||
var remain = pathLengthMm - along;
|
||||
if (remain <= finishDistance)
|
||||
break;
|
||||
|
||||
var targetSpeed = baseSpeed;
|
||||
var slowRatio = 1f;
|
||||
if (remain < slowDistance)
|
||||
{
|
||||
slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
|
||||
targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
|
||||
}
|
||||
var now = DateTime.Now;
|
||||
var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
|
||||
lastTick = now;
|
||||
var rampElapsed = (now - speedRampStart).TotalSeconds;
|
||||
var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
|
||||
var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
|
||||
cmdSpeed = speed;
|
||||
|
||||
var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
|
||||
var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
|
||||
var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
|
||||
var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
|
||||
var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
|
||||
Math.Max(speed, 0.3f)) / Math.PI * 180.0);
|
||||
var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
|
||||
var rawBiasItem = rawBiasMagnitude * biasSign;
|
||||
var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
|
||||
var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
|
||||
var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
|
||||
var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
|
||||
var rawFrontTh = baseCrabTh + biasItem + dthItem;
|
||||
var rawRearTh = baseCrabTh + biasItem - dthItem;
|
||||
ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
|
||||
out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
|
||||
holdFrontTh = frontTh;
|
||||
holdRearTh = rearTh;
|
||||
var idealAlong = Clamp(along, 0f, pathLengthMm);
|
||||
var ideal = pathStart + pathDir * idealAlong;
|
||||
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = driveSpeed;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
self.MultiVehicleAutoIdealX = ideal.X;
|
||||
self.MultiVehicleAutoIdealY = ideal.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
|
||||
{
|
||||
lastLog = DateTime.Now;
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
|
||||
DLog.Log(
|
||||
$"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||||
$"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
|
||||
$"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
|
||||
$"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
|
||||
$"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
|
||||
$"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
DLog.Log(
|
||||
$"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
|
||||
$"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
|
||||
$"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
|
||||
$"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
|
||||
$"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
|
||||
$"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
|
||||
$"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
|
||||
$"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
|
||||
$"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
|
||||
"FleetCrabHeadingDbg");
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (_stopping)
|
||||
stopReason = "stop";
|
||||
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
|
||||
"FleetCrabDbg");
|
||||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||||
while (!_stopping && DateTime.Now < settleEnd)
|
||||
{
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Cleanup();
|
||||
Hedingben.ToastText("车队蟹行完成", "FleetCrab");
|
||||
DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,411 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class FleetCurveWalk : MovementDefinition
|
||||
{
|
||||
public BezierTrack Track;
|
||||
public List<Vector2> ControlPoints = new();
|
||||
public float CurveSpeed = 0.2f;
|
||||
public float CarDirectionBias = 0f;
|
||||
public int BezierResolution = 100;
|
||||
public float SlowDistance = 2000f;
|
||||
public float FinishDistance = 20f;
|
||||
public float FinishSpeed = 0.02f;
|
||||
public float SlowingPow = 0.8f;
|
||||
public float GcpThetaThreshold = 95f;
|
||||
public float StartSyncTimeoutSec = 8f;
|
||||
|
||||
private bool _stopping;
|
||||
private MultiWheelGeometricController _controller;
|
||||
private MultiWheelChassis _chassis;
|
||||
private bool _savedControlPoints;
|
||||
private float _savedControlRadius;
|
||||
private Vector2 _savedGcp0;
|
||||
private Vector2 _savedGcp1;
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_stopping = true;
|
||||
if (_controller != null)
|
||||
_controller.BreakAndHold = true;
|
||||
Cleanup();
|
||||
}
|
||||
|
||||
public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
|
||||
{
|
||||
points = new List<Vector2>();
|
||||
error = "";
|
||||
if (string.IsNullOrWhiteSpace(text))
|
||||
{
|
||||
error = "empty control point list";
|
||||
return false;
|
||||
}
|
||||
|
||||
var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
|
||||
for (var i = 0; i < segments.Length; i++)
|
||||
{
|
||||
var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
|
||||
if (pair.Length != 2)
|
||||
{
|
||||
error = $"invalid point #{i + 1}: {segments[i]}";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
|
||||
{
|
||||
error = $"invalid number in point #{i + 1}: {segments[i]}";
|
||||
return false;
|
||||
}
|
||||
points.Add(new Vector2(x, y));
|
||||
}
|
||||
|
||||
if (points.Count < 3)
|
||||
{
|
||||
error = "Bezier curve requires at least 3 control points";
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
|
||||
{
|
||||
var source = relativePoints ?? new List<Vector2>();
|
||||
var normalized = new List<Vector2>();
|
||||
if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
|
||||
normalized.Add(Vector2.Zero);
|
||||
for (var i = 0; i < source.Count; i++)
|
||||
normalized.Add(source[i]);
|
||||
if (normalized.Count < 2)
|
||||
normalized.Add(new Vector2(1000f, 0f));
|
||||
if (normalized.Count < 3)
|
||||
normalized.Add(new Vector2(2000f, 0f));
|
||||
|
||||
var result = new List<Vector2>();
|
||||
for (var i = 0; i < normalized.Count; i++)
|
||||
result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
|
||||
return result;
|
||||
}
|
||||
|
||||
public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
|
||||
params float[] controlPointCoords)
|
||||
{
|
||||
var src = new Vector2(srcX, srcY);
|
||||
var dst = new Vector2(dstX, dstY);
|
||||
var result = new List<Vector2>();
|
||||
if (controlPointCoords == null || controlPointCoords.Length == 0)
|
||||
{
|
||||
result.Add(src);
|
||||
result.Add((src + dst) / 2f);
|
||||
result.Add(dst);
|
||||
return result;
|
||||
}
|
||||
|
||||
if (controlPointCoords.Length % 2 != 0)
|
||||
throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
|
||||
|
||||
var supplied = new List<Vector2>();
|
||||
for (var i = 0; i < controlPointCoords.Length; i += 2)
|
||||
supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
|
||||
|
||||
if (supplied.Count >= 3 &&
|
||||
Vector2.Distance(supplied[0], src) <= 10f &&
|
||||
Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
|
||||
return supplied;
|
||||
|
||||
result.Add(src);
|
||||
for (var i = 0; i < supplied.Count; i++)
|
||||
result.Add(supplied[i]);
|
||||
result.Add(dst);
|
||||
if (result.Count < 3)
|
||||
result.Insert(1, (src + dst) / 2f);
|
||||
return result;
|
||||
}
|
||||
|
||||
private static bool TryParseFloat(string text, out float value)
|
||||
{
|
||||
return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
|
||||
float.TryParse(text, out value);
|
||||
}
|
||||
|
||||
private static float ClampAbs(float value, float limit)
|
||||
{
|
||||
var absLimit = Math.Abs(limit);
|
||||
if (absLimit <= 0) return value;
|
||||
if (value > absLimit) return absLimit;
|
||||
if (value < -absLimit) return -absLimit;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
|
||||
out float centerTh, out string source)
|
||||
{
|
||||
if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "fleet";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "slam";
|
||||
return true;
|
||||
}
|
||||
|
||||
source = "none";
|
||||
return false;
|
||||
}
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = 0;
|
||||
self.MultiVehicleAutoRearTh = 0;
|
||||
self.MultiVehicleAutoHasIdeal = false;
|
||||
self.MultiVehicleAutoEnabled = false;
|
||||
RestoreControlPointRadius();
|
||||
}
|
||||
|
||||
private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
|
||||
{
|
||||
if (!_savedControlPoints)
|
||||
{
|
||||
_chassis = chassis;
|
||||
_savedControlRadius = chassis.ControlPointRadius;
|
||||
var gcps = chassis.GetGeometricControlPoints();
|
||||
if (gcps.Count >= 2)
|
||||
{
|
||||
_savedGcp0 = gcps[0].Position;
|
||||
_savedGcp1 = gcps[1].Position;
|
||||
}
|
||||
_savedControlPoints = true;
|
||||
}
|
||||
|
||||
chassis.ControlPointRadius = radius;
|
||||
var points = chassis.GetGeometricControlPoints();
|
||||
if (points.Count >= 2)
|
||||
{
|
||||
points[0].Position = new Vector2(radius, 0);
|
||||
points[1].Position = new Vector2(-radius, 0);
|
||||
}
|
||||
}
|
||||
|
||||
private void RestoreControlPointRadius()
|
||||
{
|
||||
if (!_savedControlPoints || _chassis == null)
|
||||
return;
|
||||
|
||||
_chassis.ControlPointRadius = _savedControlRadius;
|
||||
var points = _chassis.GetGeometricControlPoints();
|
||||
if (points.Count >= 2)
|
||||
{
|
||||
points[0].Position = _savedGcp0;
|
||||
points[1].Position = _savedGcp1;
|
||||
}
|
||||
_savedControlPoints = false;
|
||||
}
|
||||
|
||||
private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
|
||||
float frontTh, float rearTh)
|
||||
{
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
self.MultiVehicleAutoIdealX = idealPos.X;
|
||||
self.MultiVehicleAutoIdealY = idealPos.Y;
|
||||
self.MultiVehicleAutoIdealTh = idealTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
|
||||
_stopping = false;
|
||||
|
||||
if (chassis == null)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var baseSpeed = Math.Abs(CurveSpeed);
|
||||
if (baseSpeed <= 1e-4f)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var resolution = Math.Max(2, BezierResolution);
|
||||
var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
|
||||
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
|
||||
var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
|
||||
|
||||
ApplyFleetControlPointRadius(chassis, controlRadius);
|
||||
|
||||
try
|
||||
{
|
||||
var track = Track;
|
||||
var trackSource = "external";
|
||||
if (track == null)
|
||||
{
|
||||
var points = new List<Vector2>(ControlPoints);
|
||||
track = new BezierTrack(points, resolution);
|
||||
trackSource = "controlPoints";
|
||||
}
|
||||
track.CarDirectionBias = CarDirectionBias;
|
||||
track.Speed = baseSpeed;
|
||||
|
||||
var center = new Vector2(x0, y0);
|
||||
var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
|
||||
var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
|
||||
var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
|
||||
var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
|
||||
|
||||
DLog.Log(
|
||||
$"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
|
||||
$"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
|
||||
$"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
|
||||
$"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
|
||||
$"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
|
||||
"FleetCurveDbg");
|
||||
|
||||
var warmStart = DateTime.Now;
|
||||
var warmSeqBaseline = self.BeginFleetMotionWarmup();
|
||||
WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
|
||||
var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
|
||||
var warmIter = 0;
|
||||
var warmReady = false;
|
||||
var warmDetail = "";
|
||||
while (!_stopping && DateTime.Now < warmEnd)
|
||||
{
|
||||
warmIter++;
|
||||
WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
|
||||
if (warmIter % 5 == 0)
|
||||
{
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int cnt;
|
||||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||||
DLog.Log(
|
||||
$"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||||
$"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
|
||||
"FleetCurveDbg");
|
||||
}
|
||||
|
||||
if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
|
||||
conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
|
||||
{
|
||||
warmReady = true;
|
||||
DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (!warmReady)
|
||||
{
|
||||
DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
|
||||
Cleanup();
|
||||
yield break;
|
||||
}
|
||||
|
||||
_controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
|
||||
_controller.MultiVehicleSync = true;
|
||||
_controller.BaseSpeed = baseSpeed;
|
||||
_controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
|
||||
_controller.FinishDistance = Math.Max(0f, FinishDistance);
|
||||
_controller.FinishSpeed = speedFinish;
|
||||
_controller.SlowingPow = Math.Max(0.01f, SlowingPow);
|
||||
_controller.GcpThetaThreshold = gcpLimit;
|
||||
_controller.AddTrack(track, "FleetCurve");
|
||||
|
||||
Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
|
||||
|
||||
foreach (var running in _controller.Track())
|
||||
{
|
||||
if (_stopping)
|
||||
break;
|
||||
if (!running)
|
||||
break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (!_stopping)
|
||||
{
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||||
while (!_stopping && DateTime.Now < settleEnd)
|
||||
{
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
|
||||
DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
|
||||
}
|
||||
finally
|
||||
{
|
||||
Cleanup();
|
||||
_controller = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,606 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyDance.Detectors;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Security.Cryptography;
|
||||
using System.Threading;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
[MovementTest(name = "轮胎检测")]
|
||||
public class TireDetect : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_running = false;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_painter = UI.GetPainter("TwoLegDetectTest", false);
|
||||
_painter.Clear();
|
||||
|
||||
var frontlidar = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
|
||||
var result = int.Parse(frontlidar.ToString());
|
||||
var lastDetectX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX;
|
||||
var lastDetectY = 0f;
|
||||
while (_running)
|
||||
{
|
||||
var ld = Detect(lastDetectX, SetFilters(lastDetectX, lastDetectY), result == 1 ? true : false);
|
||||
if(ld == null)
|
||||
{
|
||||
//Console.WriteLine("ld == null");
|
||||
continue;
|
||||
}
|
||||
_painter.Clear();
|
||||
var center = (ld.Src + ld.Dst) / 2;
|
||||
var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
|
||||
var distanceLabelPos = center / 2;
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F3}", distanceLabelPos.X, distanceLabelPos.Y);
|
||||
lastDetectX = center.X;
|
||||
lastDetectY = center.Y;
|
||||
Thread.Sleep(100);
|
||||
}
|
||||
}
|
||||
|
||||
public static LineSegment Detect(float guessX, List<DetectFilter> filters, bool frontlidar)
|
||||
{
|
||||
return new Lidar2dDetect2LegTray()
|
||||
{
|
||||
BlobDist = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobDist : PilotDefinition.Conf.TireBackTwoLegBlobDist,
|
||||
BlobPtCount = frontlidar ? PilotDefinition.Conf.TireTwoLegBlobPtCount : PilotDefinition.Conf.TireTwoLegBlobPtCount,
|
||||
BlobSize = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobSize : PilotDefinition.Conf.TireBackTwoLegBlobSize,
|
||||
CenterChange = Tuple.Create(frontlidar ? PilotDefinition.Conf.TireFrontTwoLegCenterChangeX : PilotDefinition.Conf.TireBackTwoLegCenterChangeX, 0f, 0f),
|
||||
LegWidth = PilotDefinition.Conf.TireTwoLegWidth,
|
||||
LegWidthErr = frontlidar ? PilotDefinition.Conf.TireTwoLegWidthErr : PilotDefinition.Conf.TireTwoLegWidthErr,
|
||||
Padding = frontlidar ? PilotDefinition.Conf.TireFrontPadding : PilotDefinition.Conf.TireBackPadding,
|
||||
PillarFindingScope = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegPillarFindingScope : PilotDefinition.Conf.TireBackTwoLegPillarFindingScope,
|
||||
SgnDir = PilotDefinition.Conf.TwoLegSgnDir,
|
||||
}.DetectWithGuess(frontlidar ? "frontlidar" : "leftlidar,rightlidar", new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
||||
guessCoordinateSystem: CoordinateSystem.Car2D, outCoordinateSystem: CoordinateSystem.Car2D, filters);
|
||||
}
|
||||
|
||||
private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
|
||||
painter.Clear();
|
||||
painter.Clear(3000);
|
||||
|
||||
var box = new Vector2[]
|
||||
{
|
||||
new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// PC filter in car coordinate frame
|
||||
return new List<DetectFilter>()
|
||||
{
|
||||
new(CoordinateSystem.Car2D,
|
||||
p => LessMath.IsPointInPolygon4(
|
||||
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
|
||||
};
|
||||
}
|
||||
|
||||
private Painter _painter;
|
||||
private bool _running = true;
|
||||
}
|
||||
|
||||
[MovementTest(name = "钻车测试")]
|
||||
public class FollowTire : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var front = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
|
||||
var result = int.Parse(front.ToString());
|
||||
var lidarname = result == 1 ? "前雷达" : "后雷达";
|
||||
DLog.Log($"开始钻车测试，用{lidarname}识别", "TireFollowing");
|
||||
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
|
||||
StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
|
||||
StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
},
|
||||
CarDirection = result == 1 ? 0f : 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
TireNum = PilotDefinition.Conf.TireFollowingTireNum,
|
||||
WalkBlindTh = result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
|
||||
};
|
||||
_dt = new DriveTask(following.Get());
|
||||
_dt.Wait();
|
||||
DLog.Log($"结束钻车测试", "TireFollowing");
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "离车测试")]
|
||||
public class LeaveCar : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
DLog.Log($"开始离车测试，用后雷达识别", "TireFollowing");
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||||
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
_dt = new DriveTask(following.Get());
|
||||
_dt.Wait();
|
||||
DLog.Log($"结束离车测试", "TireFollowing");
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "抱夹关闭")]
|
||||
public class ClampTest1 : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
|
||||
RightClampTarget = PilotDefinition.Self.RightArmUpperPos
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "抱夹打开")]
|
||||
public class ClampTest2 : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
|
||||
RightClampTarget = PilotDefinition.Self.RightArmLowerPos
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试前进基于轮里程")]
|
||||
public class LineTrackingTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试后退基于轮里程")]
|
||||
public class ReverseLineTrackingTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new LineTracking()
|
||||
{
|
||||
Target = -PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试终点跟踪动作-前进")]
|
||||
public class DstTrackerForward : MovementTest
|
||||
{
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias = 0f;
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var p1 = UI.GetPoint("point1");
|
||||
var p2 = UI.GetPoint("point2");
|
||||
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker()
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试终点跟踪动作-后退")]
|
||||
public class DstTrackerhoutui : MovementTest
|
||||
{
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias = 180f;
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var p1 = UI.GetPoint("point1");
|
||||
var p2 = UI.GetPoint("point2");
|
||||
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker()
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试先直行再终点跟踪")]
|
||||
public class LineTrackThenDstTrackerTest : MovementTest
|
||||
{
|
||||
public float carDirectionBias = 0f;
|
||||
private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var src = UI.GetPoint("请在上位机选择起点(src)");
|
||||
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||||
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||||
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||||
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||||
IEnumerable<bool> TrackThenFollow()
|
||||
{
|
||||
foreach (var running in new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200f,
|
||||
HandoverSpeed = 0.3f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = src,
|
||||
Dst = dst,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
_dt = new DriveTask(TrackThenFollow());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试先离车再终点跟踪")]
|
||||
public class LeaveCarThenDstTrackerTest : MovementTest
|
||||
{
|
||||
private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var src = UI.GetPoint("请在上位机选择离车后起点(src)");
|
||||
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||||
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||||
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||||
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||||
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||||
|
||||
IEnumerable<bool> LeaveThenFollow()
|
||||
{
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||||
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0),
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
|
||||
foreach (var running in following.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = src,
|
||||
Dst = dst,
|
||||
CarDirectionBias = 180f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
_dt = new DriveTask(LeaveThenFollow());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
|
||||
[MovementTest(name = "驱动器下使能测试")]
|
||||
public class DriverDisableTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
new DriveTask(new DriverDisable(){ }.Get()).Wait();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "驱动器复位测试")]
|
||||
public class DriverAbleTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
new DriveTask(new DriverAble(){ }.Get()).Wait();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "底盘旋转测试")]
|
||||
public class RotateToAngleTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var target = UI.GetInput("输入旋转角度：");
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
new DriveTask(new MultiWheelRotateInPlace()
|
||||
{
|
||||
AngleTarget = float.Parse(target),
|
||||
PidparamsRead = () => new PIDParams()
|
||||
{
|
||||
Kp = PilotDefinition.Conf.TireFollowingThkp,
|
||||
Ki = PilotDefinition.Conf.TireFollowingThki,
|
||||
Kd = PilotDefinition.Conf.TireFollowingThkd,
|
||||
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
|
||||
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
|
||||
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
|
||||
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
|
||||
}
|
||||
}.Get()).Wait();
|
||||
}
|
||||
}
|
||||
|
||||
public class utils
|
||||
{
|
||||
public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)
|
||||
{
|
||||
var means = CalculateMean(data);
|
||||
var stdDevs = CalculateStandardDeviation(data, means);
|
||||
|
||||
return data.Where(point =>
|
||||
Math.Abs(point.x - means.x) <= threshold * stdDevs.x &&
|
||||
Math.Abs(point.y - means.y) <= threshold * stdDevs.y &&
|
||||
AngularDistance(point.th, means.th) <= threshold * stdDevs.th
|
||||
).ToList();
|
||||
}
|
||||
|
||||
public static (float x, float y, float th) CalculateMean(List<(float x, float y, float th)> data)
|
||||
{
|
||||
float meanX = data.Average(point => point.x);
|
||||
float meanY = data.Average(point => point.y);
|
||||
float sinSum = data.Sum(point => (float)Math.Sin(DegreeToRadian(point.th)));
|
||||
float cosSum = data.Sum(point => (float)Math.Cos(DegreeToRadian(point.th)));
|
||||
float meanTh = RadianToDegree((float)Math.Atan2(sinSum, cosSum));
|
||||
|
||||
return (meanX, meanY, meanTh);
|
||||
}
|
||||
|
||||
public static (float x, float y, float th) CalculateStandardDeviation(List<(float x, float y, float th)> data, (float x, float y, float th) means)
|
||||
{
|
||||
float varianceX = data.Average(point => (point.x - means.x) * (point.x - means.x));
|
||||
float varianceY = data.Average(point => (point.y - means.y) * (point.y - means.y));
|
||||
|
||||
// 计算角度的方差
|
||||
float varianceTh = data.Average(point => AngularDistance(point.th, means.th) * AngularDistance(point.th, means.th));
|
||||
|
||||
return ((float)Math.Sqrt(varianceX), (float)Math.Sqrt(varianceY), (float)Math.Sqrt(varianceTh));
|
||||
}
|
||||
|
||||
public static float DegreeToRadian(float degree)
|
||||
{
|
||||
return (float)(degree * Math.PI / 180.0);
|
||||
}
|
||||
|
||||
public static float RadianToDegree(float radian)
|
||||
{
|
||||
return (float)(radian * 180.0 / Math.PI);
|
||||
}
|
||||
|
||||
public static float AngularDistance(float angle1, float angle2)
|
||||
{
|
||||
return CommonMath.ThDiff(angle1, angle2);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,136 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyDance.Detectors;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using FundamentalLib;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。
|
||||
/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。
|
||||
/// </summary>
|
||||
[MovementTest(name = "多舵轮-2腿检测")]
|
||||
public class TwoLegDetect : MovementTest
|
||||
{
|
||||
private Painter _painter;
|
||||
private bool _running = true;
|
||||
|
||||
public override void TestStop() => _running = false;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_running = true;
|
||||
_painter = UI.GetPainter("MultiWheelTwoLegDetect", false);
|
||||
_painter.Clear();
|
||||
|
||||
var lidar = PilotDefinition.Conf.TwoLegLidarName;
|
||||
var lastDetectX = PilotDefinition.Conf.TwoLegGuessX;
|
||||
var lastDetectY = 0f;
|
||||
|
||||
while (_running)
|
||||
{
|
||||
var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY));
|
||||
if (ld == null)
|
||||
{
|
||||
Thread.Sleep(100);
|
||||
continue;
|
||||
}
|
||||
|
||||
var center = (ld.Src + ld.Dst) / 2;
|
||||
var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
|
||||
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2);
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " +
|
||||
$"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}",
|
||||
"MultiWheelTwoLegDetect-test");
|
||||
|
||||
// 用本帧中心作为下一帧猜测，实现闭环跟踪
|
||||
lastDetectX = center.X;
|
||||
lastDetectY = center.Y;
|
||||
Thread.Sleep(100);
|
||||
}
|
||||
|
||||
_painter.Clear();
|
||||
}
|
||||
|
||||
/// <summary>在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。</summary>
|
||||
public static LineSegment Detect(string lidarName, float guessX, List<DetectFilter> filters)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var detector = new Lidar2dDetect2LegTray
|
||||
{
|
||||
BlobDist = conf.TwoLegBlobDist,
|
||||
BlobPtCount = conf.TwoLegBlobPtCount,
|
||||
BlobSize = conf.TwoLegBlobSize,
|
||||
CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f),
|
||||
LegWidth = conf.TwoLegWidth,
|
||||
LegWidthErr = conf.TwoLegWidthErr,
|
||||
Padding = conf.TwoLegPadding,
|
||||
PillarFindingScope = conf.TwoLegPillarFindingScope,
|
||||
SgnDir = conf.TwoLegSgnDir,
|
||||
};
|
||||
#pragma warning restore CS0612, CS0618
|
||||
|
||||
var result = detector.DetectWithGuess(
|
||||
lidarName,
|
||||
new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
||||
guessCoordinateSystem: CoordinateSystem.Car2D,
|
||||
outCoordinateSystem: CoordinateSystem.Car2D,
|
||||
filters);
|
||||
return ApplyOutputBias(result);
|
||||
}
|
||||
|
||||
private static LineSegment ApplyOutputBias(LineSegment result)
|
||||
{
|
||||
if (result == null) return null;
|
||||
|
||||
var conf = PilotDefinition.Conf;
|
||||
if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
|
||||
return result;
|
||||
|
||||
var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
|
||||
return new LineSegment(result.Src + bias, result.Dst + bias);
|
||||
}
|
||||
|
||||
/// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
|
||||
public static List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false);
|
||||
painter.Clear();
|
||||
|
||||
var box = new[]
|
||||
{
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// 点云滤波在车体坐标系下进行
|
||||
return new List<DetectFilter>
|
||||
{
|
||||
new(CoordinateSystem.Car2D,
|
||||
p => LessMath.IsPointInPolygon4(
|
||||
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,608 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class MultiForwardTest : MovementDefinition
|
||||
{
|
||||
public float Speed = 0.2f;
|
||||
public float DurationSeconds = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
var end = DateTime.Now.AddSeconds(DurationSeconds);
|
||||
while (DateTime.Now < end)
|
||||
{
|
||||
chassis.SendMotion(Speed, 0, 0);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
chassis.SendMotion(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
|
||||
public class MultiRotateToWorldAngle : MovementDefinition
|
||||
{
|
||||
/// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
|
||||
public float TargetWorldDeg;
|
||||
|
||||
/// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
|
||||
public float RotSpeed = 30f;
|
||||
|
||||
/// <summary>到位角度精度（deg）。</summary>
|
||||
public float ArriveDeg = 1f;
|
||||
|
||||
/// <summary>起转前舵轮对齐精度（deg）。</summary>
|
||||
public float WheelAlignDeg = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
|
||||
// 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
|
||||
while (true)
|
||||
{
|
||||
chassis.SendRotateMotion(0);
|
||||
if (WheelsAligned(chassis, WheelAlignDeg)) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
// 阶段二：旋转到目标世界朝向，到位即停。
|
||||
var target = CommonMath.RoundTh(TargetWorldDeg);
|
||||
while (true)
|
||||
{
|
||||
var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
|
||||
var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
|
||||
if (Math.Abs(diff) <= ArriveDeg) break;
|
||||
chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
chassis.PredefinedDriveStop();
|
||||
}
|
||||
|
||||
private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
|
||||
{
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var wheels = chassis.GetSteerWheels();
|
||||
#pragma warning restore CS0612, CS0618
|
||||
foreach (var sw in wheels)
|
||||
if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-前进2秒")]
|
||||
public class MultiForwardMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiForwardTest().Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop() => _task?.Stop();
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-原地旋转到目标角度")]
|
||||
public class MultiRotateMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiRotateToWorldAngle
|
||||
{
|
||||
TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
|
||||
RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
|
||||
ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||||
WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
|
||||
}.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 车队联动-原地旋转动作 =====
|
||||
// 等价于 FleetRemote 的「原地旋转」模式（已实测可用）：FleetRemote 通过 Medulla 手动 IO
|
||||
// (MultiVehicleManualEnabled + Mode=2 + Vth) 驱动 PilotDefinition.TickMultiVehicle 绕车队中心旋转。
|
||||
// 手动 IO 是 [AsLowerIO]（Medulla→Clumsy，每周期回写），Clumsy 侧动作直接写会被覆盖；
|
||||
// 因此本动作改用 Clumsy 内部脚本字段 MultiVehicleScript*（TickMultiVehicle 已将其作为手动等价输入），
|
||||
// 不写一行底盘指令——实际的 SendRotateMotion + PI 纠偏 + 向从车广播均由 TickMultiVehicle 完成。
|
||||
//
|
||||
// 前提：在「主车」(MultiVehicleMasterEndpoint == "/") 的 Clumsy 上运行，且从车已注册(编队就绪)。
|
||||
// 停止条件：主车 SLAM 朝向累计转过 |TargetDeltaDeg|（刚体原地旋转，整车朝向变化量 == 车队转角）；
|
||||
// 无定位时退化为按 |TargetDeltaDeg| / |Omega| 估算时长；并带安全超时。
|
||||
public class FleetRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>角速度大小(deg/s)；实际方向由 TargetDeltaDeg 的符号决定。</summary>
|
||||
public float Omega = 15f;
|
||||
|
||||
/// <summary>目标相对转角(deg，带符号，+ 为逆时针)。</summary>
|
||||
public float TargetDeltaDeg = 90f;
|
||||
|
||||
/// <summary>到位角度精度(deg)。</summary>
|
||||
public float ArriveDeg = 1.5f;
|
||||
|
||||
/// <summary>减速区宽度(deg)：剩余角度小于此值时，角速度按剩余比例线性降到 MinOmega，抑制惯性超调。</summary>
|
||||
public float SlowDeg = 25f;
|
||||
|
||||
/// <summary>减速区末段最小角速度(deg/s)：避免越接近目标越慢、长尾停不下/到不了位。</summary>
|
||||
public float MinOmega = 3f;
|
||||
|
||||
/// <summary>缓启动角加速度(deg/s²)：起步时角速度从 0 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，<=0 关闭缓启动（阶跃起步）。</summary>
|
||||
public float AccelDegPerSec2 = 20f;
|
||||
|
||||
/// <summary>
|
||||
/// 是否用 Detour 主车航向闭环判停（读 getCartLocation().th 累计实际转角，到 |TargetDeltaDeg| 停）。
|
||||
/// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需要角度反馈，故默认 true。
|
||||
/// false 时退化为按估算时长开环停止（实际转速≠指令时不精确）。注意 true 时若无有效全局定位，
|
||||
/// getCartLocation() 会阻塞（与单车 MultiRotateToWorldAngle 行为一致）。
|
||||
/// </summary>
|
||||
public bool UseDetourHeading = true;
|
||||
|
||||
// 注：不设超时上限——旋转持续到到位（或无定位时按估算时长结束），或被 Stop()/TestStop() 主动中止。
|
||||
|
||||
/// <summary>到位后保持脚本使能、角速度归零的安定时长(s)，让纠偏把队形稳住再撤离。</summary>
|
||||
public float SettleSec = 0.5f;
|
||||
|
||||
private void ClearScript()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop() => ClearScript();
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var dir = Math.Sign(TargetDeltaDeg);
|
||||
if (dir == 0) dir = 1;
|
||||
var maxOmega = Math.Abs(Omega);
|
||||
var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
|
||||
var slowDeg = Math.Max(1e-3f, SlowDeg); // 减速区宽度
|
||||
var accel = AccelDegPerSec2; // 缓启动角加速度，仅作用于起步，<=0 关闭
|
||||
var targetMag = Math.Abs(TargetDeltaDeg);
|
||||
|
||||
var hasPos = UseDetourHeading;
|
||||
var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
|
||||
var startTh = prevTh;
|
||||
var accumulated = 0f; // 累计带符号转角(deg)
|
||||
var start = DateTime.Now;
|
||||
var lastTime = start;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var lastCenterLog = DateTime.MinValue;
|
||||
var cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
|
||||
var centerTracking = false;
|
||||
float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
|
||||
float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
|
||||
// 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
|
||||
var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
|
||||
if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
|
||||
|
||||
DLog.Log(
|
||||
$"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
|
||||
$"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
|
||||
$"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
|
||||
"FleetRotateDbg");
|
||||
|
||||
// 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
|
||||
// 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptMode = 2;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
self.MultiVehicleRotateWheelsReady = false;
|
||||
self.MultiVehicleRotateFleetReady = false;
|
||||
|
||||
DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
|
||||
while (!self.MultiVehicleRotateFleetReady)
|
||||
{
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptMode = 2;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
|
||||
yield return true;
|
||||
}
|
||||
|
||||
float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
|
||||
if (hasPos)
|
||||
{
|
||||
var startPos = DetourInterface.getCartLocation();
|
||||
centerStartCarX = (float)startPos.x;
|
||||
centerStartCarY = (float)startPos.y;
|
||||
centerStartCarTh = (float)startPos.th;
|
||||
prevTh = centerStartCarTh;
|
||||
startTh = prevTh;
|
||||
if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
|
||||
out centerStartX, out centerStartY, out centerStartTh))
|
||||
{
|
||||
centerLastX = centerStartX;
|
||||
centerLastY = centerStartY;
|
||||
centerLastTh = centerStartTh;
|
||||
centerMaxDrift = 0;
|
||||
centerTracking = true;
|
||||
}
|
||||
}
|
||||
accumulated = 0f;
|
||||
start = DateTime.Now;
|
||||
lastTime = start;
|
||||
lastLog = DateTime.MinValue;
|
||||
lastCenterLog = DateTime.MinValue;
|
||||
cmdMag = 0f;
|
||||
DLog.Log(
|
||||
$"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
|
||||
$"fleetAligned={self.MultiVehicleRotateFleetReady}",
|
||||
"FleetRotateDbg");
|
||||
if (centerTracking)
|
||||
{
|
||||
DLog.Log(
|
||||
$"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
|
||||
$"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
|
||||
var stopReason = "stop()";
|
||||
while (true)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
|
||||
lastTime = now;
|
||||
var elapsed = (now - start).TotalSeconds;
|
||||
|
||||
float desiredMag;
|
||||
float curTh = 0f, remaining = 0f, actualRate = 0f;
|
||||
if (hasPos)
|
||||
{
|
||||
var carPos = DetourInterface.getCartLocation();
|
||||
curTh = (float)carPos.th;
|
||||
var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
|
||||
accumulated += step;
|
||||
actualRate = dt > 1e-3 ? step / dt : 0f; // 实际角速率(deg/s)，用于对比指令
|
||||
prevTh = curTh;
|
||||
|
||||
remaining = targetMag - Math.Abs(accumulated);
|
||||
if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
|
||||
|
||||
// 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
|
||||
// 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
|
||||
desiredMag = remaining < slowDeg
|
||||
? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
|
||||
: maxOmega;
|
||||
|
||||
if (centerTracking &&
|
||||
self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
|
||||
out centerLastX, out centerLastY, out centerLastTh))
|
||||
{
|
||||
var centerDx = centerLastX - centerStartX;
|
||||
var centerDy = centerLastY - centerStartY;
|
||||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||||
centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
|
||||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||||
if ((now - lastCenterLog).TotalMilliseconds >= 250)
|
||||
{
|
||||
lastCenterLog = now;
|
||||
DLog.Log(
|
||||
$"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||||
$"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||||
$"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
|
||||
desiredMag = maxOmega;
|
||||
if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
|
||||
}
|
||||
|
||||
// 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
|
||||
// “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
|
||||
if (accel > 1e-3 && desiredMag > cmdMag)
|
||||
cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
|
||||
else
|
||||
cmdMag = desiredMag;
|
||||
|
||||
self.MultiVehicleScriptVth = dir * cmdMag;
|
||||
|
||||
// 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
|
||||
if ((now - lastLog).TotalMilliseconds >= 150)
|
||||
{
|
||||
lastLog = now;
|
||||
DLog.Log(
|
||||
hasPos
|
||||
? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||||
$"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
|
||||
: $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
|
||||
"FleetRotateDbg");
|
||||
}
|
||||
|
||||
Hedingben.ToastText(
|
||||
hasPos
|
||||
? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
|
||||
: $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
|
||||
"FleetRotate");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
// 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
|
||||
while (DateTime.Now < settleEnd)
|
||||
yield return true;
|
||||
|
||||
ClearScript();
|
||||
DLog.Log(
|
||||
$"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
|
||||
$"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
|
||||
"FleetRotateDbg");
|
||||
if (centerTracking)
|
||||
{
|
||||
var centerDx = centerLastX - centerStartX;
|
||||
var centerDy = centerLastY - centerStartY;
|
||||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||||
DLog.Log(
|
||||
$"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||||
$"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-原地旋转")]
|
||||
public class FleetRotateInPlaceTest : MovementTest
|
||||
{
|
||||
private FleetRotateInPlace _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetRotateInPlace
|
||||
{
|
||||
Omega = PilotDefinition.Conf.FleetRotateOmega,
|
||||
TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg,
|
||||
ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg,
|
||||
SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg,
|
||||
MinOmega = PilotDefinition.Conf.FleetRotateMinOmega,
|
||||
AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel,
|
||||
SettleSec = PilotDefinition.Conf.FleetRotateSettleSec,
|
||||
UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-曲线行走")]
|
||||
public class FleetCurveWalkTest : MovementTest
|
||||
{
|
||||
private FleetCurveWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
|
||||
!self.TryGetFleetCenterFromSlam(out x, out y, out th))
|
||||
{
|
||||
DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
|
||||
var controlPoints = new List<Vector2>();
|
||||
for (var i = 0; i < pointCount; i++)
|
||||
controlPoints.Add(UI.GetPoint($"FleetCurve point {i + 1}/{pointCount}"));
|
||||
var fleetCenter = new Vector2(x, y);
|
||||
if (Vector2.Distance(fleetCenter, controlPoints[0]) >
|
||||
Vector2.Distance(fleetCenter, controlPoints[controlPoints.Count - 1]))
|
||||
controlPoints.Reverse();
|
||||
var track = new BezierTrack(controlPoints)
|
||||
{
|
||||
Speed = PilotDefinition.Conf.FleetCurveSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
|
||||
_proc = new FleetCurveWalk
|
||||
{
|
||||
Track = track,
|
||||
CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
|
||||
CarDirectionBias = 0f,
|
||||
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
|
||||
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
|
||||
SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
||||
StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-自动蟹行")]
|
||||
public class FleetCrabWalkTest : MovementTest
|
||||
{
|
||||
private FleetCrabWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetCrabWalk
|
||||
{
|
||||
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||||
BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||||
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
|
||||
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
|
||||
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
|
||||
FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
|
||||
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
|
||||
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
|
||||
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
|
||||
FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
|
||||
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI前移")]
|
||||
public class WebApiForwardMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI左移")]
|
||||
public class WebApiLeftMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI旋转")]
|
||||
public class WebApiRotateTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var deg = PilotDefinition.Conf.WebApiRotateDeg;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
var ny = pose.YawDeg + deg; // 逆时针为正
|
||||
PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
|
||||
Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI恢复运动")]
|
||||
public class WebApiMotionResumeTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.ResumeMotion(url);
|
||||
Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI暂停运动")]
|
||||
public class WebApiMotionPauseTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
|
||||
Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,326 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Sensors;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using ClumsyDance.Sensors;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.HighLevelSecurity;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using Newtonsoft.Json;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Net.Http;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using static ClumsyCore.DTools.Painter;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class MultiWheelRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>
|
||||
/// 旋转目标角度
|
||||
/// </summary>
|
||||
public float AngleTarget;
|
||||
|
||||
public float MaxSpeed;
|
||||
|
||||
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
||||
|
||||
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
|
||||
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
private static float RangeAngle(float theta)
|
||||
{
|
||||
return (float)(theta - Math.Round(theta / 360.0f) * 360);
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var targetAngle = RangeAngle(AngleTarget);
|
||||
var p = PidparamsRead();
|
||||
thPid = new PIDController(ThetaReader, p.Kp);
|
||||
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||||
|
||||
DateTime lastTime = DateTime.Now;
|
||||
while (true)
|
||||
{
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
Chassis.SendXYThSpeed(0, 0, s);
|
||||
lastTime = DateTime.Now;
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
Chassis.SendXYThSpeed(0, 0, 0);
|
||||
Console.WriteLine($"final rotate to {targetAngle}");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public class ClampToTarget : MovementDefinition
|
||||
{
|
||||
public float LeftClampTarget;
|
||||
public float RightClampTarget;
|
||||
|
||||
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
|
||||
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
|
||||
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
|
||||
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
|
||||
public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
|
||||
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
|
||||
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
|
||||
private PIDController leftpid, rightpid;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd,
|
||||
ClampMaxI, ClampDeadZone, MaxClampSpeed)
|
||||
{ SpeedAccPerSec = ClampSpeedAcc };
|
||||
|
||||
rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd,
|
||||
ClampMaxI, ClampDeadZone, MaxClampSpeed)
|
||||
{ SpeedAccPerSec = ClampSpeedAcc };
|
||||
|
||||
while (true)
|
||||
{
|
||||
var leftspeed = leftpid.GetResponse(LeftClampTarget);
|
||||
var rightspeed = rightpid.GetResponse(RightClampTarget);
|
||||
Console.WriteLine($"left arm speed:{leftspeed} right arm speed:{rightspeed}");
|
||||
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||||
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||||
if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
|
||||
|
||||
if (leftpid.IsArrived() && rightpid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
Console.WriteLine($"left clamp to target:{LeftClampTarget} right clamp to target:{RightClampTarget}");
|
||||
}
|
||||
}
|
||||
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2;
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var start = DateTime.Now;
|
||||
while ((DateTime.Now-start).TotalSeconds<Second)
|
||||
{
|
||||
yield return true;
|
||||
Thread.Sleep(1000);
|
||||
Console.WriteLine("Sleep");
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
//直线行走基于detour
|
||||
public class LineTracking1 : MovementDefinition
|
||||
{
|
||||
public float LineDistance = 1000f;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
public Painter painter = UI.GetPainter("Line", false);
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var curpose = DetourInterface.getCartLocation();
|
||||
Console.WriteLine($"curpose.th:{curpose.th}");
|
||||
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||||
var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
|
||||
(float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
|
||||
Console.WriteLine($"src:{src.X} {src.Y}");
|
||||
Console.WriteLine($"dst:{dst.X} {dst.Y}");
|
||||
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
|
||||
tracker.AddTrack(linePath);
|
||||
var _dt = new DriveTask(tracker.Track());
|
||||
_dt.Wait();
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
//在世界坐标系下，从路径起点追踪到终点并停车
|
||||
public class DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
|
||||
public float InitialSendSpeed = 0;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
|
||||
if (InitialSendSpeed != 0)
|
||||
{
|
||||
tracker.SkipInitialRotate = true;
|
||||
tracker.InitialSendSpeed = InitialSendSpeed;
|
||||
}
|
||||
|
||||
var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
|
||||
tracker.AddTrack(linePath);
|
||||
var task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
|
||||
// 到点后兜底停车
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
//直线行走基于轮里程
|
||||
public class LineTracking : MovementDefinition
|
||||
{
|
||||
public float Target;
|
||||
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
|
||||
public float Kp = PilotDefinition.Conf.LineTrackKp;
|
||||
public float Ki = PilotDefinition.Conf.LineTrackKi;
|
||||
public float Kd = PilotDefinition.Conf.LineTrackKd;
|
||||
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
private PIDController pid;
|
||||
// 末段衔接：接近目标后不再让 PID 把速度降到 0，保留一个接力速度给后续动作接管
|
||||
public bool EnableHandover = false;
|
||||
public float HandoverDistance = 80f; // mm
|
||||
public float HandoverSpeed = 0.15f; // m/s
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
|
||||
pid = new PIDController(() =>
|
||||
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
|
||||
{ SpeedAccPerSec = MaxSpeed / 2f };
|
||||
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
//chassis.SetOriginBias(0, 0, 0);
|
||||
DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
|
||||
while (true)
|
||||
{
|
||||
var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
|
||||
var remain = Target - current;
|
||||
if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance))
|
||||
{
|
||||
var handoverSign = Math.Sign(remain);
|
||||
if (handoverSign == 0) handoverSign = 1;
|
||||
var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign;
|
||||
Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}");
|
||||
chassis.SendXYThSpeed(handoverSpeed, 0, 0);
|
||||
// 保留一拍接力速度，让后续 DstTracker 无缝接管
|
||||
yield return true;
|
||||
break;
|
||||
}
|
||||
var speed = pid.GetResponse(Target);
|
||||
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
|
||||
|
||||
chassis.SendXYThSpeed(speed, 0, 0);
|
||||
|
||||
if (pid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class DriverAble : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 2000;
|
||||
public int PollIntervalMs = 50;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine("驱动器上使能");
|
||||
PilotDefinition.Self.ResetFromC = true;
|
||||
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var success = PilotDefinition.Self.WheelAbleState;
|
||||
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
success = PilotDefinition.Self.WheelAbleState;
|
||||
if (!success) yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.ResetFromC = false;
|
||||
if (success)
|
||||
Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
|
||||
else
|
||||
Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class DriverDisable : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 3000;
|
||||
public int PollIntervalMs = 20;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine("驱动器下使能");
|
||||
PilotDefinition.Self.DisableFromC = true;
|
||||
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var success = !PilotDefinition.Self.WheelAbleState;
|
||||
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
success = !PilotDefinition.Self.WheelAbleState;
|
||||
if (!success) yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.DisableFromC = false;
|
||||
if (success)
|
||||
Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
|
||||
else
|
||||
Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>最终粗路径上的一个稠密连续点；长度、净空和位置使用 m，航向使用 rad，曲率使用 1/m。</summary>
|
||||
public sealed class CoarsePathPoint
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建粗路径点。
|
||||
/// 参数：xMeters、yMeters 为世界坐标 m；headingRadians 与 unwrappedHeadingRadians 为航向 rad；arcLengthMeters 和 bodyClearanceMeters 为 m；vehicleCurvaturePerMeter 为 1/m。
|
||||
/// </summary>
|
||||
public CoarsePathPoint(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double headingRadians,
|
||||
double unwrappedHeadingRadians,
|
||||
double arcLengthMeters,
|
||||
TravelDirection direction,
|
||||
double vehicleCurvaturePerMeter,
|
||||
double bodyClearanceMeters,
|
||||
bool isGearSwitchPoint,
|
||||
CoarsePathPointSource source)
|
||||
{
|
||||
X = xMeters;
|
||||
Y = yMeters;
|
||||
Heading = headingRadians;
|
||||
UnwrappedHeading = unwrappedHeadingRadians;
|
||||
ArcLength = arcLengthMeters;
|
||||
Direction = direction;
|
||||
VehicleCurvature = vehicleCurvaturePerMeter;
|
||||
BodyClearance = bodyClearanceMeters;
|
||||
IsGearSwitchPoint = isGearSwitchPoint;
|
||||
Source = source;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 坐标，单位 m。</summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>世界 Y 坐标，单位 m。</summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>归一化后可用于几何查询的车头航向，单位 rad。</summary>
|
||||
public double Heading { get; }
|
||||
|
||||
/// <summary>跨越 ±π 后仍连续的车头航向，单位 rad。</summary>
|
||||
public double UnwrappedHeading { get; }
|
||||
|
||||
/// <summary>从路径起点累计的弧长，单位 m；必须非负且不递减。</summary>
|
||||
public double ArcLength { get; }
|
||||
|
||||
/// <summary>从上一点运动到当前点所在方向段的行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>车辆在此点采用的恒曲率原语曲率，单位 1/m。</summary>
|
||||
public double VehicleCurvature { get; }
|
||||
|
||||
/// <summary>扩大车体到障碍物的保守净空下界，单位 m。</summary>
|
||||
public double BodyClearance { get; }
|
||||
|
||||
/// <summary>此点是否为新方向段开始的换向点。</summary>
|
||||
public bool IsGearSwitchPoint { get; }
|
||||
|
||||
/// <summary>此点由起点、普通原语或终点截断产生的来源。</summary>
|
||||
public CoarsePathPointSource Source { get; }
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>粗路径点在搜索与原语重建中的产生来源。</summary>
|
||||
public enum CoarsePathPointSource
|
||||
{
|
||||
/// <summary>请求提供的起始位姿。</summary>
|
||||
Start,
|
||||
|
||||
/// <summary>未截断恒曲率运动原语的内部积分点。</summary>
|
||||
MotionPrimitive,
|
||||
|
||||
/// <summary>首次满足目标容差而在原语内部截断的终点积分点。</summary>
|
||||
GoalTruncation,
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>终点候选最后一个连续运动段的方向约束。</summary>
|
||||
public enum GoalDirectionConstraint
|
||||
{
|
||||
/// <summary>不限制进入终点的运动方向。</summary>
|
||||
Any,
|
||||
|
||||
/// <summary>必须以前进方向进入终点。</summary>
|
||||
Forward,
|
||||
|
||||
/// <summary>必须以倒车方向进入终点。</summary>
|
||||
Reverse,
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid A* 粗路径搜索的可调配置。
|
||||
/// 长度和距离使用 m，航向使用 rad，曲率使用 1/m；各项有效范围由规划器在执行前校验。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarConfiguration
|
||||
{
|
||||
/// <summary>创建采用 P0 固定安全边界和代价权重的默认配置。</summary>
|
||||
public HybridAStarConfiguration()
|
||||
{
|
||||
PrimitiveLengthMeters = 0.50d;
|
||||
IntegrationStepMeters = 0.05d;
|
||||
MaximumCollisionCheckStepMeters = 0.025d;
|
||||
HeadingResolutionRadians = Math.PI / 36d;
|
||||
CurvatureLevelCount = 5;
|
||||
GoalPositionToleranceMeters = 0.15d;
|
||||
GoalHeadingToleranceRadians = Math.PI / 36d;
|
||||
MaximumExpandedNodes = 200000;
|
||||
SearchTimeout = TimeSpan.FromSeconds(5d);
|
||||
HeuristicWeight = 1d;
|
||||
ReverseCostMultiplier = 1.5d;
|
||||
GearSwitchPenaltyMeters = 1d;
|
||||
CurvatureMagnitudeWeight = 0.10d;
|
||||
CurvatureChangePenaltyMetersPerLevel = 0.05d;
|
||||
ClearanceCostWeight = 0.20d;
|
||||
ClearanceCostDistanceMeters = 0.50d;
|
||||
AllowReverse = true;
|
||||
}
|
||||
|
||||
/// <summary>单个恒曲率原语的最大行驶长度，单位 m。</summary>
|
||||
public double PrimitiveLengthMeters { get; set; }
|
||||
|
||||
/// <summary>原语内部输出积分点之间允许的最大弧长，单位 m。</summary>
|
||||
public double IntegrationStepMeters { get; set; }
|
||||
|
||||
/// <summary>连续碰撞检查允许的最大车辆中心位移，单位 m；实际值还受地图分辨率限制。</summary>
|
||||
public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
|
||||
/// <summary>搜索状态离散所使用的航向格宽，单位 rad。</summary>
|
||||
public double HeadingResolutionRadians { get; set; }
|
||||
|
||||
/// <summary>从最大负曲率到最大正曲率的离散曲率等级数。</summary>
|
||||
public int CurvatureLevelCount { get; set; }
|
||||
|
||||
/// <summary>终点位置允许的欧氏距离误差，单位 m。</summary>
|
||||
public double GoalPositionToleranceMeters { get; set; }
|
||||
|
||||
/// <summary>终点车头航向允许的最小环形角度误差，单位 rad。</summary>
|
||||
public double GoalHeadingToleranceRadians { get; set; }
|
||||
|
||||
/// <summary>单次搜索允许扩展的最大节点数。</summary>
|
||||
public int MaximumExpandedNodes { get; set; }
|
||||
|
||||
/// <summary>单次搜索允许消耗的最长时间；超出后返回 <see cref="PlanningStatus.SearchTimeout"/>。</summary>
|
||||
public TimeSpan SearchTimeout { get; set; }
|
||||
|
||||
/// <summary>二维绕障启发式的权重；1 表示不额外放大。</summary>
|
||||
public double HeuristicWeight { get; set; }
|
||||
|
||||
/// <summary>倒车原语长度代价相对前进原语的倍率。</summary>
|
||||
public double ReverseCostMultiplier { get; set; }
|
||||
|
||||
/// <summary>相邻原语发生换向时增加的等效距离代价，单位 m。</summary>
|
||||
public double GearSwitchPenaltyMeters { get; set; }
|
||||
|
||||
/// <summary>曲率绝对值对应的无量纲代价权重。</summary>
|
||||
public double CurvatureMagnitudeWeight { get; set; }
|
||||
|
||||
/// <summary>相邻曲率等级每变化一级增加的等效距离代价，单位 m。</summary>
|
||||
public double CurvatureChangePenaltyMetersPerLevel { get; set; }
|
||||
|
||||
/// <summary>车体保守净空不足时增加的无量纲代价权重。</summary>
|
||||
public double ClearanceCostWeight { get; set; }
|
||||
|
||||
/// <summary>计算净空代价时视为足够安全的车体保守净空，单位 m。</summary>
|
||||
public double ClearanceCostDistanceMeters { get; set; }
|
||||
|
||||
/// <summary>是否允许生成倒车原语；false 时搜索只生成前进原语。</summary>
|
||||
public bool AllowReverse { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>粗路径中方向一致的一段连续点范围；索引两端均包含在段内。</summary>
|
||||
public sealed class PathSegment
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建方向分段。
|
||||
/// 参数：segmentIndex 为从零开始的段序号；startIndex 与 endIndex 为 <see cref="PlanningResult.Path"/> 的包含式索引；两个换向标记描述段首或段尾是否位于换向对。
|
||||
/// </summary>
|
||||
public PathSegment(int segmentIndex, TravelDirection direction, int startIndex, int endIndex, bool startsAtGearSwitch, bool endsAtGearSwitch)
|
||||
{
|
||||
SegmentIndex = segmentIndex;
|
||||
Direction = direction;
|
||||
StartIndex = startIndex;
|
||||
EndIndex = endIndex;
|
||||
StartsAtGearSwitch = startsAtGearSwitch;
|
||||
EndsAtGearSwitch = endsAtGearSwitch;
|
||||
}
|
||||
|
||||
/// <summary>从零开始的方向段序号。</summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>此段所有连续运动点对应的行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>此段在 <see cref="PlanningResult.Path"/> 中的起始索引，包含该点。</summary>
|
||||
public int StartIndex { get; }
|
||||
|
||||
/// <summary>此段在 <see cref="PlanningResult.Path"/> 中的结束索引，包含该点。</summary>
|
||||
public int EndIndex { get; }
|
||||
|
||||
/// <summary>此段首点是否为换向后保留的新方向点。</summary>
|
||||
public bool StartsAtGearSwitch { get; }
|
||||
|
||||
/// <summary>此段尾点是否紧邻下一方向段的换向对。</summary>
|
||||
public bool EndsAtGearSwitch { get; }
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>一次规划的只读统计与终止说明；长度和净空单位为 m。</summary>
|
||||
public sealed class PlanningDiagnostics
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建规划统计快照。
|
||||
/// 参数中的节点数量均为非负计数；pathLengthMeters 和 minimumBodyClearanceMeters 单位为 m；elapsed 为总耗时；pathSearchElapsed 为地图和起终点预检通过后的路径产出耗时；terminationReason 为可读终止说明，可为 null。
|
||||
/// </summary>
|
||||
public PlanningDiagnostics(
|
||||
int expandedNodeCount = 0,
|
||||
int generatedNodeCount = 0,
|
||||
int reopenedNodeCount = 0,
|
||||
int staleOpenListEntryCount = 0,
|
||||
int peakOpenListCount = 0,
|
||||
double pathLengthMeters = 0d,
|
||||
double minimumBodyClearanceMeters = 0d,
|
||||
TimeSpan elapsed = default(TimeSpan),
|
||||
string terminationReason = null,
|
||||
TimeSpan pathSearchElapsed = default(TimeSpan))
|
||||
{
|
||||
ExpandedNodeCount = expandedNodeCount;
|
||||
GeneratedNodeCount = generatedNodeCount;
|
||||
ReopenedNodeCount = reopenedNodeCount;
|
||||
StaleOpenListEntryCount = staleOpenListEntryCount;
|
||||
PeakOpenListCount = peakOpenListCount;
|
||||
PathLengthMeters = pathLengthMeters;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
Elapsed = elapsed;
|
||||
PathSearchElapsed = pathSearchElapsed;
|
||||
TerminationReason = terminationReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>从 Open List 取出并真正扩展的节点数量。</summary>
|
||||
public int ExpandedNodeCount { get; }
|
||||
|
||||
/// <summary>生成并尝试加入搜索状态的节点数量。</summary>
|
||||
public int GeneratedNodeCount { get; }
|
||||
|
||||
/// <summary>以严格更小代价到达同一离散键而重新打开的节点数量。</summary>
|
||||
public int ReopenedNodeCount { get; }
|
||||
|
||||
/// <summary>从 Open List 取出后因已有更优条目而丢弃的陈旧堆条目数量。</summary>
|
||||
public int StaleOpenListEntryCount { get; }
|
||||
|
||||
/// <summary>搜索期间 Open List 同时容纳的最大有效或待丢弃条目数量。</summary>
|
||||
public int PeakOpenListCount { get; }
|
||||
|
||||
/// <summary>成功路径的累计弧长，单位 m；失败结果通常为 0。</summary>
|
||||
public double PathLengthMeters { get; }
|
||||
|
||||
/// <summary>成功路径所有点中车体保守净空下界的最小值，单位 m；失败结果通常为 0。</summary>
|
||||
public double MinimumBodyClearanceMeters { get; }
|
||||
|
||||
/// <summary>从规划入口到返回结果的总耗时。</summary>
|
||||
public TimeSpan Elapsed { get; }
|
||||
|
||||
/// <summary>地图和起终点预检通过后，二维启发式、Hybrid A*、回溯、装配和最终复核的耗时；不含建图，搜索前失败时为零。</summary>
|
||||
public TimeSpan PathSearchElapsed { get; }
|
||||
|
||||
/// <summary>面向调用方的终止原因；成功时可为空字符串。</summary>
|
||||
public string TerminationReason { get; }
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 已建规划地图上的一次 Hybrid A* 请求。
|
||||
/// 本对象只接受不可变 <see cref="PlanningGridMap"/> 快照，不包含地图构建器、传感器、UI 或调试对象。
|
||||
/// </summary>
|
||||
public sealed class PlanningRequest
|
||||
{
|
||||
/// <summary>创建空规划请求。调用规划器前必须提供地图、起点、终点、车辆和配置。</summary>
|
||||
public PlanningRequest()
|
||||
{
|
||||
StartVehicleCurvature = 0d;
|
||||
GoalDirection = GoalDirectionConstraint.Any;
|
||||
}
|
||||
|
||||
/// <summary>本次搜索唯一允许查询的不可变规划地图快照；位置查询单位为 m。</summary>
|
||||
public PlanningGridMap Map { get; set; }
|
||||
|
||||
/// <summary>车辆几何中心的起始位姿；位置单位 m，航向单位 rad。</summary>
|
||||
public Pose2D Start { get; set; }
|
||||
|
||||
/// <summary>车辆几何中心的目标位姿；位置单位 m，航向单位 rad。</summary>
|
||||
public Pose2D Goal { get; set; }
|
||||
|
||||
/// <summary>车辆几何、余量和曲率限制；尺寸单位 m，曲率单位 1/m。</summary>
|
||||
public VehicleParameters Vehicle { get; set; }
|
||||
|
||||
/// <summary>搜索步长、离散、终点容差、代价和资源上限配置。</summary>
|
||||
public HybridAStarConfiguration Configuration { get; set; }
|
||||
|
||||
/// <summary>车辆起步时的转向曲率，单位 1/m；默认值为 0。</summary>
|
||||
public double StartVehicleCurvature { get; set; }
|
||||
|
||||
/// <summary>起步方向约束；null 表示可从前进或倒车开始。</summary>
|
||||
public TravelDirection? StartDirection { get; set; }
|
||||
|
||||
/// <summary>目标进入方向约束；默认值为 <see cref="GoalDirectionConstraint.Any"/>。</summary>
|
||||
public GoalDirectionConstraint GoalDirection { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 一次粗路径规划的最终不可变结果。
|
||||
/// 只有 <see cref="Status"/> 为 <see cref="PlanningStatus.Success"/> 时才携带非空路径与方向分段；其他状态始终返回空只读集合。
|
||||
/// </summary>
|
||||
public sealed class PlanningResult
|
||||
{
|
||||
private static readonly IReadOnlyList<CoarsePathPoint> EmptyPath = new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
|
||||
private static readonly IReadOnlyList<PathSegment> EmptySegments = new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
|
||||
|
||||
private PlanningResult(PlanningStatus status, PlanningDiagnostics diagnostics, IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments)
|
||||
{
|
||||
Status = status;
|
||||
Diagnostics = diagnostics ?? new PlanningDiagnostics(terminationReason: "未提供诊断信息。");
|
||||
Path = path;
|
||||
Segments = segments;
|
||||
}
|
||||
|
||||
/// <summary>规划最终状态；只有 <see cref="PlanningStatus.Success"/> 可以发布路径。</summary>
|
||||
public PlanningStatus Status { get; }
|
||||
|
||||
/// <summary>节点、耗时、路径长度、净空和终止原因统计；始终非空。</summary>
|
||||
public PlanningDiagnostics Diagnostics { get; }
|
||||
|
||||
/// <summary>成功时的稠密粗路径；失败时为不可修改的空集合。</summary>
|
||||
public IReadOnlyList<CoarsePathPoint> Path { get; }
|
||||
|
||||
/// <summary>成功时覆盖 <see cref="Path"/> 的包含式方向分段；失败时为不可修改的空集合。</summary>
|
||||
public IReadOnlyList<PathSegment> Segments { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 创建成功结果。
|
||||
/// 参数：path 与 segments 必须均为非空；diagnostics 为本次规划的统计快照。参数不符合要求时抛出 <see cref="ArgumentException"/>，防止以成功状态发布不完整路径。
|
||||
/// </summary>
|
||||
public static PlanningResult Success(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, PlanningDiagnostics diagnostics)
|
||||
{
|
||||
if (path == null || path.Count == 0)
|
||||
throw new ArgumentException("Successful planning results require a non-empty path.", nameof(path));
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("Successful planning results require non-empty segments.", nameof(segments));
|
||||
return new PlanningResult(PlanningStatus.Success, diagnostics, CopyReadOnly(path), CopyReadOnly(segments));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建失败、取消或资源受限结果。
|
||||
/// 参数：status 不能为 <see cref="PlanningStatus.Success"/>；diagnostics 会原样保留。返回结果的路径与分段始终为空只读集合。
|
||||
/// </summary>
|
||||
public static PlanningResult Failure(PlanningStatus status, PlanningDiagnostics diagnostics)
|
||||
{
|
||||
if (status == PlanningStatus.Success)
|
||||
throw new ArgumentException("Use Success to create a successful planning result.", nameof(status));
|
||||
return new PlanningResult(status, diagnostics, EmptyPath, EmptySegments);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>粗路径规划的最终状态；除 <see cref="Success"/> 外均不发布路径或方向分段。</summary>
|
||||
public enum PlanningStatus
|
||||
{
|
||||
/// <summary>已得到并通过最终连续碰撞复核的完整粗路径。</summary>
|
||||
Success,
|
||||
|
||||
/// <summary>在搜索扩展检查点收到取消请求。</summary>
|
||||
Cancelled,
|
||||
|
||||
/// <summary>请求对象或其必要成员为空，或包含不符合基本契约的值。</summary>
|
||||
InvalidRequest,
|
||||
|
||||
/// <summary>请求提供的地图对象不满足规划器的结构要求。</summary>
|
||||
InvalidMap,
|
||||
|
||||
/// <summary>地图快照尚未准备好参与规划；应查看地图的阻止原因。</summary>
|
||||
MapNotReady,
|
||||
|
||||
/// <summary>车辆尺寸、安全余量或曲率限制无效。</summary>
|
||||
InvalidVehicleParameters,
|
||||
|
||||
/// <summary>原语、离散、代价、容差或资源限制配置无效。</summary>
|
||||
InvalidCurvatureConfiguration,
|
||||
|
||||
/// <summary>起始车辆几何中心或扩大车体不在地图范围内。</summary>
|
||||
StartOutsideMap,
|
||||
|
||||
/// <summary>起始扩大车体与地图障碍物相交或擦边。</summary>
|
||||
StartInCollision,
|
||||
|
||||
/// <summary>目标车辆几何中心或扩大车体不在地图范围内。</summary>
|
||||
GoalOutsideMap,
|
||||
|
||||
/// <summary>目标扩大车体与地图障碍物相交或擦边。</summary>
|
||||
GoalInCollision,
|
||||
|
||||
/// <summary>搜索达到 <see cref="HybridAStarConfiguration.SearchTimeout"/> 限制。</summary>
|
||||
SearchTimeout,
|
||||
|
||||
/// <summary>搜索达到 <see cref="HybridAStarConfiguration.MaximumExpandedNodes"/> 限制。</summary>
|
||||
SearchNodeLimitExceeded,
|
||||
|
||||
/// <summary>Open List 已耗尽，或二维启发式证明目标不可达。</summary>
|
||||
NoFeasiblePath,
|
||||
|
||||
/// <summary>搜索成功节点无法按父链回溯为完整路径。</summary>
|
||||
BacktrackingFailed,
|
||||
|
||||
/// <summary>回溯路径未通过连续碰撞、终点或输出不变量复核。</summary>
|
||||
FinalValidationFailed,
|
||||
|
||||
/// <summary>规划内部发生未预期错误；不会发布部分路径。</summary>
|
||||
InternalError,
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 粗路径规划使用的二维连续位姿。
|
||||
/// 位置以世界坐标 m 表示，航向以 rad 表示；此值对象不在构造时归一化航向，调用方可保留展开航向。
|
||||
/// </summary>
|
||||
public sealed class Pose2D
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建二维位姿。
|
||||
/// 参数：xMeters、yMeters 为世界坐标，单位 m；headingRadians 为车头航向，单位 rad。
|
||||
/// </summary>
|
||||
public Pose2D(double xMeters, double yMeters, double headingRadians)
|
||||
{
|
||||
X = xMeters;
|
||||
Y = yMeters;
|
||||
Heading = headingRadians;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 坐标，单位 m。</summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>世界 Y 坐标，单位 m。</summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>车头航向，单位 rad；可以是已展开的连续航向。</summary>
|
||||
public double Heading { get; }
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>连续运动段的行驶方向。</summary>
|
||||
public enum TravelDirection
|
||||
{
|
||||
/// <summary>沿车辆车头方向前进。</summary>
|
||||
Forward,
|
||||
|
||||
/// <summary>与车辆车头方向相反地倒车。</summary>
|
||||
Reverse,
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 车辆几何与运动学参数。
|
||||
/// 所有几何尺寸均以车辆几何中心为 <see cref="Pose2D"/> 参考点，单位为 m；曲率单位为 1/m。
|
||||
/// </summary>
|
||||
public sealed class VehicleParameters
|
||||
{
|
||||
/// <summary>创建空车辆参数。调用规划器前必须填写有效的几何尺寸和至少一种曲率限制。</summary>
|
||||
public VehicleParameters()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>车辆本体长度，单位 m；不含 <see cref="SafetyMarginMeters"/>。</summary>
|
||||
public double LengthMeters { get; set; }
|
||||
|
||||
/// <summary>车辆本体宽度，单位 m；不含 <see cref="SafetyMarginMeters"/>。</summary>
|
||||
public double WidthMeters { get; set; }
|
||||
|
||||
/// <summary>碰撞检查时加在车体四周的安全余量，单位 m；不会写入地图。</summary>
|
||||
public double SafetyMarginMeters { get; set; }
|
||||
|
||||
/// <summary>车辆允许的最大绝对曲率，单位 1/m；null 表示由 <see cref="MinimumTurningRadiusMeters"/> 提供限制。</summary>
|
||||
public double? MaximumCurvaturePerMeter { get; set; }
|
||||
|
||||
/// <summary>车辆允许的最小转弯半径，单位 m；null 表示由 <see cref="MaximumCurvaturePerMeter"/> 提供限制。</summary>
|
||||
public double? MinimumTurningRadiusMeters { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 一次从地图输入到 Hybrid A* 粗路径输出的完整业务请求。
|
||||
/// 地图边界、分辨率和障碍物几何位于 <see cref="MapRequest"/> 中并使用 mm；位姿和车辆几何使用 m，航向使用 rad，曲率使用 1/m。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathPlanningJob
|
||||
{
|
||||
/// <summary>创建默认目标方向、起步曲率和关闭调试旁路的业务请求。</summary>
|
||||
public CoarsePathPlanningJob()
|
||||
{
|
||||
StartVehicleCurvature = 0d;
|
||||
GoalDirection = GoalDirectionConstraint.Any;
|
||||
DebugOptions = new PlanningDebugOptions();
|
||||
}
|
||||
|
||||
/// <summary>本次唯一建图输入；边界、分辨率和障碍物几何均遵循 Map 模块的 mm 契约。</summary>
|
||||
public PlanningMapRequest MapRequest { get; set; }
|
||||
|
||||
/// <summary>车辆几何中心的起始位姿；位置单位 m，航向单位 rad。</summary>
|
||||
public Pose2D Start { get; set; }
|
||||
|
||||
/// <summary>车辆几何中心的目标位姿；位置单位 m，航向单位 rad。</summary>
|
||||
public Pose2D Goal { get; set; }
|
||||
|
||||
/// <summary>车辆尺寸、安全余量和曲率限制；尺寸单位 m，曲率单位 1/m。</summary>
|
||||
public VehicleParameters Vehicle { get; set; }
|
||||
|
||||
/// <summary>原语、离散、容差、代价和资源上限配置。</summary>
|
||||
public HybridAStarConfiguration Configuration { get; set; }
|
||||
|
||||
/// <summary>车辆起步曲率，单位 1/m；默认值为 0。</summary>
|
||||
public double StartVehicleCurvature { get; set; }
|
||||
|
||||
/// <summary>起步方向约束；null 表示可从前进或倒车开始。</summary>
|
||||
public TravelDirection? StartDirection { get; set; }
|
||||
|
||||
/// <summary>目标进入方向约束；默认值为 <see cref="GoalDirectionConstraint.Any"/>。</summary>
|
||||
public GoalDirectionConstraint GoalDirection { get; set; }
|
||||
|
||||
/// <summary>可选调试旁路配置；默认关闭且使用空接收器，不参与地图或路径计算。</summary>
|
||||
public PlanningDebugOptions DebugOptions { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 一次粗路径业务编排的不可变结果。
|
||||
/// 始终同时保留地图构建结果和规划结果；调试旁路消息只用于诊断，不会修改地图或规划内容。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathPlanningJobResult
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建业务编排结果。
|
||||
/// 参数：mapResult 和 planningResult 均不能为空；debugDiagnostics 可为 null，返回时会转换为只读字符串集合。
|
||||
/// </summary>
|
||||
public CoarsePathPlanningJobResult(PlanningMapBuildResult mapResult, PlanningResult planningResult,
|
||||
IReadOnlyList<string> debugDiagnostics = null)
|
||||
{
|
||||
MapResult = mapResult ?? throw new ArgumentNullException(nameof(mapResult));
|
||||
PlanningResult = planningResult ?? throw new ArgumentNullException(nameof(planningResult));
|
||||
DebugDiagnostics = CopyDiagnostics(debugDiagnostics);
|
||||
}
|
||||
|
||||
/// <summary>本次调用的地图创建结果；失败时读取 <see cref="PlanningMapBuildResult.FailureReason"/>。</summary>
|
||||
public PlanningMapBuildResult MapResult { get; }
|
||||
|
||||
/// <summary>本次调用的粗路径结果；地图创建失败时为无路径的失败状态。</summary>
|
||||
public PlanningResult PlanningResult { get; }
|
||||
|
||||
/// <summary>调试旁路的非致命诊断信息；为空时表示未启用、未发生异常或无额外调试消息。</summary>
|
||||
public IReadOnlyList<string> DebugDiagnostics { get; }
|
||||
|
||||
private static IReadOnlyList<string> CopyDiagnostics(IReadOnlyList<string> source)
|
||||
{
|
||||
if (source == null || source.Count == 0)
|
||||
return new ReadOnlyCollection<string>(new List<string>());
|
||||
|
||||
var copy = new List<string>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index] ?? string.Empty);
|
||||
return new ReadOnlyCollection<string>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,108 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 从 <see cref="PlanningMapRequest"/> 到 Hybrid A* 粗路径的一次调用服务。
|
||||
/// 服务生命周期内长期持有同一个地图工厂和规划器，以保留地图缓存并避免将 UI、传感器或调试依赖带入规划核心。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathPlanningService
|
||||
{
|
||||
private readonly PlanningMapFactory _mapFactory;
|
||||
private readonly HybridAStarPlanner _planner;
|
||||
|
||||
/// <summary>创建长期复用默认地图工厂与 Hybrid A* 规划器的服务。</summary>
|
||||
public CoarsePathPlanningService()
|
||||
: this(new PlanningMapFactory(), new HybridAStarPlanner())
|
||||
{
|
||||
}
|
||||
|
||||
internal CoarsePathPlanningService(PlanningMapFactory mapFactory, HybridAStarPlanner planner)
|
||||
{
|
||||
_mapFactory = mapFactory ?? throw new ArgumentNullException(nameof(mapFactory));
|
||||
_planner = planner ?? throw new ArgumentNullException(nameof(planner));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按固定顺序创建地图并执行一次 Hybrid A* 粗路径规划。
|
||||
/// 参数：job 的地图输入使用 mm，位姿和车辆尺寸使用 m，航向使用 rad，曲率使用 1/m；cancellationToken 会传递给搜索阶段。
|
||||
/// 返回：始终同时保留地图创建结果和规划结果；地图创建失败时不会启动搜索，并返回 <see cref="PlanningStatus.InvalidMap"/> 的空路径结果。
|
||||
/// </summary>
|
||||
public CoarsePathPlanningJobResult Plan(CoarsePathPlanningJob job,
|
||||
CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
PlanningOperationBudget budget = CreateBudget(job, cancellationToken);
|
||||
PlanningMapBuildResult mapResult = _mapFactory.Create(job == null ? null : job.MapRequest, budget);
|
||||
if (!mapResult.Succeeded || mapResult.Map == null)
|
||||
{
|
||||
PlanningResult mapFailure = PlanningResult.Failure(MapFailureStatus(mapResult),
|
||||
new PlanningDiagnostics(elapsed: budget.Elapsed, terminationReason: BuildMapFailureReason(mapResult)));
|
||||
return PublishDebug(job, mapResult, mapFailure);
|
||||
}
|
||||
|
||||
var request = new PlanningRequest
|
||||
{
|
||||
Map = mapResult.Map,
|
||||
Start = job.Start,
|
||||
Goal = job.Goal,
|
||||
Vehicle = job.Vehicle,
|
||||
Configuration = job.Configuration,
|
||||
StartVehicleCurvature = job.StartVehicleCurvature,
|
||||
StartDirection = job.StartDirection,
|
||||
GoalDirection = job.GoalDirection,
|
||||
};
|
||||
PlanningResult planningResult = _planner.Plan(request, budget);
|
||||
return PublishDebug(job, mapResult, planningResult);
|
||||
}
|
||||
|
||||
private static PlanningOperationBudget CreateBudget(CoarsePathPlanningJob job, CancellationToken cancellationToken)
|
||||
{
|
||||
HybridAStarConfiguration configuration = job == null ? null : job.Configuration;
|
||||
return configuration != null && configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? new PlanningOperationBudget(cancellationToken, configuration.SearchTimeout)
|
||||
: PlanningOperationBudget.Unlimited(cancellationToken);
|
||||
}
|
||||
|
||||
private static PlanningStatus MapFailureStatus(PlanningMapBuildResult mapResult)
|
||||
{
|
||||
if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.Cancelled) return PlanningStatus.Cancelled;
|
||||
if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.TimedOut) return PlanningStatus.SearchTimeout;
|
||||
return PlanningStatus.InvalidMap;
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJobResult PublishDebug(CoarsePathPlanningJob job, PlanningMapBuildResult mapResult,
|
||||
PlanningResult planningResult)
|
||||
{
|
||||
var diagnostics = new List<string>();
|
||||
PlanningDebugOptions options = job == null ? null : job.DebugOptions;
|
||||
if (options != null && options.Enabled)
|
||||
{
|
||||
IPlanningDebugSink sink = options.Sink ?? NullPlanningDebugSink.Instance;
|
||||
try
|
||||
{
|
||||
sink.Publish(mapResult, planningResult);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
diagnostics.Add("调试旁路发布失败：" + exception.GetType().Name + "。" + exception.Message);
|
||||
}
|
||||
}
|
||||
|
||||
return new CoarsePathPlanningJobResult(mapResult, planningResult, diagnostics);
|
||||
}
|
||||
|
||||
private static string BuildMapFailureReason(PlanningMapBuildResult mapResult)
|
||||
{
|
||||
if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.Cancelled)
|
||||
return "规划地图创建已取消，未启动 Hybrid A* 搜索。";
|
||||
if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.TimedOut)
|
||||
return "规划地图创建已超时，未启动 Hybrid A* 搜索。";
|
||||
string reason = mapResult == null ? string.Empty : mapResult.FailureReason;
|
||||
return string.IsNullOrEmpty(reason) ? "规划地图创建失败，未启动 Hybrid A* 搜索。" :
|
||||
"规划地图创建失败，未启动 Hybrid A* 搜索：" + reason;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 接收一次地图构建和粗路径规划完成后的旁路调试数据。
|
||||
/// 实现不得修改输入对象；服务会隔离实现抛出的异常，调试失败不会改变规划结果。
|
||||
/// </summary>
|
||||
public interface IPlanningDebugSink
|
||||
{
|
||||
/// <summary>
|
||||
/// 发布本次编排得到的地图和规划结果。
|
||||
/// 参数：mapResult 为地图创建结果；planningResult 为对应的完整或失败规划结果；二者均不可由接收方修改。
|
||||
/// </summary>
|
||||
void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult);
|
||||
}
|
||||
|
||||
internal sealed class NullPlanningDebugSink : IPlanningDebugSink
|
||||
{
|
||||
internal static readonly NullPlanningDebugSink Instance = new NullPlanningDebugSink();
|
||||
|
||||
private NullPlanningDebugSink()
|
||||
{
|
||||
}
|
||||
|
||||
public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 一次粗路径编排的可选调试旁路配置。
|
||||
/// 调试开关和接收器不参与地图输入指纹、缓存键、搜索状态或路径结果。
|
||||
/// </summary>
|
||||
public sealed class PlanningDebugOptions
|
||||
{
|
||||
/// <summary>创建默认关闭并使用空接收器的调试配置。</summary>
|
||||
public PlanningDebugOptions()
|
||||
{
|
||||
Enabled = false;
|
||||
Sink = NullPlanningDebugSink.Instance;
|
||||
}
|
||||
|
||||
/// <summary>是否在本次编排结束后向 <see cref="Sink"/> 发布旁路数据；默认值为 false。</summary>
|
||||
public bool Enabled { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 接收地图和规划结果的旁路对象；默认为空实现。赋值为 null 时服务仍会使用空实现，且不会影响主流程。
|
||||
/// </summary>
|
||||
public IPlanningDebugSink Sink { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,264 @@
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using System.Globalization;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 已建 <see cref="PlanningGridMap"/> 上 Hybrid A* 粗路径规划的下层门面。
|
||||
/// 本类不建图、不读取 UI 或传感器；只有路径经回溯、装配和最终连续复核后才发布成功结果。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarPlanner
|
||||
{
|
||||
private readonly HybridAStarSearch _search;
|
||||
private readonly PathBacktracker _backtracker;
|
||||
private readonly CoarsePathAssembler _assembler;
|
||||
private readonly CoarsePathValidator _validator;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认搜索、回溯、装配和最终复核组件的规划器。</summary>
|
||||
public HybridAStarPlanner()
|
||||
: this(new HybridAStarSearch(), new PathBacktracker(), new CoarsePathAssembler(), new CoarsePathValidator(),
|
||||
new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
internal HybridAStarPlanner(HybridAStarSearch search, PathBacktracker backtracker, CoarsePathAssembler assembler,
|
||||
CoarsePathValidator validator, FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_search = search ?? throw new ArgumentNullException(nameof(search));
|
||||
_backtracker = backtracker ?? throw new ArgumentNullException(nameof(backtracker));
|
||||
_assembler = assembler ?? throw new ArgumentNullException(nameof(assembler));
|
||||
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在请求提供的不可变规划地图上执行一次 Hybrid A* 粗路径规划。
|
||||
/// 参数：request 的位置单位为 m、航向单位为 rad、曲率单位为 1/m；cancellationToken 会在搜索扩展检查点取消。
|
||||
/// 返回：输入、边界、碰撞、搜索、回溯或最终复核失败均返回空路径；只有 <see cref="PlanningStatus.Success"/> 携带完整路径和分段。
|
||||
/// </summary>
|
||||
public PlanningResult Plan(PlanningRequest request, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? new PlanningOperationBudget(cancellationToken, request.Configuration.SearchTimeout)
|
||||
: PlanningOperationBudget.Unlimited(cancellationToken);
|
||||
return Plan(request, budget);
|
||||
}
|
||||
|
||||
/// <summary>使用门面传入的共享预算执行预检、搜索和最终路径复核。</summary>
|
||||
internal PlanningResult Plan(PlanningRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
Stopwatch pathSearchStopwatch = null;
|
||||
try
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return Failure(ToPlanningStatus(stopReason), budget, "规划在开始前已停止。", null);
|
||||
|
||||
PlanningStatus preflightStatus = ValidatePreflight(request, out string preflightReason);
|
||||
if (preflightStatus != PlanningStatus.Success)
|
||||
return Failure(preflightStatus, budget, preflightReason, null);
|
||||
|
||||
if (!IsFootprintInsideMap(request.Start, request.Map, request.Vehicle))
|
||||
return Failure(PlanningStatus.StartOutsideMap, budget, "起始扩大车体不完全位于地图边界内。", null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Start, request.Map, request.Vehicle, 0d, out _))
|
||||
return Failure(PlanningStatus.StartInCollision, budget, "起始扩大车体与障碍物相交或擦边。", null);
|
||||
if (!IsFootprintInsideMap(request.Goal, request.Map, request.Vehicle))
|
||||
return Failure(PlanningStatus.GoalOutsideMap, budget, "目标扩大车体不完全位于地图边界内。", null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Goal, request.Map, request.Vehicle, 0d, out _))
|
||||
return Failure(PlanningStatus.GoalInCollision, budget, "目标扩大车体与障碍物相交或擦边。", null);
|
||||
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return Failure(ToPlanningStatus(stopReason), budget, "规划在搜索前已停止。", null);
|
||||
pathSearchStopwatch = Stopwatch.StartNew();
|
||||
HybridAStarSearchResult searchResult = _search.Search(request, budget);
|
||||
if (searchResult == null)
|
||||
return Failure(PlanningStatus.InternalError, budget, "搜索器未返回结果。", null, pathSearchStopwatch);
|
||||
if (searchResult.Status != PlanningStatus.Success)
|
||||
return Failure(searchResult.Status, budget,
|
||||
BuildSearchFailureReason(searchResult, request.Configuration), searchResult, pathSearchStopwatch);
|
||||
|
||||
if (!_backtracker.TryBacktrack(searchResult, request, out BacktrackedPath backtrackedPath, out string backtrackingReason))
|
||||
return Failure(PlanningStatus.BacktrackingFailed, budget, backtrackingReason, searchResult, pathSearchStopwatch);
|
||||
if (!_assembler.TryAssemble(backtrackedPath, request, out var path, out var segments, out string assemblyReason))
|
||||
return Failure(PlanningStatus.FinalValidationFailed, budget, assemblyReason, searchResult, pathSearchStopwatch);
|
||||
if (!_validator.TryValidate(path, segments, request, out double minimumClearanceMeters, out string validationReason))
|
||||
return Failure(PlanningStatus.FinalValidationFailed, budget, validationReason, searchResult, pathSearchStopwatch);
|
||||
|
||||
double pathLengthMeters = path[path.Count - 1].ArcLength;
|
||||
return PlanningResult.Success(path, segments, CreateDiagnostics(searchResult, budget.Elapsed, pathLengthMeters,
|
||||
minimumClearanceMeters, string.Empty, pathSearchStopwatch.Elapsed));
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
string reason = "规划内部错误：" + exception.GetType().Name +
|
||||
(string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
|
||||
return Failure(PlanningStatus.InternalError, budget ?? PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
reason, null, pathSearchStopwatch);
|
||||
}
|
||||
}
|
||||
|
||||
private static PlanningStatus ValidatePreflight(PlanningRequest request, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
|
||||
!IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
|
||||
!IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
|
||||
{
|
||||
failureReason = "规划请求缺少必要对象或包含非法数值。";
|
||||
return PlanningStatus.InvalidRequest;
|
||||
}
|
||||
|
||||
PlanningGridMap map = request.Map;
|
||||
if (map.Bounds == null || map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters))
|
||||
{
|
||||
failureReason = "规划地图结构无效。";
|
||||
return PlanningStatus.InvalidMap;
|
||||
}
|
||||
if (!map.PlanningReady)
|
||||
{
|
||||
failureReason = string.IsNullOrEmpty(map.PlanningBlockReason) ? "规划地图尚未就绪。" : map.PlanningBlockReason;
|
||||
return PlanningStatus.MapNotReady;
|
||||
}
|
||||
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
|
||||
!NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
{
|
||||
failureReason = "车辆尺寸、安全余量或曲率限制无效。";
|
||||
return PlanningStatus.InvalidVehicleParameters;
|
||||
}
|
||||
|
||||
HybridAStarConfiguration configuration = request.Configuration;
|
||||
if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
|
||||
(request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
|
||||
{
|
||||
failureReason = "Hybrid A* 曲率、离散、代价或资源配置无效。";
|
||||
return PlanningStatus.InvalidCurvatureConfiguration;
|
||||
}
|
||||
|
||||
return PlanningStatus.Success;
|
||||
}
|
||||
|
||||
private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
|
||||
{
|
||||
return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
|
||||
configuration.HeadingResolutionRadians <= 2d * Math.PI && configuration.CurvatureLevelCount >= 3 &&
|
||||
configuration.CurvatureLevelCount % 2 == 1 && NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) &&
|
||||
configuration.GoalPositionToleranceMeters >= 0d && NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) &&
|
||||
configuration.GoalHeadingToleranceRadians >= 0d && configuration.MaximumExpandedNodes >= 0 &&
|
||||
configuration.SearchTimeout >= TimeSpan.Zero && NumericGuard.IsFinite(configuration.HeuristicWeight) &&
|
||||
configuration.HeuristicWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
|
||||
NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) &&
|
||||
configuration.CurvatureChangePenaltyMetersPerLevel >= 0d && NumericGuard.IsFinite(configuration.ClearanceCostWeight) &&
|
||||
configuration.ClearanceCostWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
|
||||
}
|
||||
|
||||
private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
|
||||
{
|
||||
if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;
|
||||
|
||||
double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double longitudinalX = Math.Cos(pose.Heading);
|
||||
double longitudinalY = Math.Sin(pose.Heading);
|
||||
double lateralX = -longitudinalY;
|
||||
double lateralY = longitudinalX;
|
||||
for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
|
||||
for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
|
||||
{
|
||||
double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
|
||||
double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
|
||||
if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static string BuildSearchFailureReason(HybridAStarSearchResult searchResult,
|
||||
HybridAStarConfiguration configuration)
|
||||
{
|
||||
string reason = string.IsNullOrEmpty(searchResult.TerminationReason)
|
||||
? "Hybrid A* 搜索以 " + searchResult.Status + " 状态终止。"
|
||||
: searchResult.TerminationReason;
|
||||
string resourceLimit = string.Empty;
|
||||
if (searchResult.Status == PlanningStatus.SearchTimeout)
|
||||
{
|
||||
resourceLimit = "总预算=" + configuration.SearchTimeout.TotalSeconds.ToString(
|
||||
"F3", CultureInfo.InvariantCulture) + "s；";
|
||||
}
|
||||
else if (searchResult.Status == PlanningStatus.SearchNodeLimitExceeded)
|
||||
{
|
||||
resourceLimit = "节点上限=" + configuration.MaximumExpandedNodes.ToString(
|
||||
CultureInfo.InvariantCulture) + "；";
|
||||
}
|
||||
|
||||
return reason + resourceLimit +
|
||||
"扩展=" + searchResult.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||||
"生成=" + searchResult.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||||
"重开=" + searchResult.ReopenedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||||
"陈旧条目=" + searchResult.StaleOpenListEntryCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||||
"Open List峰值=" + searchResult.PeakOpenListCount.ToString(CultureInfo.InvariantCulture) + "。";
|
||||
}
|
||||
|
||||
private static PlanningResult Failure(PlanningStatus status, PlanningOperationBudget budget, string reason,
|
||||
HybridAStarSearchResult searchResult, Stopwatch pathSearchStopwatch = null)
|
||||
{
|
||||
TimeSpan pathSearchElapsed = pathSearchStopwatch == null ? TimeSpan.Zero : pathSearchStopwatch.Elapsed;
|
||||
return PlanningResult.Failure(status, CreateDiagnostics(searchResult, budget.Elapsed, 0d, 0d,
|
||||
reason, pathSearchElapsed));
|
||||
}
|
||||
|
||||
private static PlanningDiagnostics CreateDiagnostics(HybridAStarSearchResult searchResult, TimeSpan elapsed,
|
||||
double pathLengthMeters, double minimumClearanceMeters, string reason, TimeSpan pathSearchElapsed)
|
||||
{
|
||||
return new PlanningDiagnostics(
|
||||
searchResult == null ? 0 : searchResult.ExpandedNodeCount,
|
||||
searchResult == null ? 0 : searchResult.GeneratedNodeCount,
|
||||
searchResult == null ? 0 : searchResult.ReopenedNodeCount,
|
||||
searchResult == null ? 0 : searchResult.StaleOpenListEntryCount,
|
||||
searchResult == null ? 0 : searchResult.PeakOpenListCount,
|
||||
pathLengthMeters,
|
||||
minimumClearanceMeters,
|
||||
elapsed,
|
||||
reason,
|
||||
pathSearchElapsed);
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
|
||||
private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
|
||||
if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
|
||||
throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,216 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
|
||||
/// <summary>
|
||||
/// 将回溯原语装配为调用方可消费的稠密路径和包含式方向分段。
|
||||
/// 装配器保留原语边界的换向双点，其余相邻重复位姿会被删除。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathAssembler
|
||||
{
|
||||
private const double DuplicateTolerance = 1e-8d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认连续车体检查器的路径装配器。</summary>
|
||||
public CoarsePathAssembler()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车体检查器的路径装配器。</summary>
|
||||
public CoarsePathAssembler(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从已回溯的恒曲率原语构造稠密路径。
|
||||
/// 参数：backtrackedPath 提供原语顺序；request 提供地图和车辆；path、segments 为成功时的只读输出。
|
||||
/// 返回：首点可通过连续车体检查、每个原语积分点数据一致且方向分段完整覆盖时为 true；否则返回 false 且输出为空。
|
||||
/// </summary>
|
||||
public bool TryAssemble(BacktrackedPath backtrackedPath, PlanningRequest request,
|
||||
out IReadOnlyList<CoarsePathPoint> path, out IReadOnlyList<PathSegment> segments, out string failureReason)
|
||||
{
|
||||
path = EmptyPath();
|
||||
segments = EmptySegments();
|
||||
failureReason = string.Empty;
|
||||
if (backtrackedPath == null || request == null || request.Map == null || request.Vehicle == null ||
|
||||
!IsFinitePose(backtrackedPath.Start) || !IsTravelDirection(backtrackedPath.StartDirection) ||
|
||||
!NumericGuard.IsFinite(backtrackedPath.StartCurvaturePerMeter))
|
||||
{
|
||||
failureReason = "路径装配输入无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_collisionChecker.IsPoseCollisionFree(backtrackedPath.Start, request.Map, request.Vehicle, 0d, out double startClearanceMeters))
|
||||
{
|
||||
failureReason = "回溯路径起点未通过连续车体检查。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var points = new List<CoarsePathPoint>();
|
||||
TravelDirection currentDirection = backtrackedPath.Primitives.Count > 0
|
||||
? backtrackedPath.Primitives[0].Direction
|
||||
: backtrackedPath.StartDirection;
|
||||
double currentCurvature = request.StartVehicleCurvature;
|
||||
double normalizedStartHeading = AngleMath.NormalizeRadians(backtrackedPath.Start.Heading);
|
||||
if (!NumericGuard.IsFinite(normalizedStartHeading))
|
||||
{
|
||||
failureReason = "回溯路径起点航向无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
points.Add(new CoarsePathPoint(backtrackedPath.Start.X, backtrackedPath.Start.Y, normalizedStartHeading,
|
||||
backtrackedPath.Start.Heading, 0d, currentDirection, currentCurvature, startClearanceMeters, false,
|
||||
CoarsePathPointSource.Start));
|
||||
|
||||
for (int primitiveIndex = 0; primitiveIndex < backtrackedPath.Primitives.Count; primitiveIndex++)
|
||||
{
|
||||
MotionPrimitive primitive = backtrackedPath.Primitives[primitiveIndex];
|
||||
if (!IsValidPrimitive(primitive))
|
||||
{
|
||||
failureReason = "回溯路径包含无效原语。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint lastPoint = points[points.Count - 1];
|
||||
if (primitive.Direction != lastPoint.Direction)
|
||||
{
|
||||
// 换向处的旧方向终点和新方向起点必须共存，二者位置、航向、弧长完全相同。
|
||||
points.Add(new CoarsePathPoint(lastPoint.X, lastPoint.Y, lastPoint.Heading, lastPoint.UnwrappedHeading,
|
||||
lastPoint.ArcLength, primitive.Direction, primitive.CurvaturePerMeter, lastPoint.BodyClearance,
|
||||
true, CoarsePathPointSource.MotionPrimitive));
|
||||
}
|
||||
|
||||
Pose2D previousPose = primitive.Start;
|
||||
for (int pointIndex = 0; pointIndex < primitive.Points.Count; pointIndex++)
|
||||
{
|
||||
Pose2D pose = primitive.Points[pointIndex];
|
||||
double bodyClearanceMeters = primitive.BodyClearancesMeters[pointIndex];
|
||||
if (!IsFinitePose(pose) || !IsValidClearance(bodyClearanceMeters))
|
||||
{
|
||||
failureReason = "原语积分点或净空无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint previousPoint = points[points.Count - 1];
|
||||
double arcIncrementMeters = CalculateArcIncrement(previousPose, pose, primitive.CurvaturePerMeter);
|
||||
if (!NumericGuard.IsFinite(arcIncrementMeters) || arcIncrementMeters <= 0d)
|
||||
{
|
||||
failureReason = "原语积分点未产生正弧长。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (IsSamePose(previousPoint, pose))
|
||||
{
|
||||
// 非换向情况下不允许重复采样点泄露到对外路径。
|
||||
previousPose = pose;
|
||||
continue;
|
||||
}
|
||||
|
||||
double normalizedHeading = AngleMath.NormalizeRadians(pose.Heading);
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(previousPoint.Heading, normalizedHeading);
|
||||
if (!NumericGuard.IsFinite(normalizedHeading) || !NumericGuard.IsFinite(headingDelta))
|
||||
{
|
||||
failureReason = "原语积分点航向无法展开。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPointSource source = primitive.IsGoalTruncation && pointIndex == primitive.Points.Count - 1
|
||||
? CoarsePathPointSource.GoalTruncation
|
||||
: CoarsePathPointSource.MotionPrimitive;
|
||||
points.Add(new CoarsePathPoint(pose.X, pose.Y, normalizedHeading,
|
||||
previousPoint.UnwrappedHeading + headingDelta, previousPoint.ArcLength + arcIncrementMeters,
|
||||
primitive.Direction, primitive.CurvaturePerMeter, bodyClearanceMeters, false, source));
|
||||
previousPose = pose;
|
||||
}
|
||||
}
|
||||
|
||||
if (points.Count == 0)
|
||||
{
|
||||
failureReason = "路径装配未产生起点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
path = new ReadOnlyCollection<CoarsePathPoint>(points);
|
||||
segments = BuildSegments(points);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> BuildSegments(IReadOnlyList<CoarsePathPoint> points)
|
||||
{
|
||||
var segments = new List<PathSegment>();
|
||||
int startIndex = 0;
|
||||
TravelDirection direction = points[0].Direction;
|
||||
for (int index = 1; index < points.Count; index++)
|
||||
{
|
||||
if (points[index].Direction == direction) continue;
|
||||
segments.Add(new PathSegment(segments.Count, direction, startIndex, index - 1,
|
||||
points[startIndex].IsGearSwitchPoint, true));
|
||||
startIndex = index;
|
||||
direction = points[index].Direction;
|
||||
}
|
||||
|
||||
segments.Add(new PathSegment(segments.Count, direction, startIndex, points.Count - 1,
|
||||
points[startIndex].IsGearSwitchPoint, false));
|
||||
return new ReadOnlyCollection<PathSegment>(segments);
|
||||
}
|
||||
|
||||
private static double CalculateArcIncrement(Pose2D from, Pose2D to, double curvaturePerMeter)
|
||||
{
|
||||
if (!IsFinitePose(from) || !IsFinitePose(to) || !NumericGuard.IsFinite(curvaturePerMeter)) return double.NaN;
|
||||
if (Math.Abs(curvaturePerMeter) < 1e-12d)
|
||||
{
|
||||
double deltaX = to.X - from.X;
|
||||
double deltaY = to.Y - from.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
|
||||
return Math.Abs(headingDelta / curvaturePerMeter);
|
||||
}
|
||||
|
||||
private static bool IsValidPrimitive(MotionPrimitive primitive)
|
||||
{
|
||||
return primitive != null && primitive.Start != null && primitive.Points != null && primitive.BodyClearancesMeters != null &&
|
||||
primitive.Points.Count == primitive.BodyClearancesMeters.Count && primitive.Points.Count > 0 &&
|
||||
IsTravelDirection(primitive.Direction) && NumericGuard.IsFinite(primitive.CurvaturePerMeter) &&
|
||||
NumericGuard.IsPositiveFinite(primitive.ActualLengthMeters);
|
||||
}
|
||||
|
||||
private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
|
||||
{
|
||||
return Math.Abs(point.X - pose.X) <= DuplicateTolerance && Math.Abs(point.Y - pose.Y) <= DuplicateTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= DuplicateTolerance;
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsValidClearance(double clearanceMeters)
|
||||
{
|
||||
return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CoarsePathPoint> EmptyPath()
|
||||
{
|
||||
return new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> EmptySegments()
|
||||
{
|
||||
return new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,245 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
|
||||
/// <summary>
|
||||
/// 对准备发布的粗路径执行独立的连续安全与输出契约复核。
|
||||
/// 复核失败的路径不得被包装为 <see cref="PlanningStatus.Success"/>。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathValidator
|
||||
{
|
||||
private const double NumericTolerance = 1e-6d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认连续车体检查器的最终路径复核器。</summary>
|
||||
public CoarsePathValidator()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车体检查器的最终路径复核器。</summary>
|
||||
public CoarsePathValidator(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复核路径数值、曲率、连续扫掠碰撞、终点约束、累计弧长和方向分段。
|
||||
/// 参数：path 与 segments 为待发布输出；request 必须是生成该路径的请求；minimumBodyClearanceMeters 返回沿途的保守净空下界。
|
||||
/// 返回：所有规则通过时为 true；否则返回 false、写入失败原因，调用方必须丢弃 path 和 segments。
|
||||
/// </summary>
|
||||
public bool TryValidate(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments,
|
||||
PlanningRequest request, out double minimumBodyClearanceMeters, out string failureReason)
|
||||
{
|
||||
minimumBodyClearanceMeters = 0d;
|
||||
failureReason = string.Empty;
|
||||
if (path == null || segments == null || request == null || request.Map == null || request.Vehicle == null ||
|
||||
request.Configuration == null || path.Count == 0 || segments.Count == 0)
|
||||
{
|
||||
failureReason = "最终路径或复核请求为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvaturePerMeter))
|
||||
{
|
||||
failureReason = "车辆曲率约束无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint first = path[0];
|
||||
if (!IsValidPoint(first) || first.Source != CoarsePathPointSource.Start || first.IsGearSwitchPoint ||
|
||||
Math.Abs(first.ArcLength) > NumericTolerance || !IsSamePose(first, request.Start))
|
||||
{
|
||||
failureReason = "最终路径首点不符合起点契约。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double minimumClearance = double.PositiveInfinity;
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint current = path[index];
|
||||
if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvaturePerMeter + NumericTolerance)
|
||||
{
|
||||
failureReason = "最终路径包含非法数值或超限曲率。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var currentPose = new Pose2D(current.X, current.Y, current.Heading);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(currentPose, request.Map, request.Vehicle, 0d, out double poseClearanceMeters) ||
|
||||
IsClearanceOverclaimed(current.BodyClearance, poseClearanceMeters))
|
||||
{
|
||||
failureReason = "最终路径点未通过连续车体碰撞复核。";
|
||||
return false;
|
||||
}
|
||||
|
||||
minimumClearance = Math.Min(minimumClearance, current.BodyClearance);
|
||||
if (index == 0) continue;
|
||||
|
||||
CoarsePathPoint previous = path[index - 1];
|
||||
if (current.ArcLength + NumericTolerance < previous.ArcLength ||
|
||||
!IsUnwrappedHeadingContinuous(previous, current))
|
||||
{
|
||||
failureReason = "最终路径的弧长或展开航向不连续。";
|
||||
return false;
|
||||
}
|
||||
|
||||
bool isDuplicate = IsDuplicatePoseAndArcLength(previous, current);
|
||||
if (isDuplicate)
|
||||
{
|
||||
if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
|
||||
{
|
||||
failureReason = "相邻重复点不是合法换向对。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + NumericTolerance ||
|
||||
!IsArcIncrementConsistent(previous, current))
|
||||
{
|
||||
failureReason = "非换向路径点的弧长增量不一致。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
|
||||
if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, request.Map, request.Vehicle,
|
||||
request.Configuration.MaximumCollisionCheckStepMeters, out double sweptClearanceMeters))
|
||||
{
|
||||
failureReason = "最终路径相邻点之间的车体扫掠碰撞复核失败。";
|
||||
return false;
|
||||
}
|
||||
|
||||
minimumClearance = Math.Min(minimumClearance, sweptClearanceMeters);
|
||||
}
|
||||
|
||||
CoarsePathPoint last = path[path.Count - 1];
|
||||
if (!GoalToleranceChecker.IsSatisfied(new Pose2D(last.X, last.Y, last.Heading), request.Goal, request.Configuration,
|
||||
last.Direction, request.GoalDirection) || (path.Count > 1 && last.Source != CoarsePathPointSource.GoalTruncation))
|
||||
{
|
||||
failureReason = "最终路径末点未满足终点容差、方向或来源契约。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!AreSegmentsValid(path, segments, out failureReason)) return false;
|
||||
minimumBodyClearanceMeters = minimumClearance;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool AreSegmentsValid(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
int expectedStartIndex = 0;
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment segment = segments[segmentIndex];
|
||||
if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
|
||||
segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
|
||||
segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
|
||||
{
|
||||
failureReason = "方向分段索引或起始换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
|
||||
{
|
||||
if (path[pointIndex].Direction != segment.Direction)
|
||||
{
|
||||
failureReason = "方向分段包含不同方向的路径点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool hasNextSegment = segmentIndex + 1 < segments.Count;
|
||||
bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
|
||||
path[segment.EndIndex + 1].IsGearSwitchPoint;
|
||||
if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
|
||||
{
|
||||
failureReason = "方向分段末尾换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
expectedStartIndex = segment.EndIndex + 1;
|
||||
}
|
||||
|
||||
if (expectedStartIndex != path.Count)
|
||||
{
|
||||
failureReason = "方向分段未完整覆盖路径点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(CoarsePathPoint point)
|
||||
{
|
||||
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
|
||||
!NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!NumericGuard.IsFinite(point.VehicleCurvature) || !IsTravelDirection(point.Direction) ||
|
||||
!IsValidClearance(point.BodyClearance))
|
||||
return false;
|
||||
|
||||
return Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
|
||||
{
|
||||
return point != null && pose != null && Math.Abs(point.X - pose.X) <= NumericTolerance &&
|
||||
Math.Abs(point.Y - pose.Y) <= NumericTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsUnwrappedHeadingContinuous(CoarsePathPoint previous, CoarsePathPoint current)
|
||||
{
|
||||
double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
||||
return NumericGuard.IsFinite(expectedDelta) &&
|
||||
Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsDuplicatePoseAndArcLength(CoarsePathPoint previous, CoarsePathPoint current)
|
||||
{
|
||||
return Math.Abs(previous.X - current.X) <= NumericTolerance && Math.Abs(previous.Y - current.Y) <= NumericTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= NumericTolerance &&
|
||||
Math.Abs(previous.ArcLength - current.ArcLength) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsArcIncrementConsistent(CoarsePathPoint previous, CoarsePathPoint current)
|
||||
{
|
||||
double expectedIncrement;
|
||||
if (Math.Abs(current.VehicleCurvature) < 1e-12d)
|
||||
{
|
||||
double deltaX = current.X - previous.X;
|
||||
double deltaY = current.Y - previous.Y;
|
||||
expectedIncrement = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
else
|
||||
{
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
||||
expectedIncrement = Math.Abs(headingDelta / current.VehicleCurvature);
|
||||
}
|
||||
|
||||
return NumericGuard.IsFinite(expectedIncrement) && expectedIncrement > 0d &&
|
||||
Math.Abs((current.ArcLength - previous.ArcLength) - expectedIncrement) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsClearanceOverclaimed(double reportedClearanceMeters, double checkedClearanceMeters)
|
||||
{
|
||||
if (double.IsPositiveInfinity(reportedClearanceMeters)) return !double.IsPositiveInfinity(checkedClearanceMeters);
|
||||
return reportedClearanceMeters > checkedClearanceMeters + NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsValidClearance(double clearanceMeters)
|
||||
{
|
||||
return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,167 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
|
||||
/// <summary>
|
||||
/// 成功搜索父链重新生成后的原语序列。
|
||||
/// 起点位置使用 m/rad，起始曲率使用 1/m；原语集合按从起点到终点的顺序排列。
|
||||
/// </summary>
|
||||
public sealed class BacktrackedPath
|
||||
{
|
||||
internal BacktrackedPath(Pose2D start, TravelDirection startDirection, double startCurvaturePerMeter,
|
||||
IReadOnlyList<MotionPrimitive> primitives)
|
||||
{
|
||||
Start = start;
|
||||
StartDirection = startDirection;
|
||||
StartCurvaturePerMeter = startCurvaturePerMeter;
|
||||
Primitives = primitives;
|
||||
}
|
||||
|
||||
/// <summary>原始请求中的起始车辆中心位姿；位置单位 m，航向单位 rad。</summary>
|
||||
public Pose2D Start { get; }
|
||||
|
||||
/// <summary>成功父链根节点记录的起步方向。</summary>
|
||||
public TravelDirection StartDirection { get; }
|
||||
|
||||
/// <summary>成功父链根节点离散后的起始曲率，单位 1/m。</summary>
|
||||
public double StartCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>按起点到终点顺序重新积分的恒曲率原语；每项都不包含自身起点。</summary>
|
||||
public IReadOnlyList<MotionPrimitive> Primitives { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 仅依据成功节点的父索引和原语描述，确定性地重建稠密原语序列。
|
||||
/// 不复用搜索期保存的积分点，以保证输出与当前解析积分、扫掠检查规则一致。
|
||||
/// </summary>
|
||||
public sealed class PathBacktracker
|
||||
{
|
||||
private const double PoseComparisonTolerance = 1e-7d;
|
||||
private const double LengthComparisonTolerance = 1e-7d;
|
||||
private readonly MotionPrimitiveGenerator _primitiveGenerator;
|
||||
|
||||
/// <summary>创建使用默认解析积分器的回溯器。</summary>
|
||||
public PathBacktracker()
|
||||
: this(new MotionPrimitiveGenerator())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定原语生成器的回溯器。</summary>
|
||||
public PathBacktracker(MotionPrimitiveGenerator primitiveGenerator)
|
||||
{
|
||||
_primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 依据搜索成功节点重建从起点到终点的原语序列。
|
||||
/// 参数：searchResult 必须是携带成功节点索引的搜索结果；request 必须仍指向执行搜索的同一不可变地图和配置。
|
||||
/// 返回：父链无环、索引连续且每条原语按同一解析规则可重建时为 true；失败时 path 为 null 并返回可读原因。
|
||||
/// </summary>
|
||||
public bool TryBacktrack(HybridAStarSearchResult searchResult, PlanningRequest request,
|
||||
out BacktrackedPath path, out string failureReason)
|
||||
{
|
||||
path = null;
|
||||
failureReason = string.Empty;
|
||||
if (searchResult == null || request == null || searchResult.Status != PlanningStatus.Success ||
|
||||
!searchResult.SuccessNodeIndex.HasValue || searchResult.Nodes == null)
|
||||
{
|
||||
failureReason = "搜索结果不含可回溯的成功节点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
int currentIndex = searchResult.SuccessNodeIndex.Value;
|
||||
var reverseNodes = new List<HybridAStarNode>();
|
||||
var visited = new HashSet<int>();
|
||||
while (currentIndex >= 0)
|
||||
{
|
||||
if (currentIndex >= searchResult.Nodes.Count || !visited.Add(currentIndex))
|
||||
{
|
||||
failureReason = "搜索父链索引越界或存在环。";
|
||||
return false;
|
||||
}
|
||||
|
||||
HybridAStarNode current = searchResult.Nodes[currentIndex];
|
||||
if (current == null || current.NodeIndex != currentIndex || current.Pose == null)
|
||||
{
|
||||
failureReason = "搜索节点索引或连续位姿不一致。";
|
||||
return false;
|
||||
}
|
||||
|
||||
reverseNodes.Add(current);
|
||||
currentIndex = current.ParentNodeIndex;
|
||||
}
|
||||
|
||||
if (reverseNodes.Count == 0)
|
||||
{
|
||||
failureReason = "搜索父链为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
reverseNodes.Reverse();
|
||||
HybridAStarNode root = reverseNodes[0];
|
||||
if (root.ParentNodeIndex != -1 || root.IncomingPrimitive != null || !IsFinitePose(root.Pose))
|
||||
{
|
||||
failureReason = "搜索父链根节点无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var primitives = new List<MotionPrimitive>(Math.Max(0, reverseNodes.Count - 1));
|
||||
Pose2D previousPose = root.Pose;
|
||||
for (int index = 1; index < reverseNodes.Count; index++)
|
||||
{
|
||||
HybridAStarNode node = reverseNodes[index];
|
||||
MotionPrimitive descriptor = node.IncomingPrimitive;
|
||||
if (node.ParentNodeIndex != reverseNodes[index - 1].NodeIndex || descriptor == null ||
|
||||
!IsFinitePose(node.Pose) || !NumericGuard.IsFinite(descriptor.CurvaturePerMeter) ||
|
||||
!IsTravelDirection(descriptor.Direction) || descriptor.ActualLengthMeters <= 0d)
|
||||
{
|
||||
failureReason = "搜索父链中的原语描述无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
// 只把恒曲率、方向和长度描述作为真源，再走一遍相同的解析积分与连续碰撞检查。
|
||||
MotionPrimitive rebuilt = _primitiveGenerator.Generate(previousPose, descriptor.CurvaturePerMeter, descriptor.Direction, request);
|
||||
if (rebuilt == null || !IsEquivalent(descriptor, rebuilt) || !IsSamePose(rebuilt.End, node.Pose))
|
||||
{
|
||||
failureReason = "搜索原语无法按当前解析规则确定性重建。";
|
||||
return false;
|
||||
}
|
||||
|
||||
primitives.Add(rebuilt);
|
||||
previousPose = rebuilt.End;
|
||||
}
|
||||
|
||||
path = new BacktrackedPath(root.Pose, root.Direction, root.CurvaturePerMeter,
|
||||
new ReadOnlyCollection<MotionPrimitive>(primitives));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsEquivalent(MotionPrimitive expected, MotionPrimitive actual)
|
||||
{
|
||||
return expected.Direction == actual.Direction && expected.IsGoalTruncation == actual.IsGoalTruncation &&
|
||||
Math.Abs(expected.CurvaturePerMeter - actual.CurvaturePerMeter) <= PoseComparisonTolerance &&
|
||||
Math.Abs(expected.ActualLengthMeters - actual.ActualLengthMeters) <= LengthComparisonTolerance;
|
||||
}
|
||||
|
||||
private static bool IsSamePose(Pose2D first, Pose2D second)
|
||||
{
|
||||
return IsFinitePose(first) && IsFinitePose(second) &&
|
||||
Math.Abs(first.X - second.X) <= PoseComparisonTolerance &&
|
||||
Math.Abs(first.Y - second.Y) <= PoseComparisonTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(first.Heading, second.Heading)) <= PoseComparisonTolerance;
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,387 @@
|
||||
# CoarsePath 粗路径规划（P0/P1）
|
||||
|
||||
`CoarsePath` 在 `Map` 提供的不可变 `PlanningGridMap` 上执行 Hybrid A*，输出已经过连续碰撞、终点和输出不变量复核的粗路径。它只处理“能否安全地从当前车辆几何中心到达目标”的几何搜索；不读取传感器或定位，不绘制 UI，也不向底盘发送任何命令。
|
||||
|
||||
地图障碍物来源、世界坐标栅格化、距离场和缓存细节由 [Map/README.md](../Map/README.md) 说明。本模块唯一建议的业务调用入口是：
|
||||
|
||||
```csharp
|
||||
CoarsePathPlanningService.Plan(job, cancellationToken)
|
||||
```
|
||||
|
||||
## 模块说明（Module Overview）
|
||||
|
||||
| 模块 | 负责内容 | 不负责内容 |
|
||||
| --- | --- | --- |
|
||||
| `Map` | 外部障碍物快照、占据栅格、保守障碍距离、地图缓存 | 车辆足迹、运动原语、路径搜索、控制 |
|
||||
| `CoarsePath` | 车辆扩大足迹、连续碰撞检查、前进/倒车原语、Hybrid A*、路径复核与方向分段 | 传感器读取、定位读取、速度规划、路径跟踪、底盘命令 |
|
||||
| `Facade` | 将建图、搜索、取消、总预算和可选调试旁路编排为一次调用 | 修改地图内容、写入 AMR 占据、执行轨迹 |
|
||||
| `Test` | 固定回归场景、P1 手动测试入口与规划结果可视化 | 真实作业地图、实时重规划或车辆控制 |
|
||||
|
||||
安全余量只由 `VehicleParameters.SafetyMarginMeters` 扩大车辆矩形。它不会回写到地图障碍物，因此同一个 `PlanningGridMap` 可由不同车辆参数重复使用。
|
||||
|
||||
## 文件结构（File Structure）
|
||||
|
||||
```text
|
||||
CoarsePath/
|
||||
├── README.md # 本模块说明：结构、数据流、调用与测试
|
||||
├── HybridAStarPlanner.cs # 公开规划门面后的核心编排：校验、搜索、回溯和复核
|
||||
├── Contracts/
|
||||
│ ├── Pose2D.cs # 车辆几何中心位姿：m / rad
|
||||
│ ├── PlanningRequest.cs # 已有 PlanningGridMap 上的一次内部搜索请求
|
||||
│ ├── PlanningResult.cs # 不可变规划结果：成功路径或空结果
|
||||
│ ├── PlanningStatus.cs # 成功、取消、无解、输入和资源限制状态
|
||||
│ ├── CoarsePathPoint.cs # 稠密路径点、方向、曲率、净空和换向标记
|
||||
│ ├── PathSegment.cs # 前进或倒车的包含式路径索引段
|
||||
│ ├── VehicleParameters.cs # 车体尺寸、安全余量和曲率限制
|
||||
│ ├── HybridAStarConfiguration.cs # 原语、离散、代价、容差和资源上限
|
||||
│ └── GoalDirectionConstraint.cs # 目标进入方向约束
|
||||
├── Vehicle/
|
||||
│ ├── VehicleKinematics.cs # 恒曲率车辆运动学积分
|
||||
│ ├── VehicleFootprint.cs # 扩大后的车辆矩形几何
|
||||
│ ├── OrientedRectangleCellIntersection.cs # 旋转矩形与占据格相交判定
|
||||
│ └── FootprintCollisionChecker.cs # 连续扫掠的足迹碰撞检查
|
||||
├── Search/
|
||||
│ ├── BinaryMinHeap.cs # 可更新优先级的 Open List
|
||||
│ ├── GridDijkstraHeuristic.cs # 二维栅格可达性和距离启发式
|
||||
│ ├── GoalToleranceChecker.cs # 目标位置、航向和方向约束判定
|
||||
│ ├── MotionPrimitive.cs # 单个前进或倒车恒曲率原语
|
||||
│ ├── MotionPrimitiveGenerator.cs # 原语离散与连续积分点生成
|
||||
│ ├── SearchCostCalculator.cs # 长度、倒车、换向、曲率和净空代价
|
||||
│ ├── HybridAStarNode.cs # 搜索节点与父链信息
|
||||
│ ├── HybridAStarNodeKey.cs # 离散状态键
|
||||
│ └── HybridAStarSearch.cs # Hybrid A* 主搜索循环
|
||||
├── Output/
|
||||
│ ├── PathBacktracker.cs # 从终点节点安全回溯父链
|
||||
│ ├── CoarsePathAssembler.cs # 组装稠密路径和方向段
|
||||
│ └── CoarsePathValidator.cs # 对最终输出重新进行连续复核
|
||||
├── Facade/
|
||||
│ ├── CoarsePathPlanningJob.cs # 一次完整业务输入：地图请求、位姿、车辆、配置
|
||||
│ ├── CoarsePathPlanningJobResult.cs # 同时包含地图结果和规划结果的不可变输出
|
||||
│ ├── CoarsePathPlanningService.cs # 唯一业务调用门面
|
||||
│ ├── PlanningDebugOptions.cs # 可选调试旁路配置
|
||||
│ └── IPlanningDebugSink.cs # 调试旁路接收器契约
|
||||
└── Test/
|
||||
├── CoarsePathScenarioFactory.cs # 六个固定场景和手动目标演示请求工厂
|
||||
└── MovementTest.CoarsePathTest.cs # 七个 Clumsy 入口、后台取消和 Painter 绘制
|
||||
```
|
||||
|
||||
## 规划数据流（Planning Data Flow）
|
||||
|
||||
```text
|
||||
CoarsePathPlanningJob
|
||||
│ MapRequest 使用 mm；Pose2D/车辆使用 m、rad
|
||||
▼
|
||||
CoarsePathPlanningService.Plan(job, cancellationToken)
|
||||
│
|
||||
├── PlanningMapFactory.Create(job.MapRequest)
|
||||
│ │
|
||||
│ ├── 失败、取消或超时
|
||||
│ │ └── MapResult + 空路径 PlanningResult，搜索不启动
|
||||
│ │
|
||||
│ └── 成功：不可变 PlanningGridMap
|
||||
▼
|
||||
PlanningRequest
|
||||
│
|
||||
▼
|
||||
HybridAStarPlanner
|
||||
├── 车辆扩大足迹与连续碰撞检查
|
||||
├── GridDijkstraHeuristic + Hybrid A* 搜索
|
||||
├── PathBacktracker + CoarsePathAssembler
|
||||
└── CoarsePathValidator 最终复核
|
||||
▼
|
||||
PlanningResult + MapResult
|
||||
▼
|
||||
CoarsePathPlanningJobResult
|
||||
```
|
||||
|
||||
调用方只创建 `CoarsePathPlanningJob` 并消费 `CoarsePathPlanningJobResult`。`PlanningRequest`、`HybridAStarPlanner`、原语和碰撞检查器属于模块内部协作对象，不应由 UI、传感器或 MovementTest 直接拼接。
|
||||
|
||||
## 构建状态与停止（Build Status and Stop）
|
||||
|
||||
必须一起处理 `MapResult` 和 `PlanningResult`。`MapResult.Status` 的类型是 `PlanningMapBuildStatus`；地图失败时，门面返回对应的空路径结果，并且不会启动 Hybrid A*。
|
||||
|
||||
| 情况 | `MapResult` | `PlanningResult` | 调用方处理 |
|
||||
| --- | --- | --- | --- |
|
||||
| 地图和搜索成功 | `Success` 且 `Map` 非空 | `Success`，发布完整路径和方向段 | 消费粗路径；后续模块仍需自行进行平滑、速度和控制 |
|
||||
| 地图输入/来源失败 | `Failed` | `InvalidMap` | 读取 `FailureReason`，修复地图输入 |
|
||||
| 调用被取消 | `Cancelled` | `Cancelled` | 不重试为普通无解；不会发布地图或部分路径 |
|
||||
| 总预算耗尽 | `TimedOut` | `SearchTimeout` | 根据上层策略调整预算或稍后重试 |
|
||||
| 搜索无解 | 地图成功 | `NoFeasiblePath` | 当前地图、车体和运动约束下无可行路径 |
|
||||
| 节点或搜索资源受限 | 地图成功 | `SearchNodeLimitExceeded` 或 `SearchTimeout` | 读取诊断后调整配置或上层策略 |
|
||||
|
||||
除 `PlanningStatus.Success` 外，`PlanningResult.Path` 与 `PlanningResult.Segments` 始终为空。取消、超时、无解、输入错误和最终复核失败都不能作为“部分可执行路径”使用。
|
||||
|
||||
### 总预算与取消
|
||||
|
||||
`HybridAStarConfiguration.SearchTimeout` 是从门面开始计时的一次总预算，依次覆盖建图、距离场、二维启发式和 Hybrid A*。同一个 `CancellationToken` 会沿调用链传递，取消优先于超时。
|
||||
|
||||
### 总耗时与路径搜索耗时
|
||||
|
||||
`PlanningDiagnostics.Elapsed` 是从 `CoarsePathPlanningService.Plan` 开始的总耗时，包含地图来源、缓存、栅格化、距离场和路径规划。`PathSearchElapsed`（路径搜索耗时）从地图和起终点预检通过后开始，包含二维启发式、Hybrid A*、回溯、装配、方向分段和最终复核；搜索开始前失败时为零。
|
||||
|
||||
## 坐标与单位（Coordinates and Units）
|
||||
|
||||
| 数据 | 单位 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `PlanningMapRequest.Bounds`、分辨率、障碍物几何 | mm | 来自 Map 的世界坐标；边界采用 `[min, max)` |
|
||||
| `Pose2D.X`、`Pose2D.Y`、路径位置、车辆尺寸、安全余量、弧长 | m | CoarsePath 的连续世界坐标和长度 |
|
||||
| `Pose2D.Heading`、航向容差 | rad | 核心一律使用弧度 |
|
||||
| 曲率、起步曲率 | 1/m | 最大曲率或最小转弯半径至少提供一个 |
|
||||
| `PlanningGridMap` 世界查询参数 | m | 越界位置按占据处理，净距为 0 |
|
||||
| P1 的 AMR/手动目标 X/Y | mm | 仅在 UI 边界读取，进入核心前除以 1000 |
|
||||
| P1 的 AMR/手动目标航向 | deg | 仅在 UI 边界转换为 `deg * PI / 180 -> rad` |
|
||||
|
||||
起点和终点都表示车辆**几何中心**。若上游定位参考点是雷达、天线或其他安装点，必须先在上游应用安装外参；不要在 CoarsePath 内猜测偏移。车辆外扩由 `VehicleParameters.SafetyMarginMeters` 表达，不要把余量写入 Map 障碍物。
|
||||
|
||||
## 最小调用示例（Minimal Call Example）
|
||||
|
||||
以下示例明确允许空图，因而只适合算法或单位演示。真实作业必须通过 `IMapObstacleSource` 提供有效障碍物快照；如何构造来源请阅读 [Map/README.md](../Map/README.md)。
|
||||
|
||||
```csharp
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
var service = new CoarsePathPlanningService(); // 长期持有，保留地图缓存
|
||||
|
||||
var job = new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = Array.Empty<IMapObstacleSource>(),
|
||||
AllowExplicitEmptyMap = true, // 仅演示时明确允许
|
||||
},
|
||||
Start = new Pose2D(1d, 1d, 0d),
|
||||
Goal = new Pose2D(3d, 1d, 0d),
|
||||
Vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.80d,
|
||||
WidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
},
|
||||
Configuration = new HybridAStarConfiguration(),
|
||||
GoalDirection = GoalDirectionConstraint.Forward,
|
||||
};
|
||||
|
||||
CoarsePathPlanningJobResult result =
|
||||
service.Plan(job, CancellationToken.None);
|
||||
|
||||
if (!result.MapResult.Succeeded)
|
||||
throw new InvalidOperationException(result.MapResult.FailureReason);
|
||||
|
||||
if (result.PlanningResult.Status != PlanningStatus.Success)
|
||||
throw new InvalidOperationException(
|
||||
result.PlanningResult.Diagnostics.TerminationReason);
|
||||
|
||||
foreach (CoarsePathPoint point in result.PlanningResult.Path)
|
||||
Console.WriteLine(point.X + "," + point.Y + "," + point.Heading);
|
||||
```
|
||||
|
||||
## 缓存与 SourceVersion（Cache and SourceVersion）
|
||||
|
||||
`CoarsePathPlanningService` 在生命周期内长期持有 `PlanningMapFactory`，因此重复调用时能够复用地图缓存。不要每次规划都新建服务，否则会失去缓存收益。
|
||||
|
||||
| 缓存层级 | 条件 | 结果 |
|
||||
| --- | --- | --- |
|
||||
| `Input` | 边界、分辨率、空图策略、来源 ID、`SourceVersion`、必需性和来源结果相同 | 返回同一个不可变 `PlanningGridMap` |
|
||||
| `Occupancy` | 输入版本变化，但最终占据栅格相同 | 复用占据/距离数组，生成新的快照元数据 |
|
||||
| `None` | 占据内容变化 | 重建规划快照和距离场 |
|
||||
|
||||
障碍来源内容改变时，调用方必须递增该来源的 `SourceVersion`。仅修改起终点、车辆、搜索配置、调试开关或调试接收器不会改变地图输入；改变障碍物却不递增版本则可能错误复用旧快照。
|
||||
|
||||
## 详细使用指南（Detailed Usage Guide）
|
||||
|
||||
本节说明调用方如何从一个地图输入得到可消费的粗路径。所有业务调用都通过 `CoarsePathPlanningService.Plan(job, cancellationToken)` 完成。
|
||||
|
||||
### 第 1 步：长期持有服务
|
||||
|
||||
服务持有地图工厂和规划器，应该作为规划业务、任务执行器或上层服务的长期字段，而不是在每次调用中创建：
|
||||
|
||||
```csharp
|
||||
private readonly CoarsePathPlanningService _coarsePathService =
|
||||
new CoarsePathPlanningService();
|
||||
```
|
||||
|
||||
### 第 2 步：准备地图请求
|
||||
|
||||
创建 `PlanningMapRequest`，其边界、分辨率和障碍物仍使用 mm。使用手工圆形/矩形或 TwoLeg 快照时，应先按 [Map/README.md](../Map/README.md) 将它们包装为 `IMapObstacleSource`，并为内容变化递增 `SourceVersion`。
|
||||
|
||||
```csharp
|
||||
var mapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = sources,
|
||||
AllowExplicitEmptyMap = false,
|
||||
};
|
||||
```
|
||||
|
||||
`AllowExplicitEmptyMap = true` 只在调用方明确确认空地图安全时使用。未提供有效障碍物且未显式允许空图时，地图不会进入规划。
|
||||
|
||||
### 第 3 步：填写起点、终点和方向约束
|
||||
|
||||
将车辆几何中心的世界 X/Y 从 mm 转为 m，并将航向转换为 rad 后创建 `Pose2D`。`StartDirection = null` 表示允许从前进或倒车开始；`GoalDirection` 可以限制最终进入目标的方向。
|
||||
|
||||
```csharp
|
||||
var start = new Pose2D(startXmm / 1000d, startYmm / 1000d,
|
||||
startHeadingDeg * Math.PI / 180d);
|
||||
var goal = new Pose2D(goalXmm / 1000d, goalYmm / 1000d,
|
||||
goalHeadingDeg * Math.PI / 180d);
|
||||
```
|
||||
|
||||
### 第 4 步：填写车辆参数
|
||||
|
||||
车辆尺寸和安全余量全部为 m。曲率限制可填写 `MaximumCurvaturePerMeter`，或填写 `MinimumTurningRadiusMeters`；至少必须提供一个有效限制。
|
||||
|
||||
```csharp
|
||||
var vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.80d,
|
||||
WidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
};
|
||||
```
|
||||
|
||||
### 第 5 步：调整搜索配置
|
||||
|
||||
默认 `HybridAStarConfiguration` 包含原语长度、积分步长、航向离散、终点容差、代价、节点上限和总超时。若业务需要覆盖默认值，应同时理解安全影响：`MaximumCollisionCheckStepMeters` 不能以牺牲连续碰撞检查精度为代价随意增大。
|
||||
|
||||
```csharp
|
||||
var configuration = new HybridAStarConfiguration
|
||||
{
|
||||
SearchTimeout = TimeSpan.FromSeconds(5d),
|
||||
MaximumExpandedNodes = 200000,
|
||||
};
|
||||
```
|
||||
|
||||
### 第 6 步：调用并消费成功结果
|
||||
|
||||
只有 `Success` 可以发布完整路径。`Path` 是稠密点序列；`Segments` 是覆盖整条路径的前进/倒车包含式索引段，可供后续的速度规划或显示模块消费。
|
||||
|
||||
```csharp
|
||||
var job = new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = mapRequest,
|
||||
Start = start,
|
||||
Goal = goal,
|
||||
Vehicle = vehicle,
|
||||
Configuration = configuration,
|
||||
GoalDirection = GoalDirectionConstraint.Any,
|
||||
};
|
||||
|
||||
CoarsePathPlanningJobResult result = _coarsePathService.Plan(job, cancellationToken);
|
||||
if (!result.MapResult.Succeeded)
|
||||
ReportMapFailure(result.MapResult.FailureReason);
|
||||
else if (result.PlanningResult.Status == PlanningStatus.Success)
|
||||
ConsumeCoarsePath(result.PlanningResult.Path, result.PlanningResult.Segments);
|
||||
else
|
||||
ReportPlanningFailure(result.PlanningResult.Diagnostics.TerminationReason);
|
||||
```
|
||||
|
||||
`CoarsePathPoint.IsGearSwitchPoint` 为 `true` 表示该点是新方向段开始处。换向位置会保留一对位置、航向与弧长相同、方向不同的相邻点；`UnwrappedHeading` 用于跨越 `-pi/pi` 时保持显示连续。
|
||||
|
||||
## P1 手动测试与可视化（P1 Manual Tests and Visualization）
|
||||
|
||||
P1 在 `Test/MovementTest.CoarsePathTest.cs` 提供只读测试入口。它们共用一个长期存活的 `CoarsePathPlanningService`，只提交规划并绘制结果；不发送底盘、速度或转向命令。
|
||||
|
||||
| MovementTest 名称 | 场景 | 预期 |
|
||||
| --- | --- | --- |
|
||||
| `粗路径规划-显式空图` | 显式允许的空图 | 前进直达成功 |
|
||||
| `粗路径规划-单矩形绕行` | 中央矩形阻断直线 | 成功绕障 |
|
||||
| `粗路径规划-多来源障碍` | 手工圆形、矩形和 TwoLeg 快照 | 证明多来源经过同一门面 |
|
||||
| `粗路径规划-缓存命中` | 重复相同地图输入 | 后续调用显示 `Input` 缓存命中 |
|
||||
| `粗路径规划-倒车换向` | 前进起步、倒车到达 | 成功路径含 `IsGearSwitchPoint` |
|
||||
| `粗路径规划-无解` | 贯穿地图的障碍带 | 返回 `NoFeasiblePath` 且不发布路径 |
|
||||
| `粗路径规划` | 当前 AMR 位姿、人工终点和可选人工障碍物 | 验证手动障碍物、边界、路径与可视化 |
|
||||
|
||||
### 固定案例的实时 AMR 锚定
|
||||
|
||||
`粗路径规划-显式空图`、`粗路径规划-单矩形绕行`、`粗路径规划-多来源障碍`、`粗路径规划-缓存命中`、`粗路径规划-倒车换向` 和 `粗路径规划-无解` 是六个固定案例。它们不再以写死的世界起点运行：共享运行器在前台仅调用一次 `DetourInterface.getCartLocation()`，校验并冻结本次 AMR 的世界 `X(mm)`、`Y(mm)` 与航向 `deg`，再创建本次规划请求；后台规划期间不会再次读取定位。
|
||||
|
||||
`Create(CoarsePathTestScenario scenario, double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)`
|
||||
|
||||
该入口把基准案例的起点映射为冻结的 AMR 位姿，并以相同的 `ΔX/ΔY` 平移地图边界、目标、圆形/矩形障碍，以及 TwoLeg 的检测原点。因此，固定案例始终在当前 AMR 附近保留原有的相对几何关系。起点航向严格使用冻结的 AMR 航向；终点航向保持基准案例的“终点航向减起点航向”差值，叠加到当前 AMR 航向后规范化到 `[-pi, pi]`。TwoLeg 只平移检测原点，`DetectionHeadingRadians` 不会因 AMR 航向发生旋转。
|
||||
|
||||
`粗路径规划-缓存命中` 只有两次运行冻结到相同的 AMR `X/Y`、从而形成相同的平移后地图输入时，才作为缓存命中场景；AMR 位置移动后，地图输入正常未命中并重建快照。仅 AMR 航向变化不会改变固定案例的地图输入，地图缓存仍可命中。若定位读取为空、抛出异常，或 `X`、`Y`、航向含有 `NaN`/无穷值，运行器不会提交后台规划，状态与 Toast 会显示以“AMR 位姿不可用”开头的诊断原因。
|
||||
|
||||
手动 `粗路径规划` 的人工终点、障碍物和超时输入流程保持不变；它不套用固定案例的整体平移规则。
|
||||
|
||||
### AMR 位姿、手动终点与障碍物
|
||||
|
||||
`CoarsePathPlanningTest` 启动时读取一次 `DetourInterface.getCartLocation()`，将当前 AMR 世界位姿冻结为起点；随后依次输入目标世界 `X(mm)`、`Y(mm)`、航向 `deg`，以及障碍物数量 `0-20`。每个障碍物再依次输入类型和几何参数：
|
||||
|
||||
| 类型输入 | 形状 | 输入参数（全部为 mm） |
|
||||
| --- | --- | --- |
|
||||
| `1` | 圆形 | 几何中心 `X`、`Y` 与半径；半径必须大于 0 |
|
||||
| `2` | 轴对齐矩形 | 几何中心 `X`、`Y`、X 向长度、Y 向宽度；两个尺寸必须大于 0 |
|
||||
|
||||
矩形只支持 `AxisAlignedRectangle`，不提供旋转角；其中心和长宽由 `ManualCoarsePathObstacle.AxisAlignedRectangle` 表达。圆形由 `ManualCoarsePathObstacle.Circle` 表达。所有无穷、NaN、非数字或不合法尺寸都会在输入阶段拒绝。
|
||||
|
||||
`CoarsePathScenarioFactory.CreateManualObstacleDemo` 在唯一边界完成转换：
|
||||
|
||||
```text
|
||||
AMR/目标 X、Y：mm / 1000 -> m
|
||||
AMR/目标航向：deg * PI / 180 -> rad
|
||||
```
|
||||
|
||||
当数量为 0 时，入口使用显式空图；`CreateManualGoalDemo` 保留为同一零障碍物场景的兼容帮助方法。数量大于 0 时，工厂将 `ManualCoarsePathObstacle` 快照封装成来源 ID 为 `manual-user-input` 的地图输入，并为每次手动快照分配新的 `SourceVersion`，避免错误复用地图缓存。规划边界覆盖起点、终点和每个障碍物的完整轮廓，再增加 8000 mm 余量并按 50 mm 对齐。
|
||||
|
||||
手动入口还要求输入一次“粗路径规划总超时”，单位为秒，只接受 `TimeSpan` 可表示范围内的有限正数秒；`0`、负数、NaN、Infinity 或溢出值都会在启动规划前拒绝。该值只覆盖本次 `CoarsePathPlanningJob.Configuration.SearchTimeout`，不会改变固定场景或全局默认值。
|
||||
|
||||
此入口使用固定演示车辆：长 `0.80 m`、宽 `0.60 m`、四周安全余量 `0.05 m`、最小转弯半径 `1.20 m`。这些值不是从现场 AMR 配置读取的，判断现场可行性前必须确认车辆参数一致。
|
||||
|
||||
`getCartLocation` 在无有效定位时可能阻塞，因此应在定位准备完成的测试环境使用。手动障碍物是测试输入，不能替代现场障碍物来源；零障碍物的显式空图也绝不代表现场不存在障碍物。
|
||||
|
||||
### 后台执行、停止与图层
|
||||
|
||||
每次测试启动时会创建独立的 `CancellationTokenSource`，以 `Task.Run` 调用门面，并先取消旧会话。`Test()` 不等待任务，也不读取 `Task.Result`；`TestStop` 取消当前令牌、使会话失效并清空图层。已取消任务完成后不会覆盖新会话，也不会显示部分路径。
|
||||
|
||||
专用世界坐标 Painter 图层为 `CoarsePathPlanningV1`。它直接读取本次 `PlanningGridMap` 的 `Bounds`、`ResolutionMm`、`SnapshotId` 和 `IsOccupied(row, col)`，因此边界、抽稀网格和占据格与实际规划快照一致，而不是重新绘制原始障碍物。
|
||||
|
||||
状态图层显示规划状态、总耗时、`PathSearchElapsed`（路径搜索耗时）、扩展/生成节点数、Open List 峰值、失败原因和固定演示车辆参数。Toast 同时显示两种耗时，并在失败时附加 `TerminationReason`，因此超时、节点上限、无解、碰撞和内部错误不会只显示成泛化失败。
|
||||
|
||||
| 颜色 | 可视化元素 |
|
||||
| --- | --- |
|
||||
| 灰白 | 地图边界与栅格网络 |
|
||||
| 暗红 | 占据格 |
|
||||
| 绿色 | 起点、前进路径和方向箭头 |
|
||||
| 橙色 | 终点、航向和位置容差圈 |
|
||||
| 天蓝 | 倒车路径和方向箭头 |
|
||||
| 紫色 | 换向点 |
|
||||
| 金色 | 已纳入安全余量的车辆检查框 |
|
||||
|
||||
自动化已检查 UI 入口的后台、取消和数据来源结构。仍需在实际 Clumsy 界面手动运行“粗路径规划-单矩形绕行”和“粗路径规划”，确认图层交互显示与停止按钮效果。
|
||||
|
||||
## 常见错误（Common Errors）
|
||||
|
||||
| 现象 | 原因 | 处理 |
|
||||
| --- | --- | --- |
|
||||
| 起点、终点或障碍物位置相差 1000 倍 | 将 mm 直接传给 `Pose2D` 或把 m 传给地图输入 | Map 输入使用 mm；`Pose2D`、车辆和路径使用 m |
|
||||
| 路径朝向错误或旋转异常 | 将 P1 的 deg 直接当作核心 rad | 在 UI/上层边界执行 `deg * PI / 180`，核心只保存 rad |
|
||||
| 障碍物已经变化却复用旧地图 | 内容变更后没有递增 `SourceVersion` | 每次来源快照内容变化后增加对应版本号 |
|
||||
| 地图创建成功但规划被阻止 | 没有有效障碍物且未显式允许空图 | 提供有效来源；仅在确认安全的演示中设置 `AllowExplicitEmptyMap = true` |
|
||||
| 无解、取消或超时后仍尝试使用路径 | 没有检查 `PlanningStatus.Success` | 仅成功时消费 `Path` 和 `Segments`；其他状态读取诊断 |
|
||||
| 将粗路径直接下发给车辆 | 粗路径不包含速度、时间、执行控制或实时安全闭环 | 在后续阶段增加平滑、时间参数化、跟踪和独立安全控制 |
|
||||
| P1 手动终点表现为空场地安全 | 手动入口使用显式空图演示 | 真实作业必须提供真实障碍物快照，不能复用空图语义 |
|
||||
|
||||
## 第一版限制（First-Version Limits）
|
||||
|
||||
当前 P0/P1 已提供安全、确定性的粗路径核心和手动结果可视化，但不包含：
|
||||
|
||||
- 路径平滑或曲率连续优化；
|
||||
- Reeds-Shepp 或 Dubins 精确终点连接；
|
||||
- 速度、加速度、时间标注、时间轨迹和路径跟踪控制；
|
||||
- 底盘命令、避障闭环、现场传感器采集或实时重规划调度；
|
||||
- 横移、蟹行或其他非前进/倒车运动原语；
|
||||
- 原地旋转；
|
||||
- 真实作业地图接入、交互式场景编辑和 Release 性能/资源/确定性基准。
|
||||
|
||||
当前只生成汽车式恒曲率前进/倒车原语，并允许在原语边界换向；未实现的 Reeds-Shepp、横移、蟹行和原地旋转是整个粗规划核心的第一版能力边界，不是 MovementTest 单独关闭。
|
||||
|
||||
因此，调用方只能把 `Success` 结果视作后续模块的粗路径输入，不能把它当作可直接下发的时间轨迹。
|
||||
@@ -0,0 +1,146 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 为 Hybrid A* Open List 提供确定性优先级的二叉最小堆。
|
||||
/// 排序严格依次比较 F、H、较大的 G 和插入序号；F、H、G 必须为有限且非负的等效米代价。
|
||||
/// </summary>
|
||||
/// <typeparam name="T">与一组搜索代价关联的节点或条目类型。</typeparam>
|
||||
public sealed class BinaryMinHeap<T>
|
||||
{
|
||||
private readonly List<HeapEntry> _entries = new List<HeapEntry>();
|
||||
private long _nextInsertionSequence;
|
||||
|
||||
/// <summary>创建空的确定性 Open List 堆。</summary>
|
||||
public BinaryMinHeap()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>当前堆内尚未出队的条目数量。</summary>
|
||||
public int Count { get { return _entries.Count; } }
|
||||
|
||||
/// <summary>
|
||||
/// 将一个条目和其搜索排序代价压入堆。
|
||||
/// 参数：item 为关联条目；f、h、g 均为有限且非负的等效米代价。
|
||||
/// 失败：任一代价无效、item 为 null（仅引用类型）或插入序号耗尽时抛出异常。
|
||||
/// </summary>
|
||||
public void Push(T item, double f, double h, double g)
|
||||
{
|
||||
if (ReferenceEquals(item, null)) throw new ArgumentNullException(nameof(item));
|
||||
ValidateCost(f, nameof(f));
|
||||
ValidateCost(h, nameof(h));
|
||||
ValidateCost(g, nameof(g));
|
||||
if (_nextInsertionSequence == long.MaxValue)
|
||||
throw new InvalidOperationException("The binary heap insertion sequence has been exhausted.");
|
||||
|
||||
var entry = new HeapEntry(item, f, h, g, _nextInsertionSequence);
|
||||
_nextInsertionSequence++;
|
||||
_entries.Add(entry);
|
||||
SiftUp(_entries.Count - 1);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 弹出当前排序最优的条目。
|
||||
/// 返回：按 F、H、较大 G 和插入序号排序后的最小条目；空堆时抛出 <see cref="InvalidOperationException"/>。
|
||||
/// </summary>
|
||||
public T Pop()
|
||||
{
|
||||
if (_entries.Count == 0) throw new InvalidOperationException("The binary heap is empty.");
|
||||
|
||||
HeapEntry result = _entries[0];
|
||||
int lastIndex = _entries.Count - 1;
|
||||
if (lastIndex == 0)
|
||||
{
|
||||
_entries.RemoveAt(0);
|
||||
return result.Item;
|
||||
}
|
||||
|
||||
_entries[0] = _entries[lastIndex];
|
||||
_entries.RemoveAt(lastIndex);
|
||||
SiftDown(0);
|
||||
return result.Item;
|
||||
}
|
||||
|
||||
/// <summary>清空尚未出队的条目；后续插入序号继续单调递增以保持整个实例内的确定性。</summary>
|
||||
public void Clear()
|
||||
{
|
||||
_entries.Clear();
|
||||
}
|
||||
|
||||
private void SiftUp(int index)
|
||||
{
|
||||
while (index > 0)
|
||||
{
|
||||
int parentIndex = (index - 1) / 2;
|
||||
if (Compare(_entries[index], _entries[parentIndex]) >= 0) return;
|
||||
Swap(index, parentIndex);
|
||||
index = parentIndex;
|
||||
}
|
||||
}
|
||||
|
||||
private void SiftDown(int index)
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
int leftChildIndex = index * 2 + 1;
|
||||
if (leftChildIndex >= _entries.Count) return;
|
||||
|
||||
int bestChildIndex = leftChildIndex;
|
||||
int rightChildIndex = leftChildIndex + 1;
|
||||
if (rightChildIndex < _entries.Count && Compare(_entries[rightChildIndex], _entries[leftChildIndex]) < 0)
|
||||
bestChildIndex = rightChildIndex;
|
||||
|
||||
if (Compare(_entries[bestChildIndex], _entries[index]) >= 0) return;
|
||||
Swap(index, bestChildIndex);
|
||||
index = bestChildIndex;
|
||||
}
|
||||
}
|
||||
|
||||
private void Swap(int firstIndex, int secondIndex)
|
||||
{
|
||||
HeapEntry temporary = _entries[firstIndex];
|
||||
_entries[firstIndex] = _entries[secondIndex];
|
||||
_entries[secondIndex] = temporary;
|
||||
}
|
||||
|
||||
private static int Compare(HeapEntry left, HeapEntry right)
|
||||
{
|
||||
int comparison = left.F.CompareTo(right.F);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = left.H.CompareTo(right.H);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = right.G.CompareTo(left.G);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
return left.InsertionSequence.CompareTo(right.InsertionSequence);
|
||||
}
|
||||
|
||||
private static void ValidateCost(double value, string parameterName)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Search costs must be finite and non-negative.");
|
||||
}
|
||||
|
||||
private sealed class HeapEntry
|
||||
{
|
||||
public HeapEntry(T item, double f, double h, double g, long insertionSequence)
|
||||
{
|
||||
Item = item;
|
||||
F = f;
|
||||
H = h;
|
||||
G = g;
|
||||
InsertionSequence = insertionSequence;
|
||||
}
|
||||
|
||||
public T Item { get; }
|
||||
public double F { get; }
|
||||
public double H { get; }
|
||||
public double G { get; }
|
||||
public long InsertionSequence { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>按位置、航向和进入方向约束判定连续位姿是否可以作为目标候选。</summary>
|
||||
public static class GoalToleranceChecker
|
||||
{
|
||||
/// <summary>
|
||||
/// 判断一个位姿是否满足目标容差和进入方向约束。
|
||||
/// 参数：pose 与 goal 使用世界 m/rad；configuration 提供位置 m 和航向 rad 容差；direction 为候选末段方向；goalDirection 为目标进入方向约束。
|
||||
/// 返回：输入有限、位置距离和最小环形航向误差均不超过容差且方向匹配时为 true；无效输入保守地返回 false。
|
||||
/// </summary>
|
||||
public static bool IsSatisfied(
|
||||
Pose2D pose,
|
||||
Pose2D goal,
|
||||
HybridAStarConfiguration configuration,
|
||||
TravelDirection direction,
|
||||
GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
if (!IsFinitePose(pose) || !IsFinitePose(goal) || configuration == null ||
|
||||
!NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) ||
|
||||
!NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) ||
|
||||
configuration.GoalPositionToleranceMeters < 0d || configuration.GoalHeadingToleranceRadians < 0d ||
|
||||
!IsTravelDirection(direction) || !IsGoalDirection(goalDirection))
|
||||
return false;
|
||||
|
||||
if (!MatchesDirection(direction, goalDirection)) return false;
|
||||
|
||||
double deltaX = pose.X - goal.X;
|
||||
double deltaY = pose.Y - goal.Y;
|
||||
double positionDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (!NumericGuard.IsFinite(positionDistanceMeters) || positionDistanceMeters > configuration.GoalPositionToleranceMeters)
|
||||
return false;
|
||||
|
||||
double headingDifferenceRadians = Math.Abs(AngleMath.ShortestSignedDifference(pose.Heading, goal.Heading));
|
||||
return NumericGuard.IsFinite(headingDifferenceRadians) && headingDifferenceRadians <= configuration.GoalHeadingToleranceRadians;
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
|
||||
private static bool MatchesDirection(TravelDirection direction, GoalDirectionConstraint constraint)
|
||||
{
|
||||
return constraint == GoalDirectionConstraint.Any ||
|
||||
(constraint == GoalDirectionConstraint.Forward && direction == TravelDirection.Forward) ||
|
||||
(constraint == GoalDirectionConstraint.Reverse && direction == TravelDirection.Reverse);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 基于不可变栅格地图的目标反向八邻域 Dijkstra 启发式。
|
||||
/// 距离单位为 m；对角移动仅在两个对应正交邻格都未占据时允许，以避免从障碍夹角穿越。
|
||||
/// </summary>
|
||||
public sealed class GridDijkstraHeuristic
|
||||
{
|
||||
private readonly PlanningGridMap _map;
|
||||
private readonly double[] _costs;
|
||||
|
||||
/// <summary>
|
||||
/// 从目标栅格预计算所有可达自由格到目标的二维最短距离。
|
||||
/// 参数：map 必须是已就绪的不可变地图；goalRow、goalCol 为地图内且未占据的目标格索引。
|
||||
/// 失败：地图为空、未就绪或目标格无效时抛出异常。
|
||||
/// </summary>
|
||||
public GridDijkstraHeuristic(PlanningGridMap map, int goalRow, int goalCol)
|
||||
{
|
||||
if (!TryCreate(map, goalRow, goalCol, PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
out GridDijkstraHeuristic heuristic, out _))
|
||||
throw new InvalidOperationException("Unbounded Dijkstra construction unexpectedly stopped.");
|
||||
_map = heuristic._map;
|
||||
_costs = heuristic._costs;
|
||||
}
|
||||
|
||||
private GridDijkstraHeuristic(PlanningGridMap map, double[] costs)
|
||||
{
|
||||
_map = map;
|
||||
_costs = costs;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建完整二维启发式；停止时不返回部分成本数组。</summary>
|
||||
internal static bool TryCreate(PlanningGridMap map, int goalRow, int goalCol, PlanningOperationBudget budget,
|
||||
out GridDijkstraHeuristic heuristic, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
if (!map.PlanningReady) throw new ArgumentException("The planning map must be ready.", nameof(map));
|
||||
if (goalRow < 0 || goalRow >= map.Rows || goalCol < 0 || goalCol >= map.Cols)
|
||||
throw new ArgumentOutOfRangeException(nameof(goalRow));
|
||||
if (map.IsOccupied(goalRow, goalCol))
|
||||
throw new ArgumentException("The goal grid cell must be free.", nameof(goalRow));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
|
||||
heuristic = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
var costs = new double[checked(map.Rows * map.Cols)];
|
||||
int workItemCount = 0;
|
||||
for (int index = 0; index < costs.Length; index++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
costs[index] = double.PositiveInfinity;
|
||||
}
|
||||
if (!TryBuild(map, costs, goalRow, goalCol, budget, ref workItemCount, out stopReason)) return false;
|
||||
heuristic = new GridDijkstraHeuristic(map, costs);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 查询指定栅格到构造时目标格的二维最短距离。
|
||||
/// 参数：row、col 为从零开始的栅格索引。
|
||||
/// 返回：单位 m 的有限最短距离；自由格不可达或索引越界时返回正无穷。
|
||||
/// </summary>
|
||||
public double GetCost(int row, int col)
|
||||
{
|
||||
return row < 0 || row >= _map.Rows || col < 0 || col >= _map.Cols
|
||||
? double.PositiveInfinity
|
||||
: _costs[row * _map.Cols + col];
|
||||
}
|
||||
|
||||
private static bool TryBuild(PlanningGridMap map, double[] costs, int goalRow, int goalCol,
|
||||
PlanningOperationBudget budget, ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
var openList = new BinaryMinHeap<int>();
|
||||
int goalIndex = goalRow * map.Cols + goalCol;
|
||||
costs[goalIndex] = 0d;
|
||||
openList.Push(goalIndex, 0d, 0d, 0d);
|
||||
|
||||
while (openList.Count > 0)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
int currentIndex = openList.Pop();
|
||||
int currentRow = currentIndex / map.Cols;
|
||||
int currentCol = currentIndex % map.Cols;
|
||||
double currentCost = costs[currentIndex];
|
||||
|
||||
for (int rowOffset = -1; rowOffset <= 1; rowOffset++)
|
||||
for (int colOffset = -1; colOffset <= 1; colOffset++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
if (rowOffset == 0 && colOffset == 0) continue;
|
||||
|
||||
int nextRow = currentRow + rowOffset;
|
||||
int nextCol = currentCol + colOffset;
|
||||
if (nextRow < 0 || nextRow >= map.Rows || nextCol < 0 || nextCol >= map.Cols || map.IsOccupied(nextRow, nextCol))
|
||||
continue;
|
||||
|
||||
bool isDiagonal = rowOffset != 0 && colOffset != 0;
|
||||
if (isDiagonal && (map.IsOccupied(currentRow + rowOffset, currentCol) || map.IsOccupied(currentRow, currentCol + colOffset)))
|
||||
continue;
|
||||
|
||||
double stepCost = isDiagonal ? Math.Sqrt(2d) * map.ResolutionMeters : map.ResolutionMeters;
|
||||
double candidateCost = currentCost + stepCost;
|
||||
int nextIndex = nextRow * map.Cols + nextCol;
|
||||
if (candidateCost >= costs[nextIndex]) continue;
|
||||
|
||||
costs[nextIndex] = candidateCost;
|
||||
openList.Push(nextIndex, candidateCost, 0d, 0d);
|
||||
}
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid A* 运行期节点。
|
||||
/// 节点保留连续位姿和其离散闭集键；搜索过程中不会修改已创建节点，改进代价时会追加新节点并使旧 Open List 条目失效。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarNode
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个搜索节点。
|
||||
/// 参数：nodeIndex 为本次搜索内稳定索引；key 为离散状态；pose 为连续车辆中心位姿；parentNodeIndex 为父节点索引，根节点使用 -1；
|
||||
/// incomingPrimitive 为父节点到本节点的原语，根节点为 null；curvaturePerMeter、gCostMeters 和 hCostMeters 均使用规划器的标准单位。
|
||||
/// </summary>
|
||||
public HybridAStarNode(
|
||||
int nodeIndex,
|
||||
HybridAStarNodeKey key,
|
||||
Pose2D pose,
|
||||
int parentNodeIndex,
|
||||
MotionPrimitive incomingPrimitive,
|
||||
double curvaturePerMeter,
|
||||
double gCostMeters,
|
||||
double hCostMeters)
|
||||
: this(nodeIndex, key, pose, parentNodeIndex, incomingPrimitive, curvaturePerMeter, gCostMeters, hCostMeters, false)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建一个搜索节点，并显式指定它是否为原语内部命中的终点候选。
|
||||
/// 参数：isGoalCandidate 为 true 时，本节点不参与普通离散状态的最优 G 值支配；它仍必须在从 Open List 出队后重新通过连续终点和碰撞检查才能成功。
|
||||
/// </summary>
|
||||
public HybridAStarNode(
|
||||
int nodeIndex,
|
||||
HybridAStarNodeKey key,
|
||||
Pose2D pose,
|
||||
int parentNodeIndex,
|
||||
MotionPrimitive incomingPrimitive,
|
||||
double curvaturePerMeter,
|
||||
double gCostMeters,
|
||||
double hCostMeters,
|
||||
bool isGoalCandidate)
|
||||
{
|
||||
if (key == null) throw new ArgumentNullException(nameof(key));
|
||||
if (pose == null) throw new ArgumentNullException(nameof(pose));
|
||||
|
||||
NodeIndex = nodeIndex;
|
||||
Key = key;
|
||||
Pose = pose;
|
||||
ParentNodeIndex = parentNodeIndex;
|
||||
IncomingPrimitive = incomingPrimitive;
|
||||
CurvaturePerMeter = curvaturePerMeter;
|
||||
GCostMeters = gCostMeters;
|
||||
HCostMeters = hCostMeters;
|
||||
IsGoalCandidate = isGoalCandidate;
|
||||
}
|
||||
|
||||
/// <summary>本次搜索节点数组中的稳定索引。</summary>
|
||||
public int NodeIndex { get; }
|
||||
|
||||
/// <summary>本节点用于 Open/Closed 状态管理的离散键。</summary>
|
||||
public HybridAStarNodeKey Key { get; }
|
||||
|
||||
/// <summary>未量化的连续车辆中心位姿。</summary>
|
||||
public Pose2D Pose { get; }
|
||||
|
||||
/// <summary>父节点稳定索引；根节点为 -1。</summary>
|
||||
public int ParentNodeIndex { get; }
|
||||
|
||||
/// <summary>由父节点驶入本节点的已连续碰撞检查原语；根节点为 null。</summary>
|
||||
public MotionPrimitive IncomingPrimitive { get; }
|
||||
|
||||
/// <summary>与 <see cref="IncomingPrimitive"/> 含义相同的原语别名。</summary>
|
||||
public MotionPrimitive Primitive { get { return IncomingPrimitive; } }
|
||||
|
||||
/// <summary>当前末段采用的车辆曲率，单位 1/m。</summary>
|
||||
public double CurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>当前节点的累计等效米代价。</summary>
|
||||
public double GCostMeters { get; }
|
||||
|
||||
/// <summary>当前节点的二维启发式等效米代价。</summary>
|
||||
public double HCostMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 本节点是否由原语内部首次满足终点条件而生成。
|
||||
/// 终点候选必须保留连续位姿，不能因同一离散键的普通低代价节点而被 Open List 准入规则压制。
|
||||
/// </summary>
|
||||
public bool IsGoalCandidate { get; }
|
||||
|
||||
/// <summary>Open List 排序使用的总等效米代价。</summary>
|
||||
public double FCostMeters { get { return GCostMeters + HCostMeters; } }
|
||||
|
||||
/// <summary>本节点末段的行驶方向。</summary>
|
||||
public TravelDirection Direction { get { return Key.Direction; } }
|
||||
|
||||
/// <summary>本节点末段的曲率等级索引。</summary>
|
||||
public int CurvatureLevelIndex { get { return Key.CurvatureLevelIndex; } }
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid A* 闭集使用的离散状态键。
|
||||
/// 位置使用地图行列索引；航向、行驶方向和曲率等级共同保留车辆运动学状态，避免把同一栅格中的不同可达姿态错误合并。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarNodeKey : IEquatable<HybridAStarNodeKey>
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个离散 Hybrid A* 状态键。
|
||||
/// 参数：row、column 为零开始的地图行列；headingIndex 为航向桶；direction 为末段行驶方向；curvatureLevelIndex 为末段曲率等级。
|
||||
/// </summary>
|
||||
public HybridAStarNodeKey(int row, int column, int headingIndex, TravelDirection direction, int curvatureLevelIndex)
|
||||
{
|
||||
Row = row;
|
||||
Column = column;
|
||||
HeadingIndex = headingIndex;
|
||||
Direction = direction;
|
||||
CurvatureLevelIndex = curvatureLevelIndex;
|
||||
}
|
||||
|
||||
/// <summary>车辆中心所在的零开始地图行索引。</summary>
|
||||
public int Row { get; }
|
||||
|
||||
/// <summary>车辆中心所在的零开始地图列索引。</summary>
|
||||
public int Column { get; }
|
||||
|
||||
/// <summary>与 <see cref="Column"/> 含义相同的列索引别名。</summary>
|
||||
public int Col { get { return Column; } }
|
||||
|
||||
/// <summary>根据配置航向分辨率量化后的航向桶索引。</summary>
|
||||
public int HeadingIndex { get; }
|
||||
|
||||
/// <summary>到达当前节点的最后一段行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>到达当前节点的最后一段曲率等级索引。</summary>
|
||||
public int CurvatureLevelIndex { get; }
|
||||
|
||||
/// <summary>判断另一个键是否表示完全相同的离散搜索状态。</summary>
|
||||
public bool Equals(HybridAStarNodeKey other)
|
||||
{
|
||||
return other != null && Row == other.Row && Column == other.Column && HeadingIndex == other.HeadingIndex &&
|
||||
Direction == other.Direction && CurvatureLevelIndex == other.CurvatureLevelIndex;
|
||||
}
|
||||
|
||||
/// <summary>判断另一个对象是否表示完全相同的离散搜索状态。</summary>
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
return Equals(obj as HybridAStarNodeKey);
|
||||
}
|
||||
|
||||
/// <summary>返回用于闭集字典的稳定哈希值。</summary>
|
||||
public override int GetHashCode()
|
||||
{
|
||||
unchecked
|
||||
{
|
||||
int hashCode = Row;
|
||||
hashCode = hashCode * 397 ^ Column;
|
||||
hashCode = hashCode * 397 ^ HeadingIndex;
|
||||
hashCode = hashCode * 397 ^ (int)Direction;
|
||||
return hashCode * 397 ^ CurvatureLevelIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,542 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 单次 Hybrid A* 搜索的只读结果。
|
||||
/// 即使失败也会保留已经创建的运行期节点，供上层记录诊断；仅 <see cref="PlanningStatus.Success"/> 时 <see cref="SuccessNodeIndex"/> 有值。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarSearchResult
|
||||
{
|
||||
internal HybridAStarSearchResult(
|
||||
PlanningStatus status,
|
||||
IEnumerable<HybridAStarNode> nodes,
|
||||
int expandedNodeCount,
|
||||
int generatedNodeCount,
|
||||
int reopenedNodeCount,
|
||||
int staleOpenListEntryCount,
|
||||
int peakOpenListCount,
|
||||
int? successNodeIndex,
|
||||
string terminationReason)
|
||||
{
|
||||
Status = status;
|
||||
Nodes = new ReadOnlyCollection<HybridAStarNode>(new List<HybridAStarNode>(nodes ?? Array.Empty<HybridAStarNode>()));
|
||||
ExpandedNodeCount = expandedNodeCount;
|
||||
GeneratedNodeCount = generatedNodeCount;
|
||||
ReopenedNodeCount = reopenedNodeCount;
|
||||
StaleOpenListEntryCount = staleOpenListEntryCount;
|
||||
PeakOpenListCount = peakOpenListCount;
|
||||
SuccessNodeIndex = status == PlanningStatus.Success ? successNodeIndex : null;
|
||||
TerminationReason = status == PlanningStatus.Success ? string.Empty : terminationReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>搜索终止状态。</summary>
|
||||
public PlanningStatus Status { get; }
|
||||
|
||||
/// <summary>本次搜索已经创建的全部节点；索引与 <see cref="HybridAStarNode.NodeIndex"/> 一致。</summary>
|
||||
public IReadOnlyList<HybridAStarNode> Nodes { get; }
|
||||
|
||||
/// <summary>实际从 Open List 弹出并扩展的节点数量。</summary>
|
||||
public int ExpandedNodeCount { get; }
|
||||
|
||||
/// <summary>已进入 Open List 的节点数量，包含根节点和因改进代价追加的节点。</summary>
|
||||
public int GeneratedNodeCount { get; }
|
||||
|
||||
/// <summary>更优路径到达已关闭离散状态、并重新放回 Open List 的次数。</summary>
|
||||
public int ReopenedNodeCount { get; }
|
||||
|
||||
/// <summary>从 Open List 弹出后因已有更优普通状态而被丢弃的陈旧条目数量。</summary>
|
||||
public int StaleOpenListEntryCount { get; }
|
||||
|
||||
/// <summary>搜索期间 Open List 持有的最大条目数，包含等待惰性丢弃的旧条目。</summary>
|
||||
public int PeakOpenListCount { get; }
|
||||
|
||||
/// <summary>成功时最后一个从 Open List 弹出且满足终点条件的节点索引；失败时为 null。</summary>
|
||||
public int? SuccessNodeIndex { get; }
|
||||
|
||||
/// <summary>搜索边界记录的原始终止原因；成功时为空字符串，失败时非空。</summary>
|
||||
public string TerminationReason { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在不可变 <see cref="PlanningGridMap"/> 上执行前进/倒车恒曲率原语的 Hybrid A* 搜索。
|
||||
/// 搜索只消费已准备好的地图快照；所有候选原语均先完成连续扩大车体碰撞检查，再参与 Open List 排序。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarSearch
|
||||
{
|
||||
private const double CostImprovementToleranceMeters = 1e-9d;
|
||||
private readonly MotionPrimitiveGenerator _primitiveGenerator;
|
||||
private readonly SearchCostCalculator _costCalculator;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认原语、代价和碰撞检查实现的搜索器。</summary>
|
||||
public HybridAStarSearch()
|
||||
: this(new MotionPrimitiveGenerator(), new SearchCostCalculator(), new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定协作对象的搜索器，便于在不引入地图或 UI 依赖的情况下测试搜索过程。</summary>
|
||||
public HybridAStarSearch(
|
||||
MotionPrimitiveGenerator primitiveGenerator,
|
||||
SearchCostCalculator costCalculator,
|
||||
FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
|
||||
_costCalculator = costCalculator ?? throw new ArgumentNullException(nameof(costCalculator));
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行一次不可变地图上的 Hybrid A* 搜索。
|
||||
/// 参数：request 提供地图、起终点、车辆和搜索配置；cancellationToken 在每次节点扩展前检查。
|
||||
/// 返回：成功仅在满足目标约束的候选已从 Open List 弹出时报告；任一失败状态均不报告成功节点。
|
||||
/// </summary>
|
||||
public HybridAStarSearchResult Search(PlanningRequest request, CancellationToken cancellationToken)
|
||||
{
|
||||
TimeSpan timeout = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? request.Configuration.SearchTimeout
|
||||
: TimeSpan.Zero;
|
||||
PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? new PlanningOperationBudget(cancellationToken, timeout)
|
||||
: PlanningOperationBudget.Unlimited(cancellationToken);
|
||||
return Search(request, budget);
|
||||
}
|
||||
|
||||
/// <summary>使用门面传入的共享预算执行搜索；预算从整次规划开始计时。</summary>
|
||||
internal HybridAStarSearchResult Search(PlanningRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
var nodes = new List<HybridAStarNode>();
|
||||
int expandedNodeCount = 0;
|
||||
int generatedNodeCount = 0;
|
||||
int reopenedNodeCount = 0;
|
||||
int staleOpenListEntryCount = 0;
|
||||
int peakOpenListCount = 0;
|
||||
|
||||
try
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
PlanningStatus validationStatus = ValidateRequest(request);
|
||||
if (validationStatus != PlanningStatus.Success)
|
||||
return CreateResult(validationStatus, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
PlanningGridMap map = request.Map;
|
||||
HybridAStarConfiguration configuration = request.Configuration;
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
|
||||
if (!IsFootprintInsideMap(request.Start, map, vehicle))
|
||||
return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Start, map, vehicle, 0d, out _))
|
||||
return CreateResult(PlanningStatus.StartInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!IsFootprintInsideMap(request.Goal, map, vehicle))
|
||||
return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Goal, map, vehicle, 0d, out _))
|
||||
return CreateResult(PlanningStatus.GoalInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (configuration.MaximumExpandedNodes == 0)
|
||||
return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
if (!map.TryWorldToGrid(request.Goal.X, request.Goal.Y, out int goalRow, out int goalColumn))
|
||||
return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!GridDijkstraHeuristic.TryCreate(map, goalRow, goalColumn, budget, out GridDijkstraHeuristic heuristic, out stopReason))
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return CreateResult(PlanningStatus.InvalidVehicleParameters, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
IReadOnlyList<double> curvatureLevels = _primitiveGenerator.GetCurvatureLevels(vehicle, configuration);
|
||||
if (curvatureLevels.Count == 0)
|
||||
return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
int headingBinCount = GetHeadingBinCount(configuration.HeadingResolutionRadians);
|
||||
int startCurvatureLevelIndex = GetNearestCurvatureLevelIndex(curvatureLevels, request.StartVehicleCurvature);
|
||||
if (startCurvatureLevelIndex < 0)
|
||||
return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
if (!map.TryWorldToGrid(request.Start.X, request.Start.Y, out int startRow, out int startColumn))
|
||||
return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
var openList = new BinaryMinHeap<int>();
|
||||
var bestStates = new Dictionary<HybridAStarNodeKey, NodeLabel>();
|
||||
foreach (TravelDirection startDirection in GetStartDirections(request, configuration))
|
||||
{
|
||||
int headingIndex = AngleMath.ToHeadingIndex(request.Start.Heading, configuration.HeadingResolutionRadians, headingBinCount);
|
||||
var key = new HybridAStarNodeKey(startRow, startColumn, headingIndex, startDirection, startCurvatureLevelIndex);
|
||||
if (bestStates.ContainsKey(key)) continue;
|
||||
|
||||
double hCostMeters = GetHeuristicCost(heuristic, startRow, startColumn, configuration);
|
||||
if (double.IsPositiveInfinity(hCostMeters)) continue;
|
||||
var node = new HybridAStarNode(nodes.Count, key, request.Start, -1, null,
|
||||
curvatureLevels[startCurvatureLevelIndex], 0d, hCostMeters);
|
||||
nodes.Add(node);
|
||||
bestStates.Add(key, new NodeLabel(node.NodeIndex, 0d, false));
|
||||
openList.Push(node.NodeIndex, node.FCostMeters, node.HCostMeters, node.GCostMeters);
|
||||
peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
|
||||
generatedNodeCount++;
|
||||
}
|
||||
|
||||
if (openList.Count == 0)
|
||||
return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null,
|
||||
"二维启发式标记起点不可达目标，或起始方向无法进入 Open List。");
|
||||
|
||||
while (openList.Count > 0)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (expandedNodeCount >= configuration.MaximumExpandedNodes)
|
||||
return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
int nodeIndex = openList.Pop();
|
||||
HybridAStarNode current = nodes[nodeIndex];
|
||||
NodeLabel currentLabel = null;
|
||||
if (!current.IsGoalCandidate)
|
||||
{
|
||||
if (!bestStates.TryGetValue(current.Key, out currentLabel) || currentLabel.NodeIndex != nodeIndex || currentLabel.IsClosed)
|
||||
{
|
||||
staleOpenListEntryCount++;
|
||||
continue;
|
||||
}
|
||||
|
||||
currentLabel.IsClosed = true;
|
||||
}
|
||||
|
||||
expandedNodeCount++;
|
||||
if (current.IsGoalCandidate)
|
||||
{
|
||||
if (!GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection) ||
|
||||
!_collisionChecker.IsPoseCollisionFree(current.Pose, map, vehicle, 0d, out _))
|
||||
continue;
|
||||
|
||||
return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
|
||||
}
|
||||
|
||||
if (GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection))
|
||||
return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
|
||||
|
||||
for (int curvatureLevelIndex = 0; curvatureLevelIndex < curvatureLevels.Count; curvatureLevelIndex++)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!MotionPrimitiveGenerator.AreCurvatureLevelsAdjacent(current.CurvatureLevelIndex, curvatureLevelIndex)) continue;
|
||||
|
||||
foreach (TravelDirection direction in GetSuccessorDirections(configuration))
|
||||
{
|
||||
MotionPrimitive primitive = _primitiveGenerator.Generate(current.Pose, curvatureLevels[curvatureLevelIndex], direction, request);
|
||||
if (primitive == null || primitive.ActualLengthMeters <= 0d) continue;
|
||||
|
||||
if (!map.TryWorldToGrid(primitive.End.X, primitive.End.Y, out int row, out int column)) continue;
|
||||
double hCostMeters = GetHeuristicCost(heuristic, row, column, configuration);
|
||||
if (double.IsPositiveInfinity(hCostMeters)) continue;
|
||||
|
||||
double bodyClearanceMeters = GetMinimumBodyClearance(primitive);
|
||||
bool isGearSwitch = current.ParentNodeIndex >= 0 && current.Direction != direction;
|
||||
int curvatureLevelDelta = curvatureLevelIndex - current.CurvatureLevelIndex;
|
||||
double incrementalCostMeters = _costCalculator.Calculate(
|
||||
primitive.ActualLengthMeters,
|
||||
direction,
|
||||
isGearSwitch,
|
||||
curvatureLevels[curvatureLevelIndex],
|
||||
maximumCurvaturePerMeter,
|
||||
curvatureLevelDelta,
|
||||
bodyClearanceMeters,
|
||||
configuration);
|
||||
double gCostMeters = current.GCostMeters + incrementalCostMeters;
|
||||
if (!NumericGuard.IsFinite(gCostMeters) || gCostMeters < 0d) continue;
|
||||
|
||||
int headingIndex = AngleMath.ToHeadingIndex(primitive.End.Heading, configuration.HeadingResolutionRadians, headingBinCount);
|
||||
var key = new HybridAStarNodeKey(row, column, headingIndex, direction, curvatureLevelIndex);
|
||||
bestStates.TryGetValue(key, out NodeLabel existingLabel);
|
||||
var successor = new HybridAStarNode(nodes.Count, key, primitive.End, current.NodeIndex, primitive,
|
||||
curvatureLevels[curvatureLevelIndex], gCostMeters, hCostMeters, primitive.IsGoalTruncation);
|
||||
if (existingLabel != null && !ShouldEnqueueSuccessor(successor, existingLabel.GCostMeters)) continue;
|
||||
|
||||
if (!successor.IsGoalCandidate && existingLabel != null && existingLabel.IsClosed) reopenedNodeCount++;
|
||||
nodes.Add(successor);
|
||||
if (!successor.IsGoalCandidate)
|
||||
bestStates[key] = new NodeLabel(successor.NodeIndex, gCostMeters, false);
|
||||
openList.Push(successor.NodeIndex, successor.FCostMeters, successor.HCostMeters, successor.GCostMeters);
|
||||
peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
|
||||
generatedNodeCount++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null,
|
||||
"Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。");
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
string reason = "Hybrid A* 搜索内部错误：" + exception.GetType().Name +
|
||||
(string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
|
||||
return CreateResult(PlanningStatus.InternalError, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null, reason);
|
||||
}
|
||||
}
|
||||
|
||||
private static HybridAStarSearchResult CreateResult(
|
||||
PlanningStatus status,
|
||||
IEnumerable<HybridAStarNode> nodes,
|
||||
int expandedNodeCount,
|
||||
int generatedNodeCount,
|
||||
int reopenedNodeCount,
|
||||
int staleOpenListEntryCount,
|
||||
int peakOpenListCount,
|
||||
int? successNodeIndex,
|
||||
string terminationReason = null)
|
||||
{
|
||||
string reason = status == PlanningStatus.Success
|
||||
? string.Empty
|
||||
: terminationReason ?? GetDefaultTerminationReason(status);
|
||||
return new HybridAStarSearchResult(status, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, successNodeIndex, reason);
|
||||
}
|
||||
|
||||
private static string GetDefaultTerminationReason(PlanningStatus status)
|
||||
{
|
||||
switch (status)
|
||||
{
|
||||
case PlanningStatus.Cancelled:
|
||||
return "Hybrid A* 搜索已取消。";
|
||||
case PlanningStatus.InvalidRequest:
|
||||
return "Hybrid A* 搜索请求缺少必要对象或包含非法数值。";
|
||||
case PlanningStatus.InvalidMap:
|
||||
return "Hybrid A* 搜索地图结构无效。";
|
||||
case PlanningStatus.MapNotReady:
|
||||
return "Hybrid A* 搜索地图尚未准备好。";
|
||||
case PlanningStatus.InvalidVehicleParameters:
|
||||
return "Hybrid A* 搜索车辆参数无效。";
|
||||
case PlanningStatus.InvalidCurvatureConfiguration:
|
||||
return "Hybrid A* 搜索曲率、离散、代价或资源配置无效。";
|
||||
case PlanningStatus.StartOutsideMap:
|
||||
return "Hybrid A* 搜索起始扩大车体不完全位于地图内。";
|
||||
case PlanningStatus.StartInCollision:
|
||||
return "Hybrid A* 搜索起始扩大车体与障碍物相交或擦边。";
|
||||
case PlanningStatus.GoalOutsideMap:
|
||||
return "Hybrid A* 搜索目标扩大车体不完全位于地图内。";
|
||||
case PlanningStatus.GoalInCollision:
|
||||
return "Hybrid A* 搜索目标扩大车体与障碍物相交或擦边。";
|
||||
case PlanningStatus.SearchTimeout:
|
||||
return "Hybrid A* 搜索使用的总规划预算已耗尽。";
|
||||
case PlanningStatus.SearchNodeLimitExceeded:
|
||||
return "Hybrid A* 搜索达到扩展节点上限。";
|
||||
case PlanningStatus.NoFeasiblePath:
|
||||
return "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。";
|
||||
case PlanningStatus.BacktrackingFailed:
|
||||
return "Hybrid A* 成功节点无法回溯为完整父链。";
|
||||
case PlanningStatus.FinalValidationFailed:
|
||||
return "Hybrid A* 路径未通过最终复核。";
|
||||
case PlanningStatus.InternalError:
|
||||
return "Hybrid A* 搜索发生未预期内部错误。";
|
||||
default:
|
||||
return "Hybrid A* 搜索以未识别状态终止：" + status + "。";
|
||||
}
|
||||
}
|
||||
|
||||
private static PlanningStatus ValidateRequest(PlanningRequest request)
|
||||
{
|
||||
if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
|
||||
!IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
|
||||
!IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
|
||||
return PlanningStatus.InvalidRequest;
|
||||
|
||||
PlanningGridMap map = request.Map;
|
||||
if (map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters) || map.Bounds == null)
|
||||
return PlanningStatus.InvalidMap;
|
||||
if (!map.PlanningReady) return PlanningStatus.MapNotReady;
|
||||
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
|
||||
!NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return PlanningStatus.InvalidVehicleParameters;
|
||||
|
||||
HybridAStarConfiguration configuration = request.Configuration;
|
||||
if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
|
||||
(request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
|
||||
return PlanningStatus.InvalidCurvatureConfiguration;
|
||||
|
||||
return PlanningStatus.Success;
|
||||
}
|
||||
|
||||
private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
|
||||
{
|
||||
return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
|
||||
configuration.HeadingResolutionRadians <= 2d * Math.PI &&
|
||||
configuration.CurvatureLevelCount >= 3 && configuration.CurvatureLevelCount % 2 == 1 &&
|
||||
NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) && configuration.GoalPositionToleranceMeters >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) && configuration.GoalHeadingToleranceRadians >= 0d &&
|
||||
configuration.MaximumExpandedNodes >= 0 && configuration.SearchTimeout >= TimeSpan.Zero &&
|
||||
NumericGuard.IsFinite(configuration.HeuristicWeight) && configuration.HeuristicWeight >= 0d &&
|
||||
NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
|
||||
NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) && configuration.CurvatureChangePenaltyMetersPerLevel >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.ClearanceCostWeight) && configuration.ClearanceCostWeight >= 0d &&
|
||||
NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
|
||||
}
|
||||
|
||||
private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
|
||||
{
|
||||
if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;
|
||||
|
||||
double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double longitudinalX = Math.Cos(pose.Heading);
|
||||
double longitudinalY = Math.Sin(pose.Heading);
|
||||
double lateralX = -longitudinalY;
|
||||
double lateralY = longitudinalX;
|
||||
for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
|
||||
for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
|
||||
{
|
||||
double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
|
||||
double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
|
||||
if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IEnumerable<TravelDirection> GetStartDirections(PlanningRequest request, HybridAStarConfiguration configuration)
|
||||
{
|
||||
if (request.StartDirection.HasValue)
|
||||
{
|
||||
yield return request.StartDirection.Value;
|
||||
yield break;
|
||||
}
|
||||
|
||||
yield return TravelDirection.Forward;
|
||||
if (configuration.AllowReverse) yield return TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static IEnumerable<TravelDirection> GetSuccessorDirections(HybridAStarConfiguration configuration)
|
||||
{
|
||||
yield return TravelDirection.Forward;
|
||||
if (configuration.AllowReverse) yield return TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static int GetHeadingBinCount(double headingResolutionRadians)
|
||||
{
|
||||
double rawBinCount = Math.Ceiling(2d * Math.PI / headingResolutionRadians);
|
||||
if (!NumericGuard.IsFinite(rawBinCount) || rawBinCount < 1d || rawBinCount > int.MaxValue)
|
||||
throw new ArgumentOutOfRangeException(nameof(headingResolutionRadians));
|
||||
return (int)rawBinCount;
|
||||
}
|
||||
|
||||
private static int GetNearestCurvatureLevelIndex(IReadOnlyList<double> curvatureLevels, double curvaturePerMeter)
|
||||
{
|
||||
int bestIndex = -1;
|
||||
double smallestDifference = double.PositiveInfinity;
|
||||
for (int index = 0; index < curvatureLevels.Count; index++)
|
||||
{
|
||||
double difference = Math.Abs(curvatureLevels[index] - curvaturePerMeter);
|
||||
if (difference >= smallestDifference) continue;
|
||||
smallestDifference = difference;
|
||||
bestIndex = index;
|
||||
}
|
||||
return bestIndex;
|
||||
}
|
||||
|
||||
private static double GetMinimumBodyClearance(MotionPrimitive primitive)
|
||||
{
|
||||
double minimum = double.PositiveInfinity;
|
||||
foreach (double clearanceMeters in primitive.BodyClearancesMeters)
|
||||
{
|
||||
if (double.IsNaN(clearanceMeters) || clearanceMeters < 0d) return 0d;
|
||||
minimum = Math.Min(minimum, clearanceMeters);
|
||||
}
|
||||
return minimum;
|
||||
}
|
||||
|
||||
private static double GetHeuristicCost(GridDijkstraHeuristic heuristic, int row, int column, HybridAStarConfiguration configuration)
|
||||
{
|
||||
double gridCostMeters = heuristic.GetCost(row, column);
|
||||
if (double.IsPositiveInfinity(gridCostMeters)) return gridCostMeters;
|
||||
double weightedCostMeters = gridCostMeters * configuration.HeuristicWeight;
|
||||
return NumericGuard.IsFinite(weightedCostMeters) && weightedCostMeters >= 0d
|
||||
? weightedCostMeters
|
||||
: double.PositiveInfinity;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断后继是否应进入 Open List。
|
||||
/// 参数：successor 是已完成连续碰撞检查的后继；bestKnownGCostMeters 是相同离散键普通状态的当前最优 G 值。
|
||||
/// 返回：普通状态仅在严格改善最优 G 值时入队；终点候选始终入队，以保留其未量化的连续终点位姿。
|
||||
/// </summary>
|
||||
private static bool ShouldEnqueueSuccessor(HybridAStarNode successor, double bestKnownGCostMeters)
|
||||
{
|
||||
if (successor == null || !NumericGuard.IsFinite(bestKnownGCostMeters)) return false;
|
||||
return successor.IsGoalCandidate || successor.GCostMeters < bestKnownGCostMeters - CostImprovementToleranceMeters;
|
||||
}
|
||||
|
||||
private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
|
||||
if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
|
||||
throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
|
||||
private sealed class NodeLabel
|
||||
{
|
||||
public NodeLabel(int nodeIndex, double gCostMeters, bool isClosed)
|
||||
{
|
||||
NodeIndex = nodeIndex;
|
||||
GCostMeters = gCostMeters;
|
||||
IsClosed = isClosed;
|
||||
}
|
||||
|
||||
public int NodeIndex { get; }
|
||||
public double GCostMeters { get; }
|
||||
public bool IsClosed { get; set; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 一条经连续碰撞检查的恒曲率运动原语。
|
||||
/// 位置和长度单位为 m，航向单位为 rad，曲率单位为 1/m；<see cref="Points"/> 不包含起点，只包含按积分顺序产生的后续点。
|
||||
/// </summary>
|
||||
public sealed class MotionPrimitive
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建不可变运动原语。
|
||||
/// 参数：start 为原语起点；direction 为行驶方向；curvaturePerMeter 为恒定曲率；actualLengthMeters 为已实际行驶长度；
|
||||
/// points 与 bodyClearancesMeters 按同一索引保存内部积分位姿和对应的车体净空；isGoalTruncation 表示末点是否首次命中目标容差。
|
||||
/// </summary>
|
||||
public MotionPrimitive(
|
||||
Pose2D start,
|
||||
TravelDirection direction,
|
||||
double curvaturePerMeter,
|
||||
double actualLengthMeters,
|
||||
IEnumerable<Pose2D> points,
|
||||
IEnumerable<double> bodyClearancesMeters,
|
||||
bool isGoalTruncation)
|
||||
{
|
||||
if (start == null) throw new ArgumentNullException(nameof(start));
|
||||
if (points == null) throw new ArgumentNullException(nameof(points));
|
||||
if (bodyClearancesMeters == null) throw new ArgumentNullException(nameof(bodyClearancesMeters));
|
||||
|
||||
var copiedPoints = new List<Pose2D>(points);
|
||||
var copiedClearances = new List<double>(bodyClearancesMeters);
|
||||
if (copiedPoints.Count != copiedClearances.Count)
|
||||
throw new ArgumentException("The point and clearance counts must match.", nameof(bodyClearancesMeters));
|
||||
|
||||
Start = start;
|
||||
Direction = direction;
|
||||
CurvaturePerMeter = curvaturePerMeter;
|
||||
ActualLengthMeters = actualLengthMeters;
|
||||
Points = new ReadOnlyCollection<Pose2D>(copiedPoints);
|
||||
BodyClearancesMeters = new ReadOnlyCollection<double>(copiedClearances);
|
||||
End = copiedPoints.Count == 0 ? start : copiedPoints[copiedPoints.Count - 1];
|
||||
IsGoalTruncation = isGoalTruncation;
|
||||
}
|
||||
|
||||
/// <summary>原语起始车辆几何中心位姿。</summary>
|
||||
public Pose2D Start { get; }
|
||||
|
||||
/// <summary>原语最后一个积分点；零长度终点候选时等于 <see cref="Start"/>。</summary>
|
||||
public Pose2D End { get; }
|
||||
|
||||
/// <summary>原语对应的行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>原语全程采用的恒定车辆曲率，单位 1/m。</summary>
|
||||
public double CurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>从 <see cref="Start"/> 到 <see cref="End"/> 的实际行驶弧长，单位 m。</summary>
|
||||
public double ActualLengthMeters { get; }
|
||||
|
||||
/// <summary>不含起点的连续积分位姿，只读且按行驶顺序排列。</summary>
|
||||
public IReadOnlyList<Pose2D> Points { get; }
|
||||
|
||||
/// <summary>与 <see cref="Points"/> 一一对应的扩大车体保守净空下界，单位 m。</summary>
|
||||
public IReadOnlyList<double> BodyClearancesMeters { get; }
|
||||
|
||||
/// <summary>末点是否因首次满足目标位置、航向和方向约束而截断。</summary>
|
||||
public bool IsGoalTruncation { get; }
|
||||
}
|
||||
@@ -0,0 +1,173 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 生成经解析积分和连续碰撞检查的前进或倒车恒曲率原语。
|
||||
/// 每个积分点均依次经过有限数值检查、与前一点之间的扫掠碰撞检查和终点容差检查。
|
||||
/// </summary>
|
||||
public sealed class MotionPrimitiveGenerator
|
||||
{
|
||||
private const double StraightCurvatureThreshold = 1e-12d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认车辆连续碰撞检查器的原语生成器。</summary>
|
||||
public MotionPrimitiveGenerator()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车辆碰撞检查器的原语生成器。</summary>
|
||||
public MotionPrimitiveGenerator(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用完整规划请求生成一条原语。
|
||||
/// 参数：start 为当前连续位姿；curvaturePerMeter 为候选恒定曲率；direction 为前进或倒车；request 提供地图、车辆、配置和目标。
|
||||
/// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；否则返回最大长度不超过配置上限的原语。
|
||||
/// </summary>
|
||||
public MotionPrimitive Generate(Pose2D start, double curvaturePerMeter, TravelDirection direction, PlanningRequest request)
|
||||
{
|
||||
if (request == null) return null;
|
||||
return Generate(start, curvaturePerMeter, direction, request.Map, request.Vehicle, request.Configuration, request.Goal, request.GoalDirection);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用显式地图、车辆、配置和目标生成一条原语。
|
||||
/// 参数：所有位置使用 m/rad，curvaturePerMeter 使用 1/m；goalDirection 限制末段允许的进入方向。
|
||||
/// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；首次命中目标时返回 <see cref="MotionPrimitive.IsGoalTruncation"/> 为 true 的截断原语。
|
||||
/// </summary>
|
||||
public MotionPrimitive Generate(
|
||||
Pose2D start,
|
||||
double curvaturePerMeter,
|
||||
TravelDirection direction,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
HybridAStarConfiguration configuration,
|
||||
Pose2D goal,
|
||||
GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
if (!IsValidInput(start, curvaturePerMeter, direction, map, vehicle, configuration, goal, goalDirection)) return null;
|
||||
|
||||
if (GoalToleranceChecker.IsSatisfied(start, goal, configuration, direction, goalDirection))
|
||||
return new MotionPrimitive(start, direction, curvaturePerMeter, 0d, Array.Empty<Pose2D>(), Array.Empty<double>(), true);
|
||||
|
||||
double pointStepMeters = Math.Min(configuration.IntegrationStepMeters,
|
||||
Math.Min(configuration.MaximumCollisionCheckStepMeters, map.ResolutionMeters / 2d));
|
||||
if (!NumericGuard.IsPositiveFinite(pointStepMeters)) return null;
|
||||
|
||||
var points = new List<Pose2D>();
|
||||
var bodyClearancesMeters = new List<double>();
|
||||
Pose2D previous = start;
|
||||
double actualLengthMeters = 0d;
|
||||
while (actualLengthMeters < configuration.PrimitiveLengthMeters)
|
||||
{
|
||||
double remainingLengthMeters = configuration.PrimitiveLengthMeters - actualLengthMeters;
|
||||
double stepMeters = Math.Min(pointStepMeters, remainingLengthMeters);
|
||||
if (!NumericGuard.IsPositiveFinite(stepMeters)) return null;
|
||||
|
||||
Pose2D next = Integrate(previous, curvaturePerMeter, direction, stepMeters);
|
||||
if (!IsFinitePose(next)) return null;
|
||||
if (!_collisionChecker.IsSweptMotionCollisionFree(previous, next, map, vehicle, stepMeters, out double bodyClearanceMeters)) return null;
|
||||
|
||||
actualLengthMeters += stepMeters;
|
||||
points.Add(next);
|
||||
bodyClearancesMeters.Add(bodyClearanceMeters);
|
||||
if (GoalToleranceChecker.IsSatisfied(next, goal, configuration, direction, goalDirection))
|
||||
return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, true);
|
||||
|
||||
previous = next;
|
||||
}
|
||||
|
||||
return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, false);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取从最大负曲率到最大正曲率均匀分布的候选曲率等级。
|
||||
/// 参数：vehicle 提供保守最大曲率；configuration 的曲率等级数必须为不小于 3 的奇数。
|
||||
/// 返回：输入无效时为空只读列表；有效时长度等于配置等级数且中间等级恒为零曲率。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> GetCurvatureLevels(VehicleParameters vehicle, HybridAStarConfiguration configuration)
|
||||
{
|
||||
if (configuration == null || configuration.CurvatureLevelCount < 3 || configuration.CurvatureLevelCount % 2 == 0 ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return Array.Empty<double>();
|
||||
|
||||
var levels = new double[configuration.CurvatureLevelCount];
|
||||
double increment = 2d * maximumCurvaturePerMeter / (levels.Length - 1d);
|
||||
for (int index = 0; index < levels.Length; index++) levels[index] = -maximumCurvaturePerMeter + increment * index;
|
||||
levels[levels.Length / 2] = 0d;
|
||||
return levels;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断两个曲率等级是否允许在相邻原语间直接切换。
|
||||
/// 参数：previousLevelIndex 与 nextLevelIndex 为从零开始的等级索引。
|
||||
/// 返回：两个索引均非负且最多相差一个等级时为 true。
|
||||
/// </summary>
|
||||
public static bool AreCurvatureLevelsAdjacent(int previousLevelIndex, int nextLevelIndex)
|
||||
{
|
||||
return previousLevelIndex >= 0 && nextLevelIndex >= 0 && Math.Abs(previousLevelIndex - nextLevelIndex) <= 1;
|
||||
}
|
||||
|
||||
private static bool IsValidInput(
|
||||
Pose2D start,
|
||||
double curvaturePerMeter,
|
||||
TravelDirection direction,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
HybridAStarConfiguration configuration,
|
||||
Pose2D goal,
|
||||
GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
if (!IsFinitePose(start) || !IsFinitePose(goal) || map == null || vehicle == null || configuration == null ||
|
||||
!NumericGuard.IsFinite(curvaturePerMeter) || !IsTravelDirection(direction) || !IsGoalDirection(goalDirection) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(map.ResolutionMeters) ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return false;
|
||||
|
||||
return Math.Abs(curvaturePerMeter) <= maximumCurvaturePerMeter;
|
||||
}
|
||||
|
||||
private static Pose2D Integrate(Pose2D previous, double curvaturePerMeter, TravelDirection direction, double stepMeters)
|
||||
{
|
||||
double signedDistanceMeters = direction == TravelDirection.Forward ? stepMeters : -stepMeters;
|
||||
double nextHeadingRadians = AngleMath.NormalizeRadians(previous.Heading + curvaturePerMeter * signedDistanceMeters);
|
||||
if (Math.Abs(curvaturePerMeter) < StraightCurvatureThreshold)
|
||||
{
|
||||
return new Pose2D(
|
||||
previous.X + signedDistanceMeters * Math.Cos(previous.Heading),
|
||||
previous.Y + signedDistanceMeters * Math.Sin(previous.Heading),
|
||||
nextHeadingRadians);
|
||||
}
|
||||
|
||||
return new Pose2D(
|
||||
previous.X + (Math.Sin(nextHeadingRadians) - Math.Sin(previous.Heading)) / curvaturePerMeter,
|
||||
previous.Y - (Math.Cos(nextHeadingRadians) - Math.Cos(previous.Heading)) / curvaturePerMeter,
|
||||
nextHeadingRadians);
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 将运动原语的长度、方向、曲率和净空转换为统一的等效米搜索代价。
|
||||
/// 此类型不修改搜索状态；换向标记必须由调用方依据相邻原语的方向关系提供。
|
||||
/// </summary>
|
||||
public sealed class SearchCostCalculator
|
||||
{
|
||||
/// <summary>创建使用调用时配置参数的等效米代价计算器。</summary>
|
||||
public SearchCostCalculator()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算一条运动原语的等效米增量代价。
|
||||
/// 参数：lengthMeters 为非负弧长 m；direction 为原语方向；isGearSwitch 表示该原语前是否换向；
|
||||
/// curvaturePerMeter 与 maximumCurvaturePerMeter 的单位为 1/m；curvatureLevelDelta 为相邻曲率等级差；
|
||||
/// bodyClearanceMeters 为非负车体保守净空 m，可为正无穷；configuration 提供非负权重和惩罚。
|
||||
/// 返回：有限且非负的等效米代价。
|
||||
/// 失败：任一数值、方向或权重无效时抛出 <see cref="ArgumentOutOfRangeException"/>。
|
||||
/// </summary>
|
||||
public double Calculate(
|
||||
double lengthMeters,
|
||||
TravelDirection direction,
|
||||
bool isGearSwitch,
|
||||
double curvaturePerMeter,
|
||||
double maximumCurvaturePerMeter,
|
||||
int curvatureLevelDelta,
|
||||
double bodyClearanceMeters,
|
||||
HybridAStarConfiguration configuration)
|
||||
{
|
||||
ValidateInput(lengthMeters, direction, curvaturePerMeter, maximumCurvaturePerMeter, bodyClearanceMeters, configuration);
|
||||
|
||||
double directionMultiplier = direction == TravelDirection.Reverse ? configuration.ReverseCostMultiplier : 1d;
|
||||
double normalizedCurvature = Math.Abs(curvaturePerMeter / maximumCurvaturePerMeter);
|
||||
double clearanceDeficit = GetClearanceDeficit(bodyClearanceMeters, configuration.ClearanceCostDistanceMeters);
|
||||
double levelDelta = Math.Abs((double)curvatureLevelDelta);
|
||||
double motionCost = lengthMeters * directionMultiplier *
|
||||
(1d + configuration.CurvatureMagnitudeWeight * normalizedCurvature +
|
||||
configuration.ClearanceCostWeight * clearanceDeficit);
|
||||
double gearSwitchCost = isGearSwitch ? configuration.GearSwitchPenaltyMeters : 0d;
|
||||
double curvatureChangeCost = configuration.CurvatureChangePenaltyMetersPerLevel * levelDelta;
|
||||
double totalCost = motionCost + gearSwitchCost + curvatureChangeCost;
|
||||
if (!NumericGuard.IsFinite(totalCost) || totalCost < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(lengthMeters), "The calculated cost must remain finite and non-negative.");
|
||||
|
||||
return totalCost;
|
||||
}
|
||||
|
||||
private static void ValidateInput(
|
||||
double lengthMeters,
|
||||
TravelDirection direction,
|
||||
double curvaturePerMeter,
|
||||
double maximumCurvaturePerMeter,
|
||||
double bodyClearanceMeters,
|
||||
HybridAStarConfiguration configuration)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(lengthMeters) || lengthMeters < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(lengthMeters));
|
||||
if (direction != TravelDirection.Forward && direction != TravelDirection.Reverse)
|
||||
throw new ArgumentOutOfRangeException(nameof(direction));
|
||||
if (!NumericGuard.IsFinite(curvaturePerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
|
||||
if (!NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(maximumCurvaturePerMeter));
|
||||
if (Math.Abs(curvaturePerMeter) > maximumCurvaturePerMeter)
|
||||
throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
|
||||
if (double.IsNaN(bodyClearanceMeters) || bodyClearanceMeters < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(bodyClearanceMeters));
|
||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||
|
||||
ValidateNonNegativeFinite(configuration.HeuristicWeight, nameof(configuration.HeuristicWeight));
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier))
|
||||
throw new ArgumentOutOfRangeException(nameof(configuration.ReverseCostMultiplier));
|
||||
ValidateNonNegativeFinite(configuration.GearSwitchPenaltyMeters, nameof(configuration.GearSwitchPenaltyMeters));
|
||||
ValidateNonNegativeFinite(configuration.CurvatureMagnitudeWeight, nameof(configuration.CurvatureMagnitudeWeight));
|
||||
ValidateNonNegativeFinite(configuration.CurvatureChangePenaltyMetersPerLevel, nameof(configuration.CurvatureChangePenaltyMetersPerLevel));
|
||||
ValidateNonNegativeFinite(configuration.ClearanceCostWeight, nameof(configuration.ClearanceCostWeight));
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters))
|
||||
throw new ArgumentOutOfRangeException(nameof(configuration.ClearanceCostDistanceMeters));
|
||||
}
|
||||
|
||||
private static void ValidateNonNegativeFinite(double value, string parameterName)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName);
|
||||
}
|
||||
|
||||
private static double GetClearanceDeficit(double bodyClearanceMeters, double clearanceCostDistanceMeters)
|
||||
{
|
||||
if (double.IsPositiveInfinity(bodyClearanceMeters)) return 0d;
|
||||
double deficit = 1d - bodyClearanceMeters / clearanceCostDistanceMeters;
|
||||
return deficit > 0d ? deficit : 0d;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,507 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
|
||||
/// <summary>Clumsy 粗路径手动测试可选择的固定场景。</summary>
|
||||
public enum CoarsePathTestScenario
|
||||
{
|
||||
/// <summary>明确允许的空地图直达场景。</summary>
|
||||
ExplicitEmpty,
|
||||
|
||||
/// <summary>由中央矩形阻断直线的绕行场景。</summary>
|
||||
RectangleDetour,
|
||||
|
||||
/// <summary>同时包含手工圆形、矩形与 TwoLeg 快照的多来源场景。</summary>
|
||||
ManualAndTwoLeg,
|
||||
|
||||
/// <summary>与矩形绕行输入完全一致，用于在同一服务中验证输入缓存命中。</summary>
|
||||
CacheHit,
|
||||
|
||||
/// <summary>起步前进、终点倒车进入的换向场景。</summary>
|
||||
ReverseGearSwitch,
|
||||
|
||||
/// <summary>由贯穿边界的障碍带分隔起终点的无解场景。</summary>
|
||||
NoFeasiblePath,
|
||||
}
|
||||
|
||||
/// <summary>手动障碍物输入支持的世界几何类型。</summary>
|
||||
public enum ManualCoarsePathObstacleKind
|
||||
{
|
||||
/// <summary>由世界中心和半径定义的圆形障碍物。</summary>
|
||||
Circle,
|
||||
|
||||
/// <summary>由世界中心、X 方向长度和 Y 方向宽度定义的轴对齐矩形障碍物。</summary>
|
||||
AxisAlignedRectangle,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 手动粗路径测试的不可变障碍物输入。
|
||||
/// 所有中心和尺寸均使用世界 mm；矩形始终与世界坐标轴平行，不包含旋转角。
|
||||
/// </summary>
|
||||
public sealed class ManualCoarsePathObstacle
|
||||
{
|
||||
private ManualCoarsePathObstacle(ManualCoarsePathObstacleKind kind, double centerXMillimeters,
|
||||
double centerYMillimeters, double sizeXMillimeters, double sizeYMillimeters)
|
||||
{
|
||||
Kind = kind;
|
||||
CenterXMillimeters = centerXMillimeters;
|
||||
CenterYMillimeters = centerYMillimeters;
|
||||
SizeXMillimeters = sizeXMillimeters;
|
||||
SizeYMillimeters = sizeYMillimeters;
|
||||
}
|
||||
|
||||
/// <summary>障碍物的支持几何类型。</summary>
|
||||
public ManualCoarsePathObstacleKind Kind { get; }
|
||||
|
||||
/// <summary>几何中心世界 X 坐标，单位 mm。</summary>
|
||||
public double CenterXMillimeters { get; }
|
||||
|
||||
/// <summary>几何中心世界 Y 坐标，单位 mm。</summary>
|
||||
public double CenterYMillimeters { get; }
|
||||
|
||||
/// <summary>圆形时为半径，矩形时为 X 方向长度；单位 mm。</summary>
|
||||
public double SizeXMillimeters { get; }
|
||||
|
||||
/// <summary>圆形时为半径，矩形时为 Y 方向宽度；单位 mm。</summary>
|
||||
public double SizeYMillimeters { get; }
|
||||
|
||||
/// <summary>创建圆形障碍物。参数：圆心和半径均使用世界 mm，半径必须为有限正数。</summary>
|
||||
public static ManualCoarsePathObstacle Circle(double centerXMillimeters, double centerYMillimeters,
|
||||
double radiusMillimeters)
|
||||
{
|
||||
EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
|
||||
EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
|
||||
EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
|
||||
return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.Circle, centerXMillimeters,
|
||||
centerYMillimeters, radiusMillimeters, radiusMillimeters);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建轴对齐矩形障碍物。
|
||||
/// 参数：中心、X 方向长度和 Y 方向宽度均使用世界 mm；两个尺寸必须为有限正数。
|
||||
/// </summary>
|
||||
public static ManualCoarsePathObstacle AxisAlignedRectangle(double centerXMillimeters,
|
||||
double centerYMillimeters, double lengthXMillimeters, double widthYMillimeters)
|
||||
{
|
||||
EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
|
||||
EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
|
||||
EnsurePositiveFinite(lengthXMillimeters, nameof(lengthXMillimeters));
|
||||
EnsurePositiveFinite(widthYMillimeters, nameof(widthYMillimeters));
|
||||
return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.AxisAlignedRectangle,
|
||||
centerXMillimeters, centerYMillimeters, lengthXMillimeters, widthYMillimeters);
|
||||
}
|
||||
|
||||
private static void EnsureFinite(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
|
||||
}
|
||||
|
||||
private static void EnsurePositiveFinite(double value, string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
if (value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clumsy 粗路径测试的纯输入工厂。
|
||||
/// 固定场景不读取 UI、传感器、定位或时钟；传入 AMR 位姿的手动入口仅在此处完成世界 mm/deg 到核心 m/rad 的转换。
|
||||
/// </summary>
|
||||
public static class CoarsePathScenarioFactory
|
||||
{
|
||||
private const float MapXMinMillimeters = 0f;
|
||||
private const float MapXMaxMillimeters = 6000f;
|
||||
private const float MapYMinMillimeters = 0f;
|
||||
private const float MapYMaxMillimeters = 4000f;
|
||||
private const float ResolutionMillimeters = 50f;
|
||||
private const double MillimetersPerMeter = 1000d;
|
||||
private const double DegreesToRadians = Math.PI / 180d;
|
||||
private const double ManualMapPaddingMillimeters = 8000d;
|
||||
private const int MaximumManualObstacleCount = 20;
|
||||
|
||||
/// <summary>
|
||||
/// 创建一个新的固定测试业务请求。
|
||||
/// 返回：每次调用都返回独立的可变请求对象，供调用方安全地传入同一个长期存活的规划服务。
|
||||
/// </summary>
|
||||
public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario)
|
||||
{
|
||||
return CreateCore(scenario, null);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建以当前 AMR 世界位姿为起点的固定测试业务请求。
|
||||
/// 参数：X/Y 使用世界 mm，航向使用 deg；地图、目标和障碍物仅随 AMR 坐标平移，TwoLeg 朝向保持不变。
|
||||
/// </summary>
|
||||
public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario,
|
||||
double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)
|
||||
{
|
||||
return CreateCore(scenario,
|
||||
new FixedScenarioAnchor(amrXMillimeters, amrYMillimeters, amrHeadingDegrees));
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateCore(CoarsePathTestScenario scenario, FixedScenarioAnchor anchor)
|
||||
{
|
||||
switch (scenario)
|
||||
{
|
||||
case CoarsePathTestScenario.ExplicitEmpty:
|
||||
return CreateExplicitEmpty(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
||||
case CoarsePathTestScenario.RectangleDetour:
|
||||
case CoarsePathTestScenario.CacheHit:
|
||||
return CreateRectangleDetour(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
||||
case CoarsePathTestScenario.ManualAndTwoLeg:
|
||||
return CreateManualAndTwoLeg(FixedScenarioTransform.From(1000d, 1000d, 0d, anchor));
|
||||
case CoarsePathTestScenario.ReverseGearSwitch:
|
||||
return CreateReverseGearSwitch(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
||||
case CoarsePathTestScenario.NoFeasiblePath:
|
||||
return CreateNoFeasiblePath(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建传入 AMR 世界位姿和手动世界终点的空图演示请求。
|
||||
/// 参数：X/Y 使用世界 mm，航向使用 deg；返回请求中的 <see cref="Pose2D"/> 使用世界 m/rad。
|
||||
/// 注意：这是坐标、路径和取消流程的演示空图，不能表示现场不存在障碍物。
|
||||
/// </summary>
|
||||
public static CoarsePathPlanningJob CreateManualGoalDemo(
|
||||
double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
|
||||
double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees)
|
||||
{
|
||||
return CreateManualObstacleDemo(startXMillimeters, startYMillimeters, startHeadingDegrees,
|
||||
goalXMillimeters, goalYMillimeters, goalHeadingDegrees,
|
||||
Array.Empty<ManualCoarsePathObstacle>(), 0L);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建传入 AMR 世界位姿、手动世界终点和手动障碍物快照的测试请求。
|
||||
/// 参数：位姿 X/Y、障碍物中心和尺寸使用世界 mm，航向使用 deg；返回的 <see cref="Pose2D"/> 使用 m/rad。
|
||||
/// 障碍物非空时 obstacleSnapshotVersion 必须为正数，以避免长期服务错误复用旧地图；零障碍物才创建显式空图演示。
|
||||
/// </summary>
|
||||
public static CoarsePathPlanningJob CreateManualObstacleDemo(
|
||||
double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
|
||||
double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees,
|
||||
IReadOnlyList<ManualCoarsePathObstacle> obstacles, long obstacleSnapshotVersion)
|
||||
{
|
||||
EnsureFinite(startXMillimeters, nameof(startXMillimeters));
|
||||
EnsureFinite(startYMillimeters, nameof(startYMillimeters));
|
||||
EnsureFinite(startHeadingDegrees, nameof(startHeadingDegrees));
|
||||
EnsureFinite(goalXMillimeters, nameof(goalXMillimeters));
|
||||
EnsureFinite(goalYMillimeters, nameof(goalYMillimeters));
|
||||
EnsureFinite(goalHeadingDegrees, nameof(goalHeadingDegrees));
|
||||
|
||||
if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
|
||||
if (obstacles.Count > MaximumManualObstacleCount)
|
||||
throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle count exceeds the supported limit.");
|
||||
if (obstacles.Count != 0 && obstacleSnapshotVersion <= 0L)
|
||||
throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version.");
|
||||
|
||||
return CreateJob(
|
||||
CreateManualDemoMap(startXMillimeters, startYMillimeters, goalXMillimeters, goalYMillimeters,
|
||||
obstacles, obstacleSnapshotVersion),
|
||||
ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
|
||||
ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees),
|
||||
null,
|
||||
GoalDirectionConstraint.Any);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateExplicitEmpty(FixedScenarioTransform transform)
|
||||
{
|
||||
return CreateJob(
|
||||
CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
|
||||
transform.Pose(1d, 2d, 0d),
|
||||
transform.Pose(5d, 2d, 0d),
|
||||
null,
|
||||
GoalDirectionConstraint.Forward);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateRectangleDetour(FixedScenarioTransform transform)
|
||||
{
|
||||
IMapObstacleSource[] sources =
|
||||
{
|
||||
new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(transform.X(2700f), transform.X(3300f),
|
||||
transform.Y(1200f), transform.Y(2800f)),
|
||||
}),
|
||||
};
|
||||
CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
|
||||
transform.Pose(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
|
||||
// 固定绕行场景保留最优启发式；30 秒覆盖较慢测试环境，UI 仍可随时取消。
|
||||
job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
|
||||
return job;
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateManualAndTwoLeg(FixedScenarioTransform transform)
|
||||
{
|
||||
IMapObstacleSource[] sources =
|
||||
{
|
||||
new ManualObstacleSource("manual", 2L, true, new IMapObstacle[]
|
||||
{
|
||||
new CircleObstacle(transform.X(2400f), transform.Y(1300f), 220f),
|
||||
new AxisAlignedRectangleObstacle(transform.X(3000f), transform.X(3600f),
|
||||
transform.Y(2000f), transform.Y(2600f)),
|
||||
}),
|
||||
new TwoLegObstacleSource("two-leg", 1L, true, new TwoLegProjectionInput(true,
|
||||
transform.X(3900f), transform.Y(2500f), 0d,
|
||||
-180f, -180f, -180f, 180f, 140f, "P1 fixed TwoLeg snapshot.")),
|
||||
};
|
||||
CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 1d, 0d),
|
||||
transform.Pose(5d, 3d, 0d), null, GoalDirectionConstraint.Forward);
|
||||
// 多来源场景的最优绕行会受机器负载影响；放宽演示总预算但保留全部碰撞与目标判定。
|
||||
job.Configuration.SearchTimeout = TimeSpan.FromSeconds(15d);
|
||||
return job;
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateReverseGearSwitch(FixedScenarioTransform transform)
|
||||
{
|
||||
return CreateJob(
|
||||
CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
|
||||
transform.Pose(1d, 2d, 0d),
|
||||
transform.Pose(4d, 2d, 0d),
|
||||
TravelDirection.Forward,
|
||||
GoalDirectionConstraint.Reverse);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateNoFeasiblePath(FixedScenarioTransform transform)
|
||||
{
|
||||
IMapObstacleSource[] sources =
|
||||
{
|
||||
new ManualObstacleSource("manual", 3L, true, new IMapObstacle[]
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(transform.X(2900f), transform.X(3100f),
|
||||
transform.Y(0f), transform.Y(4000f)),
|
||||
}),
|
||||
};
|
||||
return CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
|
||||
transform.Pose(5d, 2d, 0d),
|
||||
null, GoalDirectionConstraint.Forward);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateJob(PlanningMapRequest mapRequest, Pose2D start, Pose2D goal,
|
||||
TravelDirection? startDirection, GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
return new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = mapRequest,
|
||||
Start = start,
|
||||
Goal = goal,
|
||||
Vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.80d,
|
||||
WidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
},
|
||||
Configuration = new HybridAStarConfiguration(),
|
||||
StartDirection = startDirection,
|
||||
GoalDirection = goalDirection,
|
||||
};
|
||||
}
|
||||
|
||||
private static PlanningMapRequest CreateMap(bool allowExplicitEmptyMap, IReadOnlyList<IMapObstacleSource> sources,
|
||||
FixedScenarioTransform transform)
|
||||
{
|
||||
return new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(transform.X(MapXMinMillimeters), transform.X(MapXMaxMillimeters),
|
||||
transform.Y(MapYMinMillimeters), transform.Y(MapYMaxMillimeters)),
|
||||
ResolutionMm = ResolutionMillimeters,
|
||||
ObstacleSources = sources,
|
||||
AllowExplicitEmptyMap = allowExplicitEmptyMap,
|
||||
};
|
||||
}
|
||||
|
||||
private sealed class FixedScenarioAnchor
|
||||
{
|
||||
public FixedScenarioAnchor(double xMillimeters, double yMillimeters, double headingDegrees)
|
||||
{
|
||||
EnsureFinite(xMillimeters, nameof(xMillimeters));
|
||||
EnsureFinite(yMillimeters, nameof(yMillimeters));
|
||||
EnsureFinite(headingDegrees, nameof(headingDegrees));
|
||||
XMillimeters = xMillimeters;
|
||||
YMillimeters = yMillimeters;
|
||||
HeadingRadians = NormalizeRadians((headingDegrees % 360d) * DegreesToRadians);
|
||||
}
|
||||
|
||||
public double XMillimeters { get; }
|
||||
public double YMillimeters { get; }
|
||||
public double HeadingRadians { get; }
|
||||
}
|
||||
|
||||
private sealed class FixedScenarioTransform
|
||||
{
|
||||
private FixedScenarioTransform(double deltaXMillimeters, double deltaYMillimeters, double headingDeltaRadians)
|
||||
{
|
||||
DeltaXMillimeters = deltaXMillimeters;
|
||||
DeltaYMillimeters = deltaYMillimeters;
|
||||
HeadingDeltaRadians = headingDeltaRadians;
|
||||
}
|
||||
|
||||
public double DeltaXMillimeters { get; }
|
||||
public double DeltaYMillimeters { get; }
|
||||
public double HeadingDeltaRadians { get; }
|
||||
|
||||
public static FixedScenarioTransform From(double baselineStartXMillimeters,
|
||||
double baselineStartYMillimeters, double baselineStartHeadingRadians, FixedScenarioAnchor anchor)
|
||||
{
|
||||
if (anchor == null) return new FixedScenarioTransform(0d, 0d, 0d);
|
||||
return new FixedScenarioTransform(anchor.XMillimeters - baselineStartXMillimeters,
|
||||
anchor.YMillimeters - baselineStartYMillimeters,
|
||||
NormalizeRadians(anchor.HeadingRadians - baselineStartHeadingRadians));
|
||||
}
|
||||
|
||||
public float X(float value)
|
||||
{
|
||||
return ToFiniteFloat(value + DeltaXMillimeters, nameof(value));
|
||||
}
|
||||
|
||||
public float Y(float value)
|
||||
{
|
||||
return ToFiniteFloat(value + DeltaYMillimeters, nameof(value));
|
||||
}
|
||||
|
||||
public Pose2D Pose(double xMeters, double yMeters, double headingRadians)
|
||||
{
|
||||
return new Pose2D((xMeters * MillimetersPerMeter + DeltaXMillimeters) / MillimetersPerMeter,
|
||||
(yMeters * MillimetersPerMeter + DeltaYMillimeters) / MillimetersPerMeter,
|
||||
NormalizeRadians(headingRadians + HeadingDeltaRadians));
|
||||
}
|
||||
}
|
||||
|
||||
private static PlanningMapRequest CreateManualDemoMap(double startXMillimeters, double startYMillimeters,
|
||||
double goalXMillimeters, double goalYMillimeters, IReadOnlyList<ManualCoarsePathObstacle> obstacles,
|
||||
long obstacleSnapshotVersion)
|
||||
{
|
||||
double minimumX = Math.Min(startXMillimeters, goalXMillimeters);
|
||||
double maximumX = Math.Max(startXMillimeters, goalXMillimeters);
|
||||
double minimumY = Math.Min(startYMillimeters, goalYMillimeters);
|
||||
double maximumY = Math.Max(startYMillimeters, goalYMillimeters);
|
||||
for (int index = 0; index < obstacles.Count; index++)
|
||||
{
|
||||
ManualCoarsePathObstacle obstacle = obstacles[index] ??
|
||||
throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
|
||||
double halfX;
|
||||
double halfY;
|
||||
switch (obstacle.Kind)
|
||||
{
|
||||
case ManualCoarsePathObstacleKind.Circle:
|
||||
EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
|
||||
halfX = obstacle.SizeXMillimeters;
|
||||
halfY = obstacle.SizeYMillimeters;
|
||||
break;
|
||||
case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
|
||||
EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
|
||||
EnsurePositiveFinite(obstacle.SizeYMillimeters, nameof(obstacles));
|
||||
halfX = obstacle.SizeXMillimeters / 2d;
|
||||
halfY = obstacle.SizeYMillimeters / 2d;
|
||||
break;
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
|
||||
}
|
||||
|
||||
minimumX = Math.Min(minimumX, obstacle.CenterXMillimeters - halfX);
|
||||
maximumX = Math.Max(maximumX, obstacle.CenterXMillimeters + halfX);
|
||||
minimumY = Math.Min(minimumY, obstacle.CenterYMillimeters - halfY);
|
||||
maximumY = Math.Max(maximumY, obstacle.CenterYMillimeters + halfY);
|
||||
}
|
||||
|
||||
float xMin = ToGridLowerBound(minimumX - ManualMapPaddingMillimeters);
|
||||
float xMax = ToGridUpperBound(maximumX + ManualMapPaddingMillimeters);
|
||||
float yMin = ToGridLowerBound(minimumY - ManualMapPaddingMillimeters);
|
||||
float yMax = ToGridUpperBound(maximumY + ManualMapPaddingMillimeters);
|
||||
bool isExplicitEmptyMap = obstacles.Count == 0;
|
||||
return new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(xMin, xMax, yMin, yMax),
|
||||
ResolutionMm = ResolutionMillimeters,
|
||||
ObstacleSources = isExplicitEmptyMap ? Array.Empty<IMapObstacleSource>() :
|
||||
new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("manual-user-input", obstacleSnapshotVersion, true,
|
||||
ConvertManualObstacles(obstacles)),
|
||||
},
|
||||
AllowExplicitEmptyMap = isExplicitEmptyMap,
|
||||
};
|
||||
}
|
||||
|
||||
private static IMapObstacle[] ConvertManualObstacles(IReadOnlyList<ManualCoarsePathObstacle> obstacles)
|
||||
{
|
||||
var result = new IMapObstacle[obstacles.Count];
|
||||
for (int index = 0; index < obstacles.Count; index++)
|
||||
{
|
||||
ManualCoarsePathObstacle obstacle = obstacles[index] ??
|
||||
throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
|
||||
switch (obstacle.Kind)
|
||||
{
|
||||
case ManualCoarsePathObstacleKind.Circle:
|
||||
result[index] = new CircleObstacle(ToFiniteFloat(obstacle.CenterXMillimeters, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.CenterYMillimeters, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.SizeXMillimeters, nameof(obstacles)));
|
||||
break;
|
||||
case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
|
||||
double halfX = obstacle.SizeXMillimeters / 2d;
|
||||
double halfY = obstacle.SizeYMillimeters / 2d;
|
||||
result[index] = new AxisAlignedRectangleObstacle(
|
||||
ToFiniteFloat(obstacle.CenterXMillimeters - halfX, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.CenterXMillimeters + halfX, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.CenterYMillimeters - halfY, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.CenterYMillimeters + halfY, nameof(obstacles)));
|
||||
break;
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
private static Pose2D ToPose(double xMillimeters, double yMillimeters, double headingDegrees)
|
||||
{
|
||||
return new Pose2D(xMillimeters / MillimetersPerMeter, yMillimeters / MillimetersPerMeter,
|
||||
headingDegrees * DegreesToRadians);
|
||||
}
|
||||
|
||||
private static double NormalizeRadians(double angle)
|
||||
{
|
||||
double normalized = angle % (2d * Math.PI);
|
||||
if (normalized <= -Math.PI) return normalized + 2d * Math.PI;
|
||||
return normalized > Math.PI ? normalized - 2d * Math.PI : normalized;
|
||||
}
|
||||
|
||||
private static float ToGridLowerBound(double millimeters)
|
||||
{
|
||||
double rounded = Math.Floor(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
|
||||
return ToFiniteFloat(rounded, nameof(millimeters));
|
||||
}
|
||||
|
||||
private static float ToGridUpperBound(double millimeters)
|
||||
{
|
||||
double rounded = Math.Ceiling(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
|
||||
return ToFiniteFloat(rounded, nameof(millimeters));
|
||||
}
|
||||
|
||||
private static float ToFiniteFloat(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate.");
|
||||
return (float)value;
|
||||
}
|
||||
|
||||
private static void EnsureFinite(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
|
||||
}
|
||||
|
||||
private static void EnsurePositiveFinite(double value, string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
if (value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,628 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Globalization;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// 显式空图的粗路径规划测试入口。
|
||||
/// 只负责创建纯规划请求；规划、取消与可视化均由共享执行器处理，不会向底盘发送任何命令。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-显式空图")]
|
||||
public sealed class CoarsePathExplicitEmptyTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ExplicitEmpty, "显式空图");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 单矩形绕行的粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-单矩形绕行")]
|
||||
// TODO：#在该测试下发生了红色矩形栅格碰撞但依然规划成功，需要进一步核实与确认
|
||||
public sealed class CoarsePathRectangleDetourTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.RectangleDetour, "单矩形绕行");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 手工圆形、矩形和 TwoLeg 快照组合的粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-多来源障碍")]
|
||||
public sealed class CoarsePathManualAndTwoLegTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ManualAndTwoLeg, "多来源障碍");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 重复输入地图缓存命中的粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-缓存命中")]
|
||||
public sealed class CoarsePathCacheHitTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.CacheHit, "缓存命中");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 前进起步、倒车到达并显示换向点的粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-倒车换向")]
|
||||
public sealed class CoarsePathReverseGearSwitchTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ReverseGearSwitch, "倒车换向");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 障碍带完全隔开起终点的无解粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-无解")]
|
||||
public sealed class CoarsePathNoFeasiblePathTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.NoFeasiblePath, "无解障碍带");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用当前 AMR 车身几何中心位姿和人工终点的粗路径规划演示入口。
|
||||
/// 输入的 X/Y 使用世界 mm、航向使用 deg；进入规划核心前由场景工厂一次性转换为 m/rad。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划")]
|
||||
public sealed class CoarsePathPlanningTest : MovementTest
|
||||
{
|
||||
private const int MaximumManualObstacleCount = 20;
|
||||
private static long _nextManualObstacleSnapshotVersion;
|
||||
|
||||
/// <summary>
|
||||
/// 读取一次 AMR 当前世界位姿、手动终点和障碍物快照后启动规划。
|
||||
/// 注意：getCartLocation 在无定位时可能阻塞；全部输入会在启动后台任务前冻结，不会被规划线程重复读取。
|
||||
/// </summary>
|
||||
public override void Test()
|
||||
{
|
||||
try
|
||||
{
|
||||
var amrPose = DetourInterface.getCartLocation();
|
||||
double goalXmm = ReadFiniteInput("粗路径终点 X（世界 mm）");
|
||||
double goalYmm = ReadFiniteInput("粗路径终点 Y（世界 mm）");
|
||||
double goalHeadingDeg = ReadFiniteInput("粗路径终点航向（世界 deg）");
|
||||
TimeSpan searchTimeout = ReadPositiveTimeoutInput("粗路径规划总超时（秒，必须大于 0）");
|
||||
IReadOnlyList<ManualCoarsePathObstacle> obstacles = ReadManualObstacles();
|
||||
long snapshotVersion = obstacles.Count == 0 ? 0L :
|
||||
Interlocked.Increment(ref _nextManualObstacleSnapshotVersion);
|
||||
CoarsePathPlanningJob job = CoarsePathScenarioFactory.CreateManualObstacleDemo(
|
||||
amrPose.x, amrPose.y, amrPose.th, goalXmm, goalYmm, goalHeadingDeg,
|
||||
obstacles, snapshotVersion);
|
||||
job.Configuration.SearchTimeout = searchTimeout;
|
||||
CoarsePathPlanningTestRunner.Run("AMR 位姿 + 手动终点 + 手动障碍物", job);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
CoarsePathPlanningTestRunner.ShowInputFailure(exception);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
|
||||
private static IReadOnlyList<ManualCoarsePathObstacle> ReadManualObstacles()
|
||||
{
|
||||
int count = ReadBoundedIntegerInput("手动障碍物数量（0-20）", 0, MaximumManualObstacleCount);
|
||||
var obstacles = new List<ManualCoarsePathObstacle>(count);
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
string label = "障碍物 " + (index + 1);
|
||||
int kind = ReadBoundedIntegerInput(label + " 类型（1圆形，2矩形）", 1, 2);
|
||||
double centerXmm = ReadFiniteInput(label + " 中心 X（世界 mm）");
|
||||
double centerYmm = ReadFiniteInput(label + " 中心 Y（世界 mm）");
|
||||
if (kind == 1)
|
||||
{
|
||||
double radiusMm = ReadPositiveFiniteInput(label + " 半径 r（mm）");
|
||||
obstacles.Add(ManualCoarsePathObstacle.Circle(centerXmm, centerYmm, radiusMm));
|
||||
}
|
||||
else
|
||||
{
|
||||
double lengthXmm = ReadPositiveFiniteInput(label + " X方向长度（mm）");
|
||||
double widthYmm = ReadPositiveFiniteInput(label + " Y方向宽度（mm）");
|
||||
obstacles.Add(ManualCoarsePathObstacle.AxisAlignedRectangle(centerXmm, centerYmm,
|
||||
lengthXmm, widthYmm));
|
||||
}
|
||||
}
|
||||
return obstacles;
|
||||
}
|
||||
|
||||
private static int ReadBoundedIntegerInput(string prompt, int minimum, int maximum)
|
||||
{
|
||||
object raw = UI.GetInput(prompt);
|
||||
string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
|
||||
int value;
|
||||
if (!int.TryParse(text, NumberStyles.Integer, CultureInfo.CurrentCulture, out value) &&
|
||||
!int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out value))
|
||||
throw new ArgumentException("输入必须是整数：" + prompt);
|
||||
if (value < minimum || value > maximum)
|
||||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static double ReadPositiveFiniteInput(string prompt)
|
||||
{
|
||||
double value = ReadFiniteInput(prompt);
|
||||
if (value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static TimeSpan ReadPositiveTimeoutInput(string prompt)
|
||||
{
|
||||
double timeoutSeconds = ReadFiniteInput(prompt);
|
||||
if (timeoutSeconds <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
|
||||
try
|
||||
{
|
||||
return TimeSpan.FromSeconds(timeoutSeconds);
|
||||
}
|
||||
catch (OverflowException)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
|
||||
}
|
||||
}
|
||||
|
||||
private static double ReadFiniteInput(string prompt)
|
||||
{
|
||||
object raw = UI.GetInput(prompt);
|
||||
string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
|
||||
double value;
|
||||
if (!double.TryParse(text, NumberStyles.Float, CultureInfo.CurrentCulture, out value) &&
|
||||
!double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value))
|
||||
throw new ArgumentException("输入必须是有限数字：" + prompt);
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
throw new ArgumentException("输入必须是有限数字：" + prompt);
|
||||
return value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 粗路径 MovementTest 的共享后台会话、停止和绘制实现。
|
||||
/// 同一时刻只允许一个会话绘制；启动新会话或停止时会取消旧会话，但不会等待旧任务退出。
|
||||
/// </summary>
|
||||
internal static class CoarsePathPlanningTestRunner
|
||||
{
|
||||
private const string PainterLayerName = "CoarsePathPlanningV1";
|
||||
private const float MillimetersPerMeter = 1000f;
|
||||
private const int MaximumVisibleGridLines = 100;
|
||||
|
||||
private static readonly object SessionSync = new object();
|
||||
private static readonly CoarsePathPlanningService PlanningService = new CoarsePathPlanningService();
|
||||
private static readonly Painter Painter = UI.GetPainter(PainterLayerName, true);
|
||||
|
||||
private static CancellationTokenSource _activeCancellation;
|
||||
private static Task<CoarsePathPlanningJobResult> _activeTask;
|
||||
private static long _nextSessionId;
|
||||
private static long _activeSessionId;
|
||||
|
||||
/// <summary>
|
||||
/// 固定场景启动时冻结的 AMR 位姿。规划后台不会重新读取定位，确保输入一致。
|
||||
/// </summary>
|
||||
private sealed class AmrPoseSnapshot
|
||||
{
|
||||
public AmrPoseSnapshot(double xMillimeters, double yMillimeters, double headingDegrees)
|
||||
{
|
||||
EnsureFiniteAmrValue(xMillimeters, "X");
|
||||
EnsureFiniteAmrValue(yMillimeters, "Y");
|
||||
EnsureFiniteAmrValue(headingDegrees, "航向");
|
||||
XMillimeters = xMillimeters;
|
||||
YMillimeters = yMillimeters;
|
||||
HeadingDegrees = headingDegrees;
|
||||
}
|
||||
|
||||
public double XMillimeters { get; }
|
||||
public double YMillimeters { get; }
|
||||
public double HeadingDegrees { get; }
|
||||
|
||||
public string DisplayText
|
||||
{
|
||||
get
|
||||
{
|
||||
return "AMR 起点：X=" + XMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
|
||||
" mm，Y=" + YMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
|
||||
" mm，航向=" + HeadingDegrees.ToString("F1", CultureInfo.InvariantCulture) + " deg";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建指定固定场景并将其提交给共享后台服务。
|
||||
/// </summary>
|
||||
internal static void RunScenario(CoarsePathTestScenario scenario, string scenarioName)
|
||||
{
|
||||
try
|
||||
{
|
||||
var pose = DetourInterface.getCartLocation();
|
||||
if (ReferenceEquals(pose, null)) throw new ArgumentException("AMR 位姿为空。");
|
||||
var snapshot = new AmrPoseSnapshot(pose.x, pose.y, pose.th);
|
||||
CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenario,
|
||||
snapshot.XMillimeters, snapshot.YMillimeters, snapshot.HeadingDegrees);
|
||||
Run(scenarioName, job, snapshot);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
ShowInputFailure(new ArgumentException("AMR 位姿不可用：" + exception.Message, exception));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提交已经冻结输入的一次规划请求。调用立即返回，结果只会由对应会话的完成回调绘制。
|
||||
/// </summary>
|
||||
internal static void Run(string scenarioName, CoarsePathPlanningJob job)
|
||||
{
|
||||
Run(scenarioName, job, null);
|
||||
}
|
||||
|
||||
private static void Run(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
|
||||
{
|
||||
if (job == null) throw new ArgumentNullException(nameof(job));
|
||||
|
||||
var cancellation = new CancellationTokenSource();
|
||||
CancellationTokenSource previousCancellation;
|
||||
long sessionId;
|
||||
lock (SessionSync)
|
||||
{
|
||||
previousCancellation = _activeCancellation;
|
||||
_activeCancellation = cancellation;
|
||||
_activeTask = null;
|
||||
sessionId = ++_nextSessionId;
|
||||
_activeSessionId = sessionId;
|
||||
}
|
||||
|
||||
// 先发布新会话编号，再取消旧任务，避免旧完成回调覆盖新画面。
|
||||
if (previousCancellation != null) previousCancellation.Cancel();
|
||||
Painter.Clear();
|
||||
DrawPending(scenarioName, job, amrPose);
|
||||
|
||||
Task<CoarsePathPlanningJobResult> task = Task.Run(() => PlanningService.Plan(job, cancellation.Token));
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (_activeSessionId == sessionId) _activeTask = task;
|
||||
}
|
||||
|
||||
_ = task.ContinueWith(completed => Finish(sessionId, scenarioName, job, amrPose, cancellation, completed),
|
||||
TaskScheduler.Default);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 取消当前会话并清空专用图层；不等待后台任务结束。
|
||||
/// 已取消任务的完成回调只释放资源，不再记录或绘制结果。
|
||||
/// </summary>
|
||||
internal static void Stop()
|
||||
{
|
||||
CancellationTokenSource cancellation;
|
||||
lock (SessionSync)
|
||||
{
|
||||
cancellation = _activeCancellation;
|
||||
_activeCancellation = null;
|
||||
_activeTask = null;
|
||||
_activeSessionId = 0;
|
||||
}
|
||||
|
||||
if (cancellation != null) cancellation.Cancel();
|
||||
Painter.Clear();
|
||||
Hedingben.ToastText("粗路径规划已请求停止。", PainterLayerName);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 显示输入读取或校验失败，且不改变现有规划任务。
|
||||
/// </summary>
|
||||
internal static void ShowInputFailure(Exception exception)
|
||||
{
|
||||
string message = exception == null ? "未知输入错误。" : exception.Message;
|
||||
Painter.DrawText(Color.LightYellow, "粗路径规划未启动：" + message, 0f, 0f);
|
||||
Hedingben.ToastText("粗路径规划未启动：" + message, PainterLayerName);
|
||||
}
|
||||
|
||||
private static void Finish(long sessionId, string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
|
||||
CancellationTokenSource cancellation, Task<CoarsePathPlanningJobResult> completed)
|
||||
{
|
||||
try
|
||||
{
|
||||
CoarsePathPlanningJobResult result = completed.GetAwaiter().GetResult();
|
||||
bool isCurrent;
|
||||
lock (SessionSync)
|
||||
{
|
||||
isCurrent = _activeSessionId == sessionId;
|
||||
if (isCurrent)
|
||||
{
|
||||
_activeTask = null;
|
||||
_activeCancellation = null;
|
||||
}
|
||||
}
|
||||
|
||||
if (!isCurrent) return;
|
||||
DrawResult(scenarioName, job, amrPose, result);
|
||||
Hedingben.ToastText(BuildToastMessage(scenarioName, result), PainterLayerName);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
bool isCurrent;
|
||||
lock (SessionSync)
|
||||
{
|
||||
isCurrent = _activeSessionId == sessionId;
|
||||
if (isCurrent)
|
||||
{
|
||||
_activeTask = null;
|
||||
_activeCancellation = null;
|
||||
}
|
||||
}
|
||||
|
||||
if (isCurrent)
|
||||
Hedingben.ToastText("粗路径规划任务异常：" + exception.GetType().Name + "。" + exception.Message,
|
||||
PainterLayerName);
|
||||
}
|
||||
finally
|
||||
{
|
||||
cancellation.Dispose();
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawPending(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
|
||||
{
|
||||
DrawPose(job.Start, Color.LimeGreen, "起点");
|
||||
DrawPose(job.Goal, Color.Orange, "终点");
|
||||
Painter.DrawText(Color.LightGray, "场景：" + scenarioName + "（规划中）", 0f, 0f);
|
||||
if (amrPose != null) Painter.DrawText(Color.LightGray, amrPose.DisplayText, 0f, -120f);
|
||||
}
|
||||
|
||||
private static void DrawResult(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
|
||||
CoarsePathPlanningJobResult result)
|
||||
{
|
||||
Painter.Clear();
|
||||
PlanningGridMap map = result.MapResult.Map;
|
||||
if (map != null) DrawMap(map);
|
||||
|
||||
DrawPose(job.Start, Color.LimeGreen, "起点");
|
||||
DrawGoal(job.Goal, job.Configuration, Color.Orange);
|
||||
if (result.PlanningResult.Status == PlanningStatus.Success)
|
||||
DrawPath(result.PlanningResult, job.Vehicle);
|
||||
|
||||
DrawLegend(map);
|
||||
DrawStatus(scenarioName, job, result, map, amrPose);
|
||||
}
|
||||
|
||||
private static void DrawMap(PlanningGridMap map)
|
||||
{
|
||||
float xMin = map.Bounds.XMin;
|
||||
float xMax = map.Bounds.XMax;
|
||||
float yMin = map.Bounds.YMin;
|
||||
float yMax = map.Bounds.YMax;
|
||||
float resolution = map.ResolutionMm;
|
||||
int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) / (double)MaximumVisibleGridLines));
|
||||
|
||||
// 真实 ResolutionMm 决定网格位置，gridStride 只影响显示抽稀。
|
||||
for (int col = 0; col <= map.Cols; col += gridStride)
|
||||
{
|
||||
float x = Math.Min(xMax, xMin + col * resolution);
|
||||
Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), x, yMin, x, yMax, width: 1);
|
||||
}
|
||||
for (int row = 0; row <= map.Rows; row += gridStride)
|
||||
{
|
||||
float y = Math.Min(yMax, yMin + row * resolution);
|
||||
Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
|
||||
}
|
||||
|
||||
DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
|
||||
if (xMin <= 0f && 0f < xMax) Painter.DrawLine(Color.DimGray, 0f, yMin, 0f, yMax, width: 2);
|
||||
if (yMin <= 0f && 0f < yMax) Painter.DrawLine(Color.DimGray, xMin, 0f, xMax, 0f, width: 2);
|
||||
|
||||
for (int row = 0; row < map.Rows; row++)
|
||||
{
|
||||
for (int col = 0; col < map.Cols; col++)
|
||||
{
|
||||
if (!map.IsOccupied(row, col)) continue;
|
||||
float x = xMin + col * resolution + resolution / 2f;
|
||||
float y = yMin + row * resolution;
|
||||
Painter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + resolution,
|
||||
width: Math.Max(1, (int)Math.Round(resolution)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawPose(Pose2D pose, Color color, string label)
|
||||
{
|
||||
if (pose == null) return;
|
||||
float x = ToMillimeters(pose.X);
|
||||
float y = ToMillimeters(pose.Y);
|
||||
Painter.DrawCircle(color, x, y, 80f);
|
||||
DrawHeadingArrow(x, y, pose.Heading, color, 260f);
|
||||
Painter.DrawText(color, label, x + 100f, y + 100f);
|
||||
}
|
||||
|
||||
private static void DrawGoal(Pose2D goal, HybridAStarConfiguration configuration, Color color)
|
||||
{
|
||||
DrawPose(goal, color, "终点");
|
||||
if (goal == null || configuration == null) return;
|
||||
Painter.DrawCircle(Color.FromArgb(150, color), ToMillimeters(goal.X), ToMillimeters(goal.Y),
|
||||
ToMillimeters(configuration.GoalPositionToleranceMeters));
|
||||
}
|
||||
|
||||
private static void DrawPath(PlanningResult planningResult, VehicleParameters vehicle)
|
||||
{
|
||||
if (planningResult.Path == null || planningResult.Path.Count == 0) return;
|
||||
|
||||
int frameStride = Math.Max(1, planningResult.Path.Count / 10);
|
||||
for (int index = 1; index < planningResult.Path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint previous = planningResult.Path[index - 1];
|
||||
CoarsePathPoint current = planningResult.Path[index];
|
||||
Color color = current.Direction == TravelDirection.Forward ? Color.LimeGreen : Color.DeepSkyBlue;
|
||||
Painter.DrawLine(color, ToMillimeters(previous.X), ToMillimeters(previous.Y),
|
||||
ToMillimeters(current.X), ToMillimeters(current.Y), width: 4);
|
||||
|
||||
if (index % frameStride == 0 || current.IsGearSwitchPoint || index == planningResult.Path.Count - 1)
|
||||
DrawVehicleFrame(current, vehicle);
|
||||
if (index % Math.Max(1, frameStride / 2) == 0)
|
||||
DrawHeadingArrow(ToMillimeters(current.X), ToMillimeters(current.Y),
|
||||
current.Heading + (current.Direction == TravelDirection.Reverse ? Math.PI : 0d), color, 140f);
|
||||
if (!current.IsGearSwitchPoint) continue;
|
||||
|
||||
float x = ToMillimeters(current.X);
|
||||
float y = ToMillimeters(current.Y);
|
||||
Painter.DrawCircle(Color.MediumPurple, x, y, 100f);
|
||||
Painter.DrawText(Color.MediumPurple, "换向", x + 110f, y - 110f);
|
||||
}
|
||||
|
||||
DrawVehicleFrame(planningResult.Path[0], vehicle);
|
||||
}
|
||||
|
||||
private static void DrawVehicleFrame(CoarsePathPoint point, VehicleParameters vehicle)
|
||||
{
|
||||
if (point == null || vehicle == null) return;
|
||||
float halfLength = ToMillimeters(vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters);
|
||||
float halfWidth = ToMillimeters(vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters);
|
||||
float centerX = ToMillimeters(point.X);
|
||||
float centerY = ToMillimeters(point.Y);
|
||||
double cos = Math.Cos(point.Heading);
|
||||
double sin = Math.Sin(point.Heading);
|
||||
|
||||
TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, halfWidth, out float frontLeftX, out float frontLeftY);
|
||||
TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, -halfWidth, out float frontRightX, out float frontRightY);
|
||||
TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, -halfWidth, out float rearRightX, out float rearRightY);
|
||||
TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, halfWidth, out float rearLeftX, out float rearLeftY);
|
||||
Painter.DrawLine(Color.Gold, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 2);
|
||||
Painter.DrawLine(Color.Gold, frontRightX, frontRightY, rearRightX, rearRightY, width: 2);
|
||||
Painter.DrawLine(Color.Gold, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 2);
|
||||
Painter.DrawLine(Color.Gold, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 2);
|
||||
}
|
||||
|
||||
private static void TransformVehicleCorner(float centerX, float centerY, double cos, double sin,
|
||||
float longitudinal, float lateral, out float x, out float y)
|
||||
{
|
||||
x = centerX + (float)(cos * longitudinal - sin * lateral);
|
||||
y = centerY + (float)(sin * longitudinal + cos * lateral);
|
||||
}
|
||||
|
||||
private static void DrawHeadingArrow(float x, float y, double headingRadians, Color color, float length)
|
||||
{
|
||||
float endX = x + (float)Math.Cos(headingRadians) * length;
|
||||
float endY = y + (float)Math.Sin(headingRadians) * length;
|
||||
Painter.DrawLine(color, x, y, endX, endY, endArrow: true, width: 3);
|
||||
}
|
||||
|
||||
private static void DrawLegend(PlanningGridMap map)
|
||||
{
|
||||
float x = map == null ? 0f : map.Bounds.XMin + 150f;
|
||||
float y = map == null ? 250f : map.Bounds.YMax - 180f;
|
||||
Painter.DrawText(Color.White, "图例", x, y);
|
||||
DrawLegendItem(x, y - 130f, Color.Gainsboro, "边界 / 栅格");
|
||||
DrawLegendItem(x, y - 260f, Color.Firebrick, "占据格");
|
||||
DrawLegendItem(x, y - 390f, Color.LimeGreen, "起点 / 前进");
|
||||
DrawLegendItem(x, y - 520f, Color.Orange, "终点 / 容差");
|
||||
DrawLegendItem(x, y - 650f, Color.DeepSkyBlue, "倒车");
|
||||
DrawLegendItem(x, y - 780f, Color.MediumPurple, "换向");
|
||||
DrawLegendItem(x, y - 910f, Color.Gold, "扩大车体检查框");
|
||||
}
|
||||
|
||||
private static void DrawLegendItem(float x, float y, Color color, string text)
|
||||
{
|
||||
Painter.DrawLine(color, x, y, x + 90f, y, width: 5);
|
||||
Painter.DrawText(color, text, x + 120f, y - 30f);
|
||||
}
|
||||
|
||||
private static void DrawStatus(string scenarioName, CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult result, PlanningGridMap map, AmrPoseSnapshot amrPose)
|
||||
{
|
||||
float x = map == null ? 0f : map.Bounds.XMin + 150f;
|
||||
float y = map == null ? -250f : map.Bounds.YMin + 150f;
|
||||
string snapshot = map == null ? "无" : map.SnapshotId.ToString(CultureInfo.InvariantCulture);
|
||||
string resolution = map == null ? "无" : map.ResolutionMm.ToString("F0", CultureInfo.InvariantCulture) + " mm";
|
||||
PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
|
||||
string reason = diagnostics.TerminationReason ?? string.Empty;
|
||||
string turningRadius = "无";
|
||||
if (job != null && VehicleKinematics.TryGetMaximumCurvaturePerMeter(job.Vehicle, out double maximumCurvaturePerMeter))
|
||||
turningRadius = (1d / maximumCurvaturePerMeter).ToString("F2", CultureInfo.InvariantCulture) + " m";
|
||||
Painter.DrawText(Color.White, "场景：" + scenarioName, x, y);
|
||||
float detailOffset = 0f;
|
||||
if (amrPose != null)
|
||||
{
|
||||
Painter.DrawText(Color.White, amrPose.DisplayText, x, y + 120f);
|
||||
detailOffset = 120f;
|
||||
}
|
||||
Painter.DrawText(Color.White, "地图：" + result.MapResult.Status + "，缓存：" + result.MapResult.CacheHit + "，快照：" + snapshot,
|
||||
x, y + 120f + detailOffset);
|
||||
Painter.DrawText(Color.White, "栅格：" + resolution + "，规划：" + result.PlanningResult.Status + "，总耗时：" +
|
||||
diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms，路径搜索：" +
|
||||
diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms", x, y + 240f + detailOffset);
|
||||
Painter.DrawText(Color.White, "节点：扩展=" + diagnostics.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) +
|
||||
"，生成=" + diagnostics.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，Open List峰值=" +
|
||||
diagnostics.PeakOpenListCount.ToString(CultureInfo.InvariantCulture), x, y + 360f + detailOffset);
|
||||
if (job != null && job.Vehicle != null)
|
||||
{
|
||||
Painter.DrawText(Color.White, "演示车辆：长=" + job.Vehicle.LengthMeters.ToString("F2", CultureInfo.InvariantCulture) +
|
||||
" m，宽=" + job.Vehicle.WidthMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，余量=" +
|
||||
job.Vehicle.SafetyMarginMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，最小转弯半径=" +
|
||||
turningRadius, x, y + 480f + detailOffset);
|
||||
}
|
||||
if (!string.IsNullOrEmpty(reason))
|
||||
Painter.DrawText(Color.LightYellow, "原因：" + reason, x, y + 600f + detailOffset);
|
||||
}
|
||||
|
||||
private static string BuildToastMessage(string scenarioName, CoarsePathPlanningJobResult result)
|
||||
{
|
||||
PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
|
||||
string message = "粗路径[" + scenarioName + "]：地图=" + result.MapResult.Status + "，规划=" +
|
||||
result.PlanningResult.Status + "，总耗时=" +
|
||||
diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms，路径搜索=" +
|
||||
diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms";
|
||||
if (result.PlanningResult.Status != PlanningStatus.Success && !string.IsNullOrEmpty(diagnostics.TerminationReason))
|
||||
message += "，原因=" + diagnostics.TerminationReason;
|
||||
return message + "。";
|
||||
}
|
||||
|
||||
private static void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
|
||||
{
|
||||
Painter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
|
||||
Painter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
|
||||
Painter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
|
||||
Painter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
|
||||
}
|
||||
|
||||
private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);
|
||||
|
||||
private static void EnsureFiniteAmrValue(double value, string name)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
throw new ArgumentException(name + " 必须是有限数。");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,160 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
|
||||
/// <summary>
|
||||
/// 对以车辆几何中心表示的扩大矩形执行连续碰撞检查。
|
||||
/// 地图查询使用 m；任何地图外车辆部分、占据格相交或擦边均按碰撞处理。
|
||||
/// </summary>
|
||||
public sealed class FootprintCollisionChecker
|
||||
{
|
||||
/// <summary>创建连续车辆碰撞检查器。</summary>
|
||||
public FootprintCollisionChecker()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断单个车辆位姿是否无碰撞。
|
||||
/// 参数：pose 为车辆几何中心的世界 m/rad 位姿；map 为不可变规划地图；vehicle 为车辆尺寸；
|
||||
/// additionalMarginMeters 为临时额外安全余量，单位 m；bodyClearanceMeters 输出不含该临时余量的保守车体净空下界，单位 m。
|
||||
/// 返回：扩大车辆矩形完整位于地图内且不与任何占据格相交或擦边时为 true；无效输入保守地返回 false。
|
||||
/// </summary>
|
||||
public bool IsPoseCollisionFree(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
|
||||
double additionalMarginMeters, out double bodyClearanceMeters)
|
||||
{
|
||||
bodyClearanceMeters = 0d;
|
||||
if (map == null || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d ||
|
||||
!VehicleFootprint.TryCreate(pose, vehicle, 0d, out VehicleFootprint bodyFootprint) ||
|
||||
!VehicleFootprint.TryCreate(pose, vehicle, additionalMarginMeters, out VehicleFootprint checkedFootprint))
|
||||
return false;
|
||||
|
||||
if (!AreCornersInsideMap(checkedFootprint, map)) return false;
|
||||
|
||||
double centerDistanceMeters = map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y);
|
||||
bodyClearanceMeters = GetBodyClearance(centerDistanceMeters, bodyFootprint.CircumscribedRadiusMeters);
|
||||
if (centerDistanceMeters > checkedFootprint.CircumscribedRadiusMeters) return true;
|
||||
|
||||
return !IntersectsOccupiedCell(checkedFootprint, map);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断两个位姿之间的平移和转向扫掠是否无碰撞。
|
||||
/// 参数：from、to 为世界 m/rad 位姿；maximumCenterStepMeters 为允许的最大中心采样间距，单位 m；
|
||||
/// minimumBodyClearanceMeters 输出沿途不含临时扫掠余量的保守车体净空下界，单位 m。
|
||||
/// 返回：端点和每个分段扫掠均无碰撞时为 true；无效输入、地图外或任一中间碰撞时返回 false。
|
||||
/// </summary>
|
||||
public bool IsSweptMotionCollisionFree(Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
|
||||
double maximumCenterStepMeters, out double minimumBodyClearanceMeters)
|
||||
{
|
||||
minimumBodyClearanceMeters = 0d;
|
||||
if (map == null || from == null || to == null || !NumericGuard.IsFinite(maximumCenterStepMeters) || maximumCenterStepMeters <= 0d ||
|
||||
!NumericGuard.IsFinite(from.X) || !NumericGuard.IsFinite(from.Y) || !NumericGuard.IsFinite(from.Heading) ||
|
||||
!NumericGuard.IsFinite(to.X) || !NumericGuard.IsFinite(to.Y) || !NumericGuard.IsFinite(to.Heading) ||
|
||||
!VehicleFootprint.TryCreate(from, vehicle, 0d, out VehicleFootprint bodyFootprint))
|
||||
return false;
|
||||
|
||||
double allowedStepMeters = Math.Min(maximumCenterStepMeters, map.ResolutionMeters / 2d);
|
||||
if (!NumericGuard.IsPositiveFinite(allowedStepMeters)) return false;
|
||||
|
||||
if (!IsPoseCollisionFree(from, map, vehicle, 0d, out double fromClearanceMeters)) return false;
|
||||
minimumBodyClearanceMeters = fromClearanceMeters;
|
||||
|
||||
double deltaX = to.X - from.X;
|
||||
double deltaY = to.Y - from.Y;
|
||||
double centerDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (!NumericGuard.IsFinite(centerDistanceMeters)) return false;
|
||||
double headingDeltaRadians = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
|
||||
if (!NumericGuard.IsFinite(headingDeltaRadians)) return false;
|
||||
double rawSegmentCount = Math.Ceiling(centerDistanceMeters / allowedStepMeters);
|
||||
if (!NumericGuard.IsFinite(rawSegmentCount) || rawSegmentCount > int.MaxValue) return false;
|
||||
int segmentCount = Math.Max(1, (int)rawSegmentCount);
|
||||
|
||||
Pose2D previousPose = from;
|
||||
for (int segment = 1; segment <= segmentCount; segment++)
|
||||
{
|
||||
double endFraction = (double)segment / segmentCount;
|
||||
double middleFraction = ((double)segment - 0.5d) / segmentCount;
|
||||
var currentPose = new Pose2D(
|
||||
from.X + deltaX * endFraction,
|
||||
from.Y + deltaY * endFraction,
|
||||
from.Heading + headingDeltaRadians * endFraction);
|
||||
var middlePose = new Pose2D(
|
||||
from.X + deltaX * middleFraction,
|
||||
from.Y + deltaY * middleFraction,
|
||||
from.Heading + headingDeltaRadians * middleFraction);
|
||||
double segmentDeltaX = currentPose.X - previousPose.X;
|
||||
double segmentDeltaY = currentPose.Y - previousPose.Y;
|
||||
double segmentCenterDisplacementMeters = Math.Sqrt(segmentDeltaX * segmentDeltaX + segmentDeltaY * segmentDeltaY);
|
||||
double segmentHeadingDeltaRadians = currentPose.Heading - previousPose.Heading;
|
||||
double temporaryMarginMeters = 0.5d * (segmentCenterDisplacementMeters +
|
||||
bodyFootprint.CircumscribedRadiusMeters * Math.Abs(segmentHeadingDeltaRadians));
|
||||
if (!NumericGuard.IsFinite(temporaryMarginMeters) ||
|
||||
!IsPoseCollisionFree(middlePose, map, vehicle, temporaryMarginMeters, out double middleClearanceMeters))
|
||||
return false;
|
||||
minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, middleClearanceMeters);
|
||||
previousPose = currentPose;
|
||||
}
|
||||
|
||||
if (!IsPoseCollisionFree(to, map, vehicle, 0d, out double toClearanceMeters)) return false;
|
||||
minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, toClearanceMeters);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool AreCornersInsideMap(VehicleFootprint footprint, PlanningGridMap map)
|
||||
{
|
||||
for (int index = 0; index < 4; index++)
|
||||
{
|
||||
footprint.GetCorner(index, out double cornerX, out double cornerY);
|
||||
if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double GetBodyClearance(double centerDistanceMeters, double bodyRadiusMeters)
|
||||
{
|
||||
if (double.IsPositiveInfinity(centerDistanceMeters)) return double.PositiveInfinity;
|
||||
if (!NumericGuard.IsFinite(centerDistanceMeters) || !NumericGuard.IsFinite(bodyRadiusMeters)) return 0d;
|
||||
return Math.Max(0d, centerDistanceMeters - bodyRadiusMeters);
|
||||
}
|
||||
|
||||
private static bool IntersectsOccupiedCell(VehicleFootprint footprint, PlanningGridMap map)
|
||||
{
|
||||
GetCellRange(map, footprint.MinX, footprint.MaxX, footprint.MinY, footprint.MaxY,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
||||
for (int row = firstRow; row <= lastRow; row++)
|
||||
for (int col = firstCol; col <= lastCol; col++)
|
||||
{
|
||||
if (!map.IsOccupied(row, col)) continue;
|
||||
GetCellBoundsMeters(map, row, col, out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY);
|
||||
if (OrientedRectangleCellIntersection.Intersects(footprint, cellMinX, cellMaxX, cellMinY, cellMaxY)) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static void GetCellRange(PlanningGridMap map, double minX, double maxX, double minY, double maxY,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol)
|
||||
{
|
||||
double minimumMapX = map.Bounds.XMin / 1000d;
|
||||
double minimumMapY = map.Bounds.YMin / 1000d;
|
||||
firstCol = Clamp((int)Math.Floor((minX - minimumMapX) / map.ResolutionMeters) - 1, 0, map.Cols - 1);
|
||||
lastCol = Clamp((int)Math.Floor((maxX - minimumMapX) / map.ResolutionMeters), 0, map.Cols - 1);
|
||||
firstRow = Clamp((int)Math.Floor((minY - minimumMapY) / map.ResolutionMeters) - 1, 0, map.Rows - 1);
|
||||
lastRow = Clamp((int)Math.Floor((maxY - minimumMapY) / map.ResolutionMeters), 0, map.Rows - 1);
|
||||
}
|
||||
|
||||
private static void GetCellBoundsMeters(PlanningGridMap map, int row, int col,
|
||||
out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY)
|
||||
{
|
||||
cellMinX = map.Bounds.XMin / 1000d + col * map.ResolutionMeters;
|
||||
cellMinY = map.Bounds.YMin / 1000d + row * map.ResolutionMeters;
|
||||
cellMaxX = Math.Min(map.Bounds.XMax / 1000d, cellMinX + map.ResolutionMeters);
|
||||
cellMaxY = Math.Min(map.Bounds.YMax / 1000d, cellMinY + map.ResolutionMeters);
|
||||
}
|
||||
|
||||
private static int Clamp(int value, int minimum, int maximum)
|
||||
{
|
||||
return value < minimum ? minimum : value > maximum ? maximum : value;
|
||||
}
|
||||
}
|
||||
+33
@@ -0,0 +1,33 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
|
||||
/// <summary>旋转矩形与轴对齐栅格的分离轴相交判定。</summary>
|
||||
internal static class OrientedRectangleCellIntersection
|
||||
{
|
||||
/// <summary>任一投影轴没有严格分离时返回 true；擦边按相交处理。</summary>
|
||||
public static bool Intersects(VehicleFootprint rectangle, double cellMinX, double cellMaxX, double cellMinY, double cellMaxY)
|
||||
{
|
||||
if (rectangle == null || cellMaxX < cellMinX || cellMaxY < cellMinY) return false;
|
||||
double cellCenterX = (cellMinX + cellMaxX) / 2d;
|
||||
double cellCenterY = (cellMinY + cellMaxY) / 2d;
|
||||
double cellHalfX = (cellMaxX - cellMinX) / 2d;
|
||||
double cellHalfY = (cellMaxY - cellMinY) / 2d;
|
||||
|
||||
return !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLongitudinalX, rectangle.AxisLongitudinalY) &&
|
||||
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLateralX, rectangle.AxisLateralY) &&
|
||||
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 1d, 0d) &&
|
||||
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 0d, 1d);
|
||||
}
|
||||
|
||||
private static bool HasStrictSeparation(VehicleFootprint rectangle, double cellCenterX, double cellCenterY, double cellHalfX, double cellHalfY,
|
||||
double axisX, double axisY)
|
||||
{
|
||||
double rectangleCenter = rectangle.CenterX * axisX + rectangle.CenterY * axisY;
|
||||
double cellCenter = cellCenterX * axisX + cellCenterY * axisY;
|
||||
double rectangleRadius = rectangle.HalfLengthMeters * Math.Abs(rectangle.AxisLongitudinalX * axisX + rectangle.AxisLongitudinalY * axisY) +
|
||||
rectangle.HalfWidthMeters * Math.Abs(rectangle.AxisLateralX * axisX + rectangle.AxisLateralY * axisY);
|
||||
double cellRadius = cellHalfX * Math.Abs(axisX) + cellHalfY * Math.Abs(axisY);
|
||||
return Math.Abs(rectangleCenter - cellCenter) > rectangleRadius + cellRadius;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
|
||||
/// <summary>以车辆几何中心为原点的扩大旋转矩形。</summary>
|
||||
internal sealed class VehicleFootprint
|
||||
{
|
||||
private VehicleFootprint(Pose2D pose, double halfLengthMeters, double halfWidthMeters)
|
||||
{
|
||||
CenterX = pose.X;
|
||||
CenterY = pose.Y;
|
||||
HalfLengthMeters = halfLengthMeters;
|
||||
HalfWidthMeters = halfWidthMeters;
|
||||
AxisLongitudinalX = Math.Cos(pose.Heading);
|
||||
AxisLongitudinalY = Math.Sin(pose.Heading);
|
||||
AxisLateralX = -AxisLongitudinalY;
|
||||
AxisLateralY = AxisLongitudinalX;
|
||||
CircumscribedRadiusMeters = Math.Sqrt(halfLengthMeters * halfLengthMeters + halfWidthMeters * halfWidthMeters);
|
||||
|
||||
double minX = double.PositiveInfinity;
|
||||
double maxX = double.NegativeInfinity;
|
||||
double minY = double.PositiveInfinity;
|
||||
double maxY = double.NegativeInfinity;
|
||||
for (int index = 0; index < 4; index++)
|
||||
{
|
||||
GetCorner(index, out double x, out double y);
|
||||
minX = Math.Min(minX, x);
|
||||
maxX = Math.Max(maxX, x);
|
||||
minY = Math.Min(minY, y);
|
||||
maxY = Math.Max(maxY, y);
|
||||
}
|
||||
MinX = minX;
|
||||
MaxX = maxX;
|
||||
MinY = minY;
|
||||
MaxY = maxY;
|
||||
}
|
||||
|
||||
public double CenterX { get; }
|
||||
public double CenterY { get; }
|
||||
public double HalfLengthMeters { get; }
|
||||
public double HalfWidthMeters { get; }
|
||||
public double AxisLongitudinalX { get; }
|
||||
public double AxisLongitudinalY { get; }
|
||||
public double AxisLateralX { get; }
|
||||
public double AxisLateralY { get; }
|
||||
public double CircumscribedRadiusMeters { get; }
|
||||
public double MinX { get; }
|
||||
public double MaxX { get; }
|
||||
public double MinY { get; }
|
||||
public double MaxY { get; }
|
||||
|
||||
/// <summary>创建包含车辆安全余量和临时扫掠余量的矩形。</summary>
|
||||
public static bool TryCreate(Pose2D pose, VehicleParameters vehicle, double additionalMarginMeters, out VehicleFootprint footprint)
|
||||
{
|
||||
footprint = null;
|
||||
if (pose == null || vehicle == null || !NumericGuard.IsFinite(pose.X) || !NumericGuard.IsFinite(pose.Y) ||
|
||||
!NumericGuard.IsFinite(pose.Heading) || !NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(vehicle.WidthMeters) || !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) ||
|
||||
vehicle.SafetyMarginMeters < 0d || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d)
|
||||
return false;
|
||||
|
||||
double totalMarginMeters = vehicle.SafetyMarginMeters + additionalMarginMeters;
|
||||
if (!NumericGuard.IsFinite(totalMarginMeters)) return false;
|
||||
double halfLengthMeters = vehicle.LengthMeters / 2d + totalMarginMeters;
|
||||
double halfWidthMeters = vehicle.WidthMeters / 2d + totalMarginMeters;
|
||||
if (!NumericGuard.IsPositiveFinite(halfLengthMeters) || !NumericGuard.IsPositiveFinite(halfWidthMeters)) return false;
|
||||
|
||||
footprint = new VehicleFootprint(pose, halfLengthMeters, halfWidthMeters);
|
||||
return NumericGuard.IsFinite(footprint.CircumscribedRadiusMeters) && NumericGuard.IsFinite(footprint.MinX) &&
|
||||
NumericGuard.IsFinite(footprint.MaxX) && NumericGuard.IsFinite(footprint.MinY) && NumericGuard.IsFinite(footprint.MaxY);
|
||||
}
|
||||
|
||||
/// <summary>获取指定角点。索引按逆时针顺序为 0 到 3。</summary>
|
||||
public void GetCorner(int index, out double x, out double y)
|
||||
{
|
||||
double longitudinalSign;
|
||||
double lateralSign;
|
||||
switch (index)
|
||||
{
|
||||
case 0: longitudinalSign = 1d; lateralSign = 1d; break;
|
||||
case 1: longitudinalSign = -1d; lateralSign = 1d; break;
|
||||
case 2: longitudinalSign = -1d; lateralSign = -1d; break;
|
||||
case 3: longitudinalSign = 1d; lateralSign = -1d; break;
|
||||
default: throw new ArgumentOutOfRangeException(nameof(index));
|
||||
}
|
||||
x = CenterX + longitudinalSign * HalfLengthMeters * AxisLongitudinalX + lateralSign * HalfWidthMeters * AxisLateralX;
|
||||
y = CenterY + longitudinalSign * HalfLengthMeters * AxisLongitudinalY + lateralSign * HalfWidthMeters * AxisLateralY;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
|
||||
/// <summary>
|
||||
/// 从车辆参数提取运动学限制的辅助方法。
|
||||
/// 曲率单位为 1/m；当最大曲率与最小转弯半径同时给出时，始终选择更保守的较小曲率。
|
||||
/// </summary>
|
||||
public static class VehicleKinematics
|
||||
{
|
||||
/// <summary>
|
||||
/// 尝试获取车辆允许的最大绝对曲率。
|
||||
/// 参数：vehicle 为车辆参数；maximumCurvaturePerMeter 为输出的正有限曲率，单位 1/m。
|
||||
/// 返回：至少提供一种正有限曲率限制时为 true；任一已提供限制无效或两种限制均未提供时为 false。
|
||||
/// </summary>
|
||||
public static bool TryGetMaximumCurvaturePerMeter(VehicleParameters vehicle, out double maximumCurvaturePerMeter)
|
||||
{
|
||||
maximumCurvaturePerMeter = 0d;
|
||||
if (vehicle == null) return false;
|
||||
|
||||
bool hasMaximumCurvature = vehicle.MaximumCurvaturePerMeter.HasValue;
|
||||
bool hasMinimumRadius = vehicle.MinimumTurningRadiusMeters.HasValue;
|
||||
if (hasMaximumCurvature && !NumericGuard.IsPositiveFinite(vehicle.MaximumCurvaturePerMeter.Value)) return false;
|
||||
if (hasMinimumRadius && !NumericGuard.IsPositiveFinite(vehicle.MinimumTurningRadiusMeters.Value)) return false;
|
||||
if (!hasMaximumCurvature && !hasMinimumRadius) return false;
|
||||
|
||||
if (hasMaximumCurvature && hasMinimumRadius)
|
||||
{
|
||||
maximumCurvaturePerMeter = System.Math.Min(
|
||||
vehicle.MaximumCurvaturePerMeter.Value,
|
||||
1d / vehicle.MinimumTurningRadiusMeters.Value);
|
||||
return true;
|
||||
}
|
||||
|
||||
maximumCurvaturePerMeter = hasMaximumCurvature
|
||||
? vehicle.MaximumCurvaturePerMeter.Value
|
||||
: 1d / vehicle.MinimumTurningRadiusMeters.Value;
|
||||
return NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter);
|
||||
}
|
||||
}
|
||||
+812
@@ -0,0 +1,812 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="utf-8"/>
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1"/>
|
||||
<title>AMR 非结构化道路 Hybrid A* 粗路径规划总体技术方案</title>
|
||||
<style>
|
||||
:root{
|
||||
--bg:#0b1020;
|
||||
--panel:#121a2f;
|
||||
--panel2:#18223c;
|
||||
--text:#eef4ff;
|
||||
--muted:#aebbd2;
|
||||
--line:#2b3a5f;
|
||||
--accent:#68a7ff;
|
||||
--accent2:#7ce7c4;
|
||||
--warn:#ffd479;
|
||||
--danger:#ff8b8b;
|
||||
--ok:#78e08f;
|
||||
--code:#09101f;
|
||||
--shadow:0 14px 40px rgba(0,0,0,.28);
|
||||
}
|
||||
*{box-sizing:border-box}
|
||||
html{scroll-behavior:smooth}
|
||||
body{
|
||||
margin:0;
|
||||
font-family:"Segoe UI","Microsoft YaHei",system-ui,-apple-system,sans-serif;
|
||||
background:linear-gradient(180deg,#08101f 0%,#0d1427 45%,#0b1020 100%);
|
||||
color:var(--text);
|
||||
line-height:1.78;
|
||||
}
|
||||
a{color:var(--accent);text-decoration:none}
|
||||
a:hover{text-decoration:underline}
|
||||
.hero{
|
||||
padding:64px 7vw 40px;
|
||||
border-bottom:1px solid var(--line);
|
||||
background:
|
||||
radial-gradient(circle at 85% 10%,rgba(104,167,255,.20),transparent 34%),
|
||||
radial-gradient(circle at 10% 0%,rgba(124,231,196,.12),transparent 30%);
|
||||
}
|
||||
.hero h1{font-size:clamp(30px,5vw,56px);line-height:1.18;margin:0 0 18px}
|
||||
.hero p{max-width:1000px;color:var(--muted);font-size:18px}
|
||||
.badges{display:flex;gap:10px;flex-wrap:wrap;margin-top:24px}
|
||||
.badge{padding:6px 12px;border:1px solid var(--line);border-radius:999px;background:rgba(255,255,255,.03);font-size:13px}
|
||||
.badge.ok{border-color:rgba(120,224,143,.5);color:var(--ok)}
|
||||
.badge.todo{border-color:rgba(255,212,121,.5);color:var(--warn)}
|
||||
.layout{display:grid;grid-template-columns:290px minmax(0,1fr);gap:28px;max-width:1500px;margin:0 auto;padding:32px 28px 80px}
|
||||
aside{
|
||||
position:sticky;top:18px;align-self:start;max-height:calc(100vh - 36px);overflow:auto;
|
||||
background:rgba(18,26,47,.86);border:1px solid var(--line);border-radius:18px;padding:18px;box-shadow:var(--shadow);
|
||||
}
|
||||
aside h3{margin:0 0 10px;font-size:16px}
|
||||
aside a{display:block;padding:7px 9px;border-radius:8px;color:var(--muted);font-size:14px}
|
||||
aside a:hover{background:var(--panel2);color:var(--text);text-decoration:none}
|
||||
main{min-width:0}
|
||||
section{
|
||||
background:rgba(18,26,47,.90);border:1px solid var(--line);border-radius:20px;
|
||||
padding:30px;margin-bottom:24px;box-shadow:var(--shadow);
|
||||
}
|
||||
section h2{margin-top:0;font-size:28px;border-bottom:1px solid var(--line);padding-bottom:12px}
|
||||
section h3{margin-top:26px;font-size:20px}
|
||||
section h4{margin-top:20px;font-size:17px;color:var(--accent2)}
|
||||
p,li{color:#dce6f8}
|
||||
.note,.warn,.success,.decision{
|
||||
border-left:4px solid var(--accent);padding:14px 16px;background:rgba(104,167,255,.08);border-radius:8px;margin:18px 0;
|
||||
}
|
||||
.warn{border-color:var(--warn);background:rgba(255,212,121,.08)}
|
||||
.success{border-color:var(--ok);background:rgba(120,224,143,.08)}
|
||||
.decision{border-color:var(--accent2);background:rgba(124,231,196,.08)}
|
||||
code{background:var(--code);padding:2px 6px;border-radius:6px;color:#d8e6ff}
|
||||
pre{
|
||||
background:var(--code);border:1px solid #223253;border-radius:12px;padding:16px;overflow:auto;
|
||||
color:#d9e7ff;line-height:1.55;
|
||||
}
|
||||
table{width:100%;border-collapse:collapse;margin:16px 0;font-size:14px}
|
||||
th,td{border:1px solid var(--line);padding:10px 12px;vertical-align:top}
|
||||
th{background:var(--panel2);text-align:left}
|
||||
.flow{
|
||||
display:grid;gap:10px;margin:18px 0
|
||||
}
|
||||
.flow .node{
|
||||
border:1px solid var(--line);background:linear-gradient(135deg,#15213b,#10182b);
|
||||
border-radius:12px;padding:13px 16px;position:relative
|
||||
}
|
||||
.flow .node:not(:last-child)::after{
|
||||
content:"↓";display:block;text-align:center;color:var(--accent);font-size:22px;position:relative;bottom:-18px;height:20px
|
||||
}
|
||||
.grid2{display:grid;grid-template-columns:repeat(2,minmax(0,1fr));gap:16px}
|
||||
.grid3{display:grid;grid-template-columns:repeat(3,minmax(0,1fr));gap:16px}
|
||||
.card{border:1px solid var(--line);background:var(--panel2);border-radius:14px;padding:16px}
|
||||
.card h4{margin:0 0 8px}
|
||||
.kpi{font-size:28px;font-weight:700;color:var(--accent2)}
|
||||
.small{font-size:13px;color:var(--muted)}
|
||||
.step-id{display:inline-flex;align-items:center;justify-content:center;width:34px;height:34px;border-radius:50%;background:var(--accent);color:#06101f;font-weight:800;margin-right:8px}
|
||||
.status{float:right;font-size:13px;border-radius:999px;padding:4px 10px;border:1px solid var(--line)}
|
||||
.status.done{color:var(--ok);border-color:rgba(120,224,143,.4)}
|
||||
.status.todo{color:var(--warn);border-color:rgba(255,212,121,.4)}
|
||||
.diagram{
|
||||
font-family:Consolas,monospace;white-space:pre;overflow:auto;background:var(--code);border:1px solid var(--line);
|
||||
padding:18px;border-radius:12px;color:#cfe1ff
|
||||
}
|
||||
footer{max-width:1500px;margin:0 auto;padding:0 28px 50px;color:var(--muted);font-size:13px}
|
||||
@media(max-width:980px){
|
||||
.layout{grid-template-columns:1fr}
|
||||
aside{position:relative;top:0;max-height:none}
|
||||
.grid2,.grid3{grid-template-columns:1fr}
|
||||
}
|
||||
@media print{
|
||||
body{background:white;color:#111}
|
||||
.hero,section,aside{background:white;color:#111;box-shadow:none}
|
||||
.layout{display:block}
|
||||
aside{display:none}
|
||||
p,li{color:#222}
|
||||
code,pre,.diagram{background:#f5f5f5;color:#111}
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<header class="hero">
|
||||
<h1>AMR 非结构化道路 Hybrid A* 粗路径规划总体技术方案</h1>
|
||||
<p>
|
||||
面向四舵轮AMR的第一阶段工程实现：暂不考虑蟹行与纯横移，仅采用车式运动模型，
|
||||
支持前进、倒车及换向，输出供后续SQP使用的无时间空间粗路径。
|
||||
本文统一整理已经确认的第1至第14步，作为软件设计、编码、调试与验收依据。
|
||||
</p>
|
||||
<div class="badges">
|
||||
<span class="badge ok">步骤1–12：已确认</span>
|
||||
<span class="badge todo">步骤13：性能优化TODO</span>
|
||||
<span class="badge ok">步骤14：接口与验收已确认</span>
|
||||
<span class="badge">地图分辨率初值：0.05 m</span>
|
||||
<span class="badge">原语长度：0.50 m</span>
|
||||
<span class="badge">积分步长：0.05 m</span>
|
||||
</div>
|
||||
</header>
|
||||
|
||||
<div class="layout">
|
||||
<aside>
|
||||
<h3>目录</h3>
|
||||
<a href="#overview">0. 总体概览</a>
|
||||
<a href="#step1">1. 任务边界</a>
|
||||
<a href="#step2">2. 地图表达</a>
|
||||
<a href="#step3">3. 运动模型与曲率</a>
|
||||
<a href="#step4">4. 搜索节点状态</a>
|
||||
<a href="#step5">5. 运动原语</a>
|
||||
<a href="#step6">6. 原语终止与积分</a>
|
||||
<a href="#step7">7. 碰撞检测与模板</a>
|
||||
<a href="#step8">8. 代价与启发</a>
|
||||
<a href="#step9">9. 终点与Reeds–Shepp</a>
|
||||
<a href="#step10">10. 回溯与稠密点</a>
|
||||
<a href="#step11">11. 重采样</a>
|
||||
<a href="#step12">12. 平滑与校验</a>
|
||||
<a href="#step13">13. 性能优化TODO</a>
|
||||
<a href="#step14">14. 接口与验收</a>
|
||||
<a href="#params">附录A. 推荐参数</a>
|
||||
<a href="#pseudocode">附录B. 总体伪代码</a>
|
||||
<a href="#milestones">附录C. 开发里程碑</a>
|
||||
</aside>
|
||||
|
||||
<main>
|
||||
<section id="overview">
|
||||
<h2>0. 总体概览</h2>
|
||||
<div class="grid3">
|
||||
<div class="card"><div class="kpi">车式运动</div><div class="small">车头/车尾方向运动，不考虑蟹行</div></div>
|
||||
<div class="card"><div class="kpi">前进 + 倒车</div><div class="small">允许在原语边界切换方向</div></div>
|
||||
<div class="card"><div class="kpi">空间粗路径</div><div class="small">不包含速度、加速度、时间戳</div></div>
|
||||
</div>
|
||||
|
||||
<h3>0.1 总体职责边界</h3>
|
||||
<table>
|
||||
<tr><th>模块</th><th>主要职责</th><th>不负责</th></tr>
|
||||
<tr><td>Hybrid A*</td><td>拓扑可达、无碰撞、基本运动学可行、前进/倒车结构、目标位置与航向接近</td><td>最终速度、加速度、时间、四轮舵角与轮速</td></tr>
|
||||
<tr><td>路径后处理</td><td>回溯、稠密点恢复、关键点保留、弧长重采样、快速曲线平滑、完整复核</td><td>动态约束和最终轨迹时间参数化</td></tr>
|
||||
<tr><td>后续SQP</td><td>终点精确收敛、速度/加速度/角速度/时间戳、换向点停车、动态可跟踪性</td><td>重新决定绕障侧或凭空增加倒车拓扑</td></tr>
|
||||
<tr><td>底盘逆解与控制</td><td>四轮舵角、轮速分配和轨迹跟踪</td><td>全局绕障搜索</td></tr>
|
||||
</table>
|
||||
|
||||
<h3>0.2 总体数据流</h3>
|
||||
<div class="flow">
|
||||
<div class="node">感知障碍物 / 点云 → 过滤、投影到二维</div>
|
||||
<div class="node">二维占据栅格 + 障碍物距离场</div>
|
||||
<div class="node">起点、终点、车辆参数、规划配置</div>
|
||||
<div class="node">二维Dijkstra启发图 + Hybrid A*搜索</div>
|
||||
<div class="node">机会式/强制 Reeds–Shepp 终点连接</div>
|
||||
<div class="node">父节点回溯 + 0.05 m内部积分点恢复</div>
|
||||
<div class="node">关键点保留 + 分方向弧长重采样</div>
|
||||
<div class="node">三次B样条优先，局部Bézier/五次多项式，局部QP兜底</div>
|
||||
<div class="node">最终碰撞、净空、曲率、换向结构校验</div>
|
||||
<div class="node">输出空间粗路径 → 后续SQP</div>
|
||||
</div>
|
||||
|
||||
<div class="decision">
|
||||
<strong>第一版核心原则：</strong>先保证算法完整跑通、路径正确、可解释、可复现；性能优化整体放入第十三步TODO，待前12步稳定后再开展。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step1">
|
||||
<h2><span class="step-id">1</span>任务边界与输出定义 <span class="status done">已确认</span></h2>
|
||||
<h3>1.1 车辆运动模式</h3>
|
||||
<p>目标平台为四舵轮AMR，但第一阶段只使用车式运动能力：</p>
|
||||
<ul>
|
||||
<li>车辆沿车头或车尾方向运动；</li>
|
||||
<li>允许前进、倒车以及换向；</li>
|
||||
<li>暂不考虑蟹行、纯横移和任意方向全向运动；</li>
|
||||
<li>车体航向角始终表示车头朝向，即使车辆正在倒车。</li>
|
||||
</ul>
|
||||
|
||||
<h3>1.2 Hybrid A*最小输出</h3>
|
||||
<p>输出空间路径点序列，至少包含：</p>
|
||||
<pre>位置：x, y
|
||||
车体航向：heading
|
||||
运动方向：Forward / Reverse
|
||||
累计弧长：s
|
||||
参考/几何曲率：kappa
|
||||
换向标记：isGearSwitchPoint
|
||||
安全净空：clearance</pre>
|
||||
<p>不输出速度、加速度、角速度、时间戳、四个舵轮角度与轮速。</p>
|
||||
|
||||
<h3>1.3 成功判据</h3>
|
||||
<ul>
|
||||
<li>路径全程无碰撞且不进入未知区域；</li>
|
||||
<li>粗路径基本满足最大曲率约束；</li>
|
||||
<li>包含正确绕障侧、前进/后退和换向结构；</li>
|
||||
<li>终点位置与航向满足第九步终止规则；</li>
|
||||
<li>可作为后续SQP的有效初值。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<section id="step2">
|
||||
<h2><span class="step-id">2</span>地图表达与环境输入 <span class="status done">已确认</span></h2>
|
||||
<h3>2.1 地图生成流程</h3>
|
||||
<div class="diagram">原始点云 / 障碍信息
|
||||
↓ 过滤噪声、地面、离群点
|
||||
投影到二维平面
|
||||
↓
|
||||
二维占据栅格 Occupancy Grid
|
||||
↓
|
||||
障碍物距离场 Distance Field / EDT</div>
|
||||
|
||||
<h3>2.2 栅格定义</h3>
|
||||
<ul>
|
||||
<li>地图分辨率初值：<code>0.05 m</code>；</li>
|
||||
<li>状态：Free、Occupied、Unknown；</li>
|
||||
<li>第一版中Unknown按障碍物处理；</li>
|
||||
<li>地图包含Origin、Resolution、Width、Height及Version；</li>
|
||||
<li>算法内部不得写死分辨率，统一读取地图参数。</li>
|
||||
</ul>
|
||||
|
||||
<h3>2.3 车体碰撞模型</h3>
|
||||
<p>AMR使用旋转矩形车体碰撞模型，不能只用中心点或单圆近似。安全余量主要加到车体矩形上：</p>
|
||||
<pre>CheckLength = VehicleLength + 2 × SafetyMargin
|
||||
CheckWidth = VehicleWidth + 2 × SafetyMargin</pre>
|
||||
<p>初始安全余量为<code>0.03 m</code>。避免地图膨胀与车体扩大同时重复计算同一余量。</p>
|
||||
</section>
|
||||
|
||||
<section id="step3">
|
||||
<h2><span class="step-id">3</span>车辆运动模型与最大曲率 <span class="status done">已确认</span></h2>
|
||||
<h3>3.1 参考点</h3>
|
||||
<p>车辆状态参考点采用AMR几何中心。</p>
|
||||
|
||||
<h3>3.2 基于路径弧长的运动学</h3>
|
||||
<pre>dx/ds = cos(theta)
|
||||
dy/ds = sin(theta)
|
||||
dtheta/ds = kappa</pre>
|
||||
<p>其中<code>theta</code>为车头航向角，<code>kappa</code>为车体中心路径曲率。</p>
|
||||
|
||||
<h3>3.3 最大曲率来源</h3>
|
||||
<p>必须同时支持两种来源：</p>
|
||||
<ol>
|
||||
<li>外部直接输入<code>MaxCurvature</code>，或输入<code>MinTurningRadius</code>并换算<code>kappa_max = 1 / R_min</code>；</li>
|
||||
<li>由四轮转向几何调用<code>ComputeMaxCurvature(VehicleKinematicParameters)</code>计算。</li>
|
||||
</ol>
|
||||
<p>推荐三种模式：</p>
|
||||
<pre>ExternalOnly
|
||||
GeometryOnly
|
||||
ConservativeMinimum</pre>
|
||||
<p>当两种来源同时存在时，建议取更保守的较小值。若均无有效值，则返回配置错误。</p>
|
||||
|
||||
<div class="warn">
|
||||
曲率变化率不在本步骤强制建模。第一版先保证曲率不超过上限；渐变曲率原语和实车舵轮速率约束作为后续升级。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step4">
|
||||
<h2><span class="step-id">4</span>Hybrid A*搜索节点状态 <span class="status done">已确认</span></h2>
|
||||
<h3>4.1 连续状态</h3>
|
||||
<pre>(x, y, theta, direction, curvatureIndex)</pre>
|
||||
<ul>
|
||||
<li><code>x, y, theta</code>保持连续值用于运动积分；</li>
|
||||
<li><code>direction</code>为Forward或Reverse；</li>
|
||||
<li><code>curvatureIndex</code>表示当前离散曲率等级。</li>
|
||||
</ul>
|
||||
|
||||
<h3>4.2 Closed Set键</h3>
|
||||
<pre>(ix, iy, iHeading, direction, curvatureIndex)</pre>
|
||||
<p>推荐初始离散：</p>
|
||||
<ul>
|
||||
<li>位置Closed Set分辨率：<code>0.05–0.10 m</code>；</li>
|
||||
<li>航向离散：<code>5°</code>；</li>
|
||||
<li>方向和曲率等级均进入键，避免把运动状态不同的节点错误合并。</li>
|
||||
</ul>
|
||||
|
||||
<h3>4.3 搜索元数据</h3>
|
||||
<p>节点还应保存G/H/F代价、父节点索引、生成当前节点的原语积分点、最小净空、终止原因等。这些属于搜索管理信息，不属于车辆物理状态。</p>
|
||||
</section>
|
||||
|
||||
<section id="step5">
|
||||
<h2><span class="step-id">5</span>运动原语与扩展规则 <span class="status done">已确认</span></h2>
|
||||
<h3>5.1 第一版原语集合</h3>
|
||||
<pre>{ -kappa_max, -0.5 kappa_max, 0, 0.5 kappa_max, kappa_max }</pre>
|
||||
<p>每种曲率均支持Forward与Reverse。</p>
|
||||
|
||||
<h3>5.2 曲率切换限制</h3>
|
||||
<p>相邻原语曲率等级最多变化1级。例如：</p>
|
||||
<pre>允许：0 → 0.5κmax
|
||||
允许：0.5κmax → κmax
|
||||
不允许：κmax → -κmax</pre>
|
||||
<p>该限制可减少“左打死后立即右打死”的不合理跳变。</p>
|
||||
|
||||
<h3>5.3 换向规则</h3>
|
||||
<ul>
|
||||
<li>前进/倒车只能在运动原语边界切换；</li>
|
||||
<li>起始曲率若上层可提供则使用实际值，否则默认0；</li>
|
||||
<li>第一版采用恒定曲率原语。</li>
|
||||
</ul>
|
||||
|
||||
<div class="warn">
|
||||
<strong>TODO：</strong>当恒定曲率原语造成曲率跳变过大、平滑后仍不可跟踪或实车舵轮变化速度不足时，增加线性渐变曲率原语。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step6">
|
||||
<h2><span class="step-id">6</span>原语长度、积分与终止 <span class="status done">已确认</span></h2>
|
||||
<h3>6.1 固定参数</h3>
|
||||
<pre>PrimitiveLength = 0.50 m
|
||||
IntegrationStep = 0.05 m</pre>
|
||||
<p>完整原语最多包含10个内部积分点。只有原语终点进入Open List，内部积分点用于碰撞、净空和最终路径恢复。</p>
|
||||
|
||||
<h3>6.2 终止条件</h3>
|
||||
<table>
|
||||
<tr><th>条件</th><th>处理</th></tr>
|
||||
<tr><td>累计长度达到0.50 m</td><td>生成正常候选节点</td></tr>
|
||||
<tr><td>任一积分点碰撞</td><td>原语无效，立即终止</td></tr>
|
||||
<tr><td>越界或进入未知区域</td><td>原语无效</td></tr>
|
||||
<tr><td>中途满足目标规则</td><td>提前终止并保存实际有效段</td></tr>
|
||||
<tr><td>Reeds–Shepp连接成功</td><td>结束搜索</td></tr>
|
||||
</table>
|
||||
|
||||
<div class="warn">
|
||||
<strong>TODO：</strong>后续根据障碍物距离、目标距离和窄通道自动调整原语长度；碰撞积分步长仍保持0.05 m。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step7">
|
||||
<h2><span class="step-id">7</span>碰撞检测与航向栅格模板 <span class="status done">已确认</span></h2>
|
||||
<h3>7.1 两级碰撞检测</h3>
|
||||
<ol>
|
||||
<li><strong>距离场快速放行：</strong>若车辆中心到最近障碍物距离大于扩大车体外接圆半径及离散补偿，则该点必然安全；</li>
|
||||
<li><strong>旋转矩形模板精确检测：</strong>距离不足以保证安全时，检查当前航向下扩大车体覆盖的所有栅格。</li>
|
||||
</ol>
|
||||
|
||||
<h3>7.2 外接圆仅用于“安全放行”</h3>
|
||||
<pre>R_outer = 0.5 × sqrt(CheckLength² + CheckWidth²)</pre>
|
||||
<p><code>D > R_outer</code>表示安全；<code>D ≤ R_outer</code>只表示“无法确定”，不等于碰撞，必须进入矩形模板检查。</p>
|
||||
|
||||
<h3>7.3 航向栅格模板的准备</h3>
|
||||
<p>航向分辨率5°时，在初始化阶段自动生成72组模板：</p>
|
||||
<pre>0°, 5°, 10°, ... , 355°</pre>
|
||||
<p>每个模板保存旋转后的扩大车体矩形与地图栅格相交所对应的相对栅格偏移：</p>
|
||||
<pre>GridOffset { Dx, Dy }</pre>
|
||||
<p>运行时把当前车辆中心转换为栅格坐标，选择最近航向模板，将模板中的相对偏移平移到当前位置并查询占据状态。</p>
|
||||
|
||||
<h3>7.4 模板生成规则</h3>
|
||||
<ul>
|
||||
<li>模板以车体几何中心为原点；</li>
|
||||
<li>车体尺寸包含安全余量与少量离散补偿；</li>
|
||||
<li>只要栅格方块与旋转矩形有交集，就加入模板；</li>
|
||||
<li>不能只判断栅格中心是否在矩形内部，否则可能漏掉边角碰撞；</li>
|
||||
<li>真实车体始终是矩形，模板呈阶梯状只是栅格化结果。</li>
|
||||
</ul>
|
||||
|
||||
<h3>7.5 每个积分点的处理</h3>
|
||||
<pre>读取距离场
|
||||
↓
|
||||
可安全放行？——是→继续
|
||||
↓否
|
||||
选择航向模板
|
||||
↓
|
||||
平移模板并查询Occupied / Unknown / 越界
|
||||
↓
|
||||
任一命中→碰撞；全部通过→安全</pre>
|
||||
</section>
|
||||
|
||||
<section id="step8">
|
||||
<h2><span class="step-id">8</span>搜索代价与启发函数 <span class="status done">已确认</span></h2>
|
||||
<h3>8.1 节点排序</h3>
|
||||
<pre>f(n) = g(n) + w_h × h(n)</pre>
|
||||
<p>第一版<code>HeuristicWeight = 1.3</code>。</p>
|
||||
|
||||
<h3>8.2 累计代价</h3>
|
||||
<pre>g_new =
|
||||
g_parent
|
||||
+ C_length
|
||||
+ C_reverse
|
||||
+ C_switch
|
||||
+ C_curvature
|
||||
+ C_curvatureChange
|
||||
+ C_obstacle</pre>
|
||||
<ul>
|
||||
<li>路径长度是基础代价；</li>
|
||||
<li>后退系数初值<code>ReversePenalty = 1.3</code>；</li>
|
||||
<li>换向固定惩罚初值<code>GearSwitchPenalty = 2.0</code>；</li>
|
||||
<li>曲率代价轻度抑制长期极限转向；</li>
|
||||
<li>曲率变化代价偏好平缓动作序列；</li>
|
||||
<li>障碍物代价让路径尽量远离障碍物。</li>
|
||||
</ul>
|
||||
|
||||
<h3>8.3 双启发</h3>
|
||||
<pre>h = max(h_2D, h_RS)</pre>
|
||||
<ul>
|
||||
<li><code>h_2D</code>：从目标反向运行二维Dijkstra得到的绕障距离；</li>
|
||||
<li><code>h_RS</code>：基于当前位姿、目标位姿和最小转弯半径的Reeds–Shepp距离。</li>
|
||||
</ul>
|
||||
<p>所有权重必须外部可配置。</p>
|
||||
</section>
|
||||
|
||||
<section id="step9">
|
||||
<h2><span class="step-id">9</span>终点判定、SQP精修与Reeds–Shepp <span class="status done">已确认</span></h2>
|
||||
<h3>9.1 基本终点容差</h3>
|
||||
<pre>GoalPositionTolerance = 0.15 m
|
||||
GoalHeadingTolerance = 5°</pre>
|
||||
<p>在每个0.05 m内部积分点检查位置和航向误差，避免越过目标。</p>
|
||||
|
||||
<h3>9.2 容差到达与SQP</h3>
|
||||
<p>Hybrid A*可以在容差范围内结束，由后续SQP通过终端约束精确收敛到目标。但必须满足：</p>
|
||||
<ul>
|
||||
<li>粗路径已包含正确绕障、倒车和换向拓扑；</li>
|
||||
<li>当前终点、目标位姿及末端局部区域无碰撞；</li>
|
||||
<li>终点附近存在足够局部调整空间；</li>
|
||||
<li>SQP只负责局部修正，不负责创造新的换向或改变绕障侧。</li>
|
||||
</ul>
|
||||
|
||||
<h3>9.3 目标进入方向</h3>
|
||||
<pre>GoalDirectionConstraint = Any | Forward | Reverse</pre>
|
||||
<p>默认Any。该字段约束最后一段运动方向，不改变目标车头航向定义。</p>
|
||||
|
||||
<h3>9.4 Reeds–Shepp模式</h3>
|
||||
<table>
|
||||
<tr><th>模式</th><th>连接失败后的行为</th><th>适用场景</th></tr>
|
||||
<tr><td>Disabled</td><td>不尝试，满足容差即可结束</td><td>开阔终点、调试对照</td></tr>
|
||||
<tr><td>Opportunistic</td><td>失败继续搜索，满足容差仍可交SQP</td><td>默认、普通导航</td></tr>
|
||||
<tr><td>RequiredNearGoal</td><td>失败不能按容差结束，必须继续找可连接节点</td><td>狭窄钻入、精确泊入</td></tr>
|
||||
</table>
|
||||
|
||||
<h3>9.5 自动触发</h3>
|
||||
<pre>2–5 m：每扩展10个节点尝试一次
|
||||
≤2 m：每扩展3个节点尝试一次
|
||||
采样碰撞步长：0.05 m</pre>
|
||||
<p>连接路径必须检查地图边界、Unknown、扩大车体碰撞、安全净空、最大曲率、方向约束及与当前曲率的衔接。</p>
|
||||
|
||||
<h3>9.6 默认策略</h3>
|
||||
<pre>ReedsSheppMode = Opportunistic</pre>
|
||||
<p>无需人工每次开启，由程序自动触发。任务类型可覆盖默认模式。</p>
|
||||
</section>
|
||||
|
||||
<section id="step10">
|
||||
<h2><span class="step-id">10</span>搜索回溯与原始稠密路径 <span class="status done">已确认</span></h2>
|
||||
<h3>10.1 回溯内容</h3>
|
||||
<p>搜索成功后从终止节点沿父节点索引回溯到起点，反转节点链，并恢复每条原语保存的0.05 m内部积分点。</p>
|
||||
|
||||
<h3>10.2 不能只输出0.5 m搜索节点</h3>
|
||||
<p>只输出原语终点会丢失圆弧形状、碰撞检查细节和换向局部结构。因此必须复用搜索时已经计算的内部点。</p>
|
||||
|
||||
<h3>10.3 路径点字段</h3>
|
||||
<pre>x, y
|
||||
heading, unwrappedHeading
|
||||
curvature
|
||||
direction
|
||||
arcLength
|
||||
clearance
|
||||
isGearSwitchPoint
|
||||
source</pre>
|
||||
|
||||
<h3>10.4 特殊处理</h3>
|
||||
<ul>
|
||||
<li>删除相邻原语重复边界点；</li>
|
||||
<li>中途终止原语只保留真实有效积分点；</li>
|
||||
<li>Reeds–Shepp连接点追加到普通搜索路径之后；</li>
|
||||
<li>机会式容差终点不允许在本步骤被“硬改”为目标点；</li>
|
||||
<li>航向同时保存归一化值和连续解缠值；</li>
|
||||
<li>累计弧长始终单调增加，方向单独保存；</li>
|
||||
<li>按换向点划分Forward/Reverse连续段。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<section id="step11">
|
||||
<h2><span class="step-id">11</span>关键点保留与弧长重采样 <span class="status done">已确认</span></h2>
|
||||
<h3>11.1 处理目标</h3>
|
||||
<p>原始0.05 m稠密路径不直接全部送入平滑和SQP。先保留关键结构，再按照累计弧长重采样。</p>
|
||||
|
||||
<h3>11.2 必须保留的关键点</h3>
|
||||
<ul>
|
||||
<li>起点与终点；</li>
|
||||
<li>所有换向点；</li>
|
||||
<li>曲率等级变化点；</li>
|
||||
<li>直线/弯道切换点；</li>
|
||||
<li>Reeds–Shepp分段和连接点；</li>
|
||||
<li>障碍物附近最小净空点；</li>
|
||||
<li>必要的明显航向变化点。</li>
|
||||
</ul>
|
||||
|
||||
<h3>11.3 分方向重采样</h3>
|
||||
<p>每个Forward或Reverse段独立处理，不能跨越换向点插值。</p>
|
||||
|
||||
<h3>11.4 推荐间距</h3>
|
||||
<pre>普通区域：0.10 m
|
||||
重点区域：0.05 m</pre>
|
||||
<p>以下区域采用0.05 m：</p>
|
||||
<ul>
|
||||
<li>障碍距离小于0.50 m；</li>
|
||||
<li>|κ| ≥ 0.5 κmax；</li>
|
||||
<li>目标2 m范围内；</li>
|
||||
<li>换向点前后0.30 m；</li>
|
||||
<li>曲率变化点前后0.25 m；</li>
|
||||
<li>Reeds–Shepp连接段与窄通道。</li>
|
||||
</ul>
|
||||
|
||||
<h3>11.5 重采样后复核</h3>
|
||||
<p>新插值点必须重新执行车体碰撞检查。最终输出点可为0.10 m，但碰撞验证内部步长仍不得大于0.05 m。</p>
|
||||
</section>
|
||||
|
||||
<section id="step12">
|
||||
<h2><span class="step-id">12</span>快速曲线平滑、局部QP兜底与最终校验 <span class="status done">已确认</span></h2>
|
||||
<h3>12.1 最终采用的分层方案</h3>
|
||||
<div class="flow">
|
||||
<div class="node">按Forward/Reverse方向段拆分</div>
|
||||
<div class="node">普通路径段：三次B样条快速平滑</div>
|
||||
<div class="node">局部短转角：分段三次Bézier</div>
|
||||
<div class="node">末端短连接：五次多项式（可选）</div>
|
||||
<div class="node">0.05 m完整碰撞、净空、曲率验证</div>
|
||||
<div class="node">失败局部：安全走廊约束QP</div>
|
||||
<div class="node">局部QP仍失败：回退第十一步原始可行路径</div>
|
||||
</div>
|
||||
|
||||
<h3>12.2 为什么不默认全路径QP</h3>
|
||||
<p>全路径多轮QP还需要走廊生成、曲率线性化和反复碰撞检测，可能与后续SQP功能重复并增加耗时。因此第一版采用快速曲线优先，仅对失败局部使用QP。</p>
|
||||
|
||||
<h3>12.3 平滑结构约束</h3>
|
||||
<ul>
|
||||
<li>换向点固定，不能使用一条曲线跨越换向；</li>
|
||||
<li>不改变绕障侧、前进/后退顺序和换向次数；</li>
|
||||
<li>Reeds–Shepp精确目标点固定；</li>
|
||||
<li>容差终点不在本步骤强行修改为目标；</li>
|
||||
<li>障碍物附近控制点移动范围收紧。</li>
|
||||
</ul>
|
||||
|
||||
<h3>12.4 典型偏移范围</h3>
|
||||
<pre>普通区域最大偏移：0.10–0.15 m
|
||||
障碍物附近最大偏移：0.02–0.05 m
|
||||
局部QP问题区间：失败点前后各约1.0 m</pre>
|
||||
|
||||
<h3>12.5 航向与曲率</h3>
|
||||
<ul>
|
||||
<li>平滑后必须根据新几何路径重新计算切线与航向；</li>
|
||||
<li>前进段车头航向与路径切线一致；</li>
|
||||
<li>倒车段车头航向与路径切线相差π；</li>
|
||||
<li>区分几何曲率与车辆控制曲率；</li>
|
||||
<li>建议平滑后最大车辆曲率不超过<code>0.95 κmax</code>。</li>
|
||||
</ul>
|
||||
|
||||
<h3>12.6 最终验证</h3>
|
||||
<ul>
|
||||
<li>0.05 m采样完整车体碰撞检测；</li>
|
||||
<li>Unknown与越界检查；</li>
|
||||
<li>安全余量检查；</li>
|
||||
<li>最大曲率与曲率变化检查；</li>
|
||||
<li>起终点、换向点、方向分段和路径连续性检查。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<section id="step13">
|
||||
<h2><span class="step-id">13</span>性能优化阶段 <span class="status todo">整体TODO</span></h2>
|
||||
<p>第十三步不作为第一版主流程实现要求。前12步完整跑通并建立正确性基线后，再进行性能剖析与针对性优化。</p>
|
||||
|
||||
<h3>13.1 第一版仅保留防失控措施</h3>
|
||||
<ul>
|
||||
<li>较宽松的搜索超时；</li>
|
||||
<li>最大扩展节点和最大生成节点限制；</li>
|
||||
<li>取消请求机制；</li>
|
||||
<li>各阶段基础耗时与节点数量统计。</li>
|
||||
</ul>
|
||||
|
||||
<h3>13.2 后续性能TODO</h3>
|
||||
<ol>
|
||||
<li>统计地图处理、Dijkstra、Hybrid A*、碰撞检测、Reeds–Shepp、回溯和平滑耗时；</li>
|
||||
<li>根据剖析结果识别真实瓶颈；</li>
|
||||
<li>优化Open List、Closed Set、节点内存与对象分配；</li>
|
||||
<li>预计算运动原语、航向模板、距离场和启发图；</li>
|
||||
<li>地图裁剪、路径热启动、增量更新、多分辨率搜索；</li>
|
||||
<li>最终验证平均、P95、最坏时间和100 ms达成率。</li>
|
||||
</ol>
|
||||
|
||||
<div class="decision">
|
||||
开发顺序：正确性 → 可行性 → 可复现性 → 性能剖析 → 针对性优化 → 100 ms验收。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step14">
|
||||
<h2><span class="step-id">14</span>输入输出接口、错误状态与验收 <span class="status done">已确认</span></h2>
|
||||
<h3>14.1 推荐模块</h3>
|
||||
<pre>HybridAStarPlanner
|
||||
├── MapProcessor
|
||||
├── VehicleModel
|
||||
├── MotionPrimitiveGenerator
|
||||
├── CollisionChecker
|
||||
├── HeuristicProvider
|
||||
├── ReedsSheppConnector
|
||||
├── GoalChecker
|
||||
├── PathBacktracker
|
||||
├── PathResampler
|
||||
├── PathSmoother
|
||||
├── PathValidator
|
||||
└── PlanningDiagnostics</pre>
|
||||
|
||||
<h3>14.2 主接口</h3>
|
||||
<pre>public interface IHybridAStarPlanner
|
||||
{
|
||||
PlanningResult Plan(PlanningRequest request);
|
||||
}</pre>
|
||||
|
||||
<h3>14.3 输入</h3>
|
||||
<ul>
|
||||
<li>二维占据栅格；</li>
|
||||
<li>起点位姿与可选起始方向、起始曲率；</li>
|
||||
<li>目标位姿与目标进入方向；</li>
|
||||
<li>车辆尺寸、安全余量、最大曲率/最小转弯半径；</li>
|
||||
<li>Hybrid A*配置、Reeds–Shepp模式和平滑配置。</li>
|
||||
</ul>
|
||||
|
||||
<h3>14.4 输出</h3>
|
||||
<p>输出不带时间的空间路径点和分段信息：</p>
|
||||
<pre>X, Y
|
||||
Heading, UnwrappedHeading
|
||||
ArcLength
|
||||
GeometricCurvature
|
||||
VehicleCurvature
|
||||
Direction
|
||||
BodyClearance
|
||||
IsGearSwitchPoint
|
||||
Source</pre>
|
||||
|
||||
<h3>14.5 终点到达类型</h3>
|
||||
<pre>ExactByReedsShepp
|
||||
ReachedWithinTolerance
|
||||
ExactAfterDirectSearch</pre>
|
||||
|
||||
<h3>14.6 主要返回状态</h3>
|
||||
<pre>Success
|
||||
SuccessWithToleranceGoal
|
||||
SuccessWithSmoothingFallback
|
||||
InvalidRequest / InvalidMap / InvalidVehicleParameters
|
||||
StartOutsideMap / StartInCollision / StartInUnknownArea
|
||||
GoalOutsideMap / GoalInCollision / GoalInUnknownArea
|
||||
InvalidCurvatureConfiguration
|
||||
SearchTimeout / SearchNodeLimitExceeded / NoFeasiblePath
|
||||
ReedsSheppRequiredButFailed
|
||||
BacktrackingFailed / ResamplingFailed / FinalValidationFailed
|
||||
Cancelled / InternalError</pre>
|
||||
|
||||
<h3>14.7 第一版验收重点</h3>
|
||||
<ul>
|
||||
<li>有效场景能输出完整路径；</li>
|
||||
<li>起点一致，终点满足第九步规则；</li>
|
||||
<li>完整车体全程无碰撞且不进入Unknown；</li>
|
||||
<li>最大曲率不超过车辆上限；</li>
|
||||
<li>前进、倒车和换向结构正确；</li>
|
||||
<li>回溯无断点和异常重复；</li>
|
||||
<li>平滑失败可以回退原路径；</li>
|
||||
<li>失败场景返回明确状态；</li>
|
||||
<li>后续SQP能够读取并收敛。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<section id="params">
|
||||
<h2>附录A：第一版推荐配置参数</h2>
|
||||
<table>
|
||||
<tr><th>参数</th><th>推荐初值</th><th>说明</th></tr>
|
||||
<tr><td>MapResolution</td><td>0.05 m</td><td>读取地图配置，不写死</td></tr>
|
||||
<tr><td>HeadingResolution</td><td>5°</td><td>72个航向模板</td></tr>
|
||||
<tr><td>SafetyMargin</td><td>0.03 m</td><td>主要加在车体矩形</td></tr>
|
||||
<tr><td>PrimitiveLength</td><td>0.50 m</td><td>第一版固定</td></tr>
|
||||
<tr><td>IntegrationStep</td><td>0.05 m</td><td>碰撞与积分步长</td></tr>
|
||||
<tr><td>CurvatureLevels</td><td>-1,-0.5,0,0.5,1 × κmax</td><td>5级</td></tr>
|
||||
<tr><td>GoalPositionTolerance</td><td>0.15 m</td><td>Hybrid A*基础容差</td></tr>
|
||||
<tr><td>GoalHeadingTolerance</td><td>5°</td><td>Hybrid A*基础容差</td></tr>
|
||||
<tr><td>HeuristicWeight</td><td>1.3</td><td>Weighted A*</td></tr>
|
||||
<tr><td>ReversePenalty</td><td>1.3</td><td>允许倒车但略微惩罚</td></tr>
|
||||
<tr><td>GearSwitchPenalty</td><td>2.0</td><td>减少频繁换向</td></tr>
|
||||
<tr><td>ReedsSheppMode</td><td>Opportunistic</td><td>默认自动机会式</td></tr>
|
||||
<tr><td>AnalyticExpansionDistance</td><td>5.0 m</td><td>进入后周期尝试</td></tr>
|
||||
<tr><td>NearGoalDistance</td><td>2.0 m</td><td>提高尝试频率</td></tr>
|
||||
<tr><td>AnalyticExpansionInterval</td><td>10节点</td><td>2–5 m</td></tr>
|
||||
<tr><td>NearGoalInterval</td><td>3节点</td><td>≤2 m</td></tr>
|
||||
<tr><td>NormalResampleSpacing</td><td>0.10 m</td><td>普通区域</td></tr>
|
||||
<tr><td>FineResampleSpacing</td><td>0.05 m</td><td>重点区域</td></tr>
|
||||
<tr><td>FineObstacleDistance</td><td>0.50 m</td><td>小于此值加密</td></tr>
|
||||
<tr><td>FineGoalDistance</td><td>2.0 m</td><td>目标附近加密</td></tr>
|
||||
<tr><td>GearSwitchDenseRange</td><td>±0.30 m</td><td>换向点附近</td></tr>
|
||||
<tr><td>CurvatureTransitionDenseRange</td><td>±0.25 m</td><td>曲率切换附近</td></tr>
|
||||
<tr><td>CurvatureLimitRatio</td><td>0.95</td><td>平滑后留控制余量</td></tr>
|
||||
<tr><td>SearchTimeLimitMs</td><td>3000–5000 ms</td><td>第一版防失控，不是性能指标</td></tr>
|
||||
</table>
|
||||
</section>
|
||||
|
||||
<section id="pseudocode">
|
||||
<h2>附录B：总体伪代码</h2>
|
||||
<pre>PlanningResult Plan(request)
|
||||
{
|
||||
ValidateRequest(request);
|
||||
ValidateMap(request.Map);
|
||||
ResolveVehicleCurvatureLimit(request.Vehicle);
|
||||
ValidateStartAndGoal();
|
||||
|
||||
PrepareDistanceFieldIfNeeded();
|
||||
PrepareHeadingFootprintTemplatesIfNeeded();
|
||||
BuildGoalDijkstraHeuristic();
|
||||
|
||||
startNode = CreateStartNode();
|
||||
PushOpen(startNode);
|
||||
|
||||
while (OpenList not empty)
|
||||
{
|
||||
CheckCancellationAndSafetyLimits();
|
||||
|
||||
current = PopBestValidNode();
|
||||
|
||||
if (ShouldTryReedsShepp(current))
|
||||
{
|
||||
rsPath = TryReedsShepp(current, goal);
|
||||
if (ValidateAnalyticPath(rsPath))
|
||||
return BuildFinalResult(current, rsPath);
|
||||
}
|
||||
|
||||
if (GoalChecker.CanTerminateByTolerance(current))
|
||||
return BuildFinalResult(current, noAnalyticPath);
|
||||
|
||||
foreach (primitive in GenerateAllowedPrimitives(current))
|
||||
{
|
||||
integrated = IntegratePrimitive(
|
||||
current,
|
||||
primitive,
|
||||
step = 0.05 m,
|
||||
maxLength = 0.50 m);
|
||||
|
||||
if (!integrated.Valid)
|
||||
continue;
|
||||
|
||||
child = CreateChildNode(integrated);
|
||||
if (!ImproveBestCost(child))
|
||||
continue;
|
||||
|
||||
PushOpen(child);
|
||||
}
|
||||
}
|
||||
|
||||
return Failure(NoFeasiblePath);
|
||||
}
|
||||
|
||||
BuildFinalResult(goalNode, analyticPath)
|
||||
{
|
||||
rawDense = BacktrackAndRestorePrimitivePoints(goalNode);
|
||||
AppendAnalyticPathIfAny(rawDense, analyticPath);
|
||||
|
||||
segmented = SplitByMotionDirection(rawDense);
|
||||
resampled = PreserveKeyPointsAndResample(segmented);
|
||||
|
||||
smoothed = TryFastSplineSmoothing(resampled);
|
||||
if (!ValidatePath(smoothed))
|
||||
smoothed = TryLocalCurveOrQPFallback(resampled);
|
||||
|
||||
if (!ValidatePath(smoothed))
|
||||
smoothed = resampled;
|
||||
|
||||
if (!ValidatePath(smoothed))
|
||||
return Failure(FinalValidationFailed);
|
||||
|
||||
return Success(smoothed, diagnostics);
|
||||
}</pre>
|
||||
</section>
|
||||
|
||||
<section id="milestones">
|
||||
<h2>附录C：建议开发里程碑</h2>
|
||||
<table>
|
||||
<tr><th>阶段</th><th>范围</th><th>完成标准</th></tr>
|
||||
<tr><td>M0 数据与工具</td><td>Pose、地图、车辆参数、角度/坐标工具</td><td>单元测试通过</td></tr>
|
||||
<tr><td>M1 运动学与碰撞</td><td>恒曲率积分、航向模板、矩形碰撞</td><td>可视化验证不同角度无漏检</td></tr>
|
||||
<tr><td>M2 最小Hybrid A*</td><td>无障碍、只前进、基础启发</td><td>稳定从起点到终点</td></tr>
|
||||
<tr><td>M3 障碍与倒车</td><td>Dijkstra、前进/倒车、换向代价</td><td>绕障与一次倒车场景通过</td></tr>
|
||||
<tr><td>M4 终点模块</td><td>容差、GoalDirection、Reeds–Shepp三模式</td><td>普通与狭窄目标场景通过</td></tr>
|
||||
<tr><td>M5 后处理</td><td>回溯、0.05 m稠密点、分段、重采样</td><td>路径结构无断点、换向明确</td></tr>
|
||||
<tr><td>M6 平滑与复核</td><td>B样条、Bézier、局部QP兜底</td><td>不改变拓扑，失败可回退</td></tr>
|
||||
<tr><td>M7 接口与集成</td><td>PlanningRequest/Result、状态、诊断</td><td>可接入SQP</td></tr>
|
||||
<tr><td>M8 性能TODO</td><td>剖析、优化、100 ms目标</td><td>在正确性基线后执行</td></tr>
|
||||
</table>
|
||||
</section>
|
||||
</main>
|
||||
</div>
|
||||
|
||||
<footer>
|
||||
文档用途：四舵轮AMR第一阶段非结构化道路粗路径规划开发依据。内部单位统一采用米、弧度、1/米。
|
||||
</footer>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,726 @@
|
||||
# Hybrid A* 粗路径规划实施计划
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`，按任务顺序实施，并使用 `- [ ]` 更新执行状态。
|
||||
|
||||
**目标：** 在 ClumsyPilot `netstandard2.0` 项目中交付可复用的静态规划地图和 Hybrid A* 粗路径规划模块，通过一次 `CoarsePathPlanningService.Plan(job)` 完成建图、快照复用、粗路径搜索和可选调试发布。
|
||||
|
||||
**架构：** `Map` 模块统一接收人工障碍、TwoLeg 及未来障碍来源，生成只含外部障碍物的不可变 `PlanningGridMap`；`HybridAStarPlanner` 只消费该快照并完成保守碰撞检查与搜索；`CoarsePathPlanningService` 负责编排。核心逻辑不读取传感器、UI 或系统时间，Clumsy `MovementTest` 和 PNG 导出只作为旁路适配器。
|
||||
|
||||
**技术栈：** C# 10、.NET Standard 2.0、PowerShell 反射契约测试、Clumsy `MovementTest`、StbImageWriteSharp 1.16.7。
|
||||
|
||||
**设计依据：** `docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md`
|
||||
|
||||
## 全局约束
|
||||
|
||||
- 所有命令均从仓库根目录执行；项目文件为 `ClumsyPilot/ClumsyPilot.csproj`。
|
||||
- 目标框架保持 `netstandard2.0`，不得直接使用 `PriorityQueue`、`Math.Clamp`、`double.IsFinite` 或依赖 `record/init` 的实现。
|
||||
- 地图边界、人工障碍和 TwoLeg 快照使用世界坐标 mm；规划内部统一使用 m、rad、1/m。
|
||||
- 地图只保存外部障碍物，不写入 AMR 自身足迹，不在地图侧添加车辆安全膨胀。
|
||||
- 安全余量只在碰撞检查时扩大车体矩形，防止地图膨胀与车体膨胀重复计算。
|
||||
- `ResolutionMm` 必须在 20～200 mm；地图最多 4,000,000 格，分配前使用 `checked` 检查。
|
||||
- 地图坐标采用 `[XMin, XMax) × [YMin, YMax)`；地图外始终按占据处理。
|
||||
- `PrimitiveLengthMeters=0.50` 表示原语最大长度；`IntegrationStepMeters=0.05` 表示积分最大步长。
|
||||
- 碰撞采样中心位移不得超过 `min(0.025 m, Map.ResolutionMeters / 2)`。
|
||||
- 默认终点容差为 0.15 m 和 5°。每个原语在内部积分点逐点检查目标，第一次满足条件即截断为终点候选。
|
||||
- 生成终点候选时不能立即成功；候选必须进入 Open List，作为当前最佳有效条目出队时才成功。
|
||||
- 第一版只支持恒曲率前进、倒车及原语边界换向；不支持蟹行、横移、原地旋转、Reeds-Shepp 精确连接、平滑、速度规划或底盘控制。
|
||||
- PowerShell 脚本首行设置 `$ErrorActionPreference = 'Stop'`，统一加载 `bin/Debug/netstandard2.0/ClumsyPilot.dll`。
|
||||
- 用户已明确不需要 Git 自检。本计划不包含 `git diff`、`git add`、`git commit` 等步骤；版本管理由用户另行处理。
|
||||
|
||||
## P0 与 P1 完成定义
|
||||
|
||||
| 阶段 | 必须完成的结果 | 阶段出口 |
|
||||
| --- | --- | --- |
|
||||
| P0-MAP：当前首要工作 | Map 文件结构、统一障碍来源、TwoLeg 投影、栅格化、距离场、快照缓存、PNG 迁移、自动测试和 `MovementTest.MapTest` 实际调用 | Map 独立构建成功；3 个 Map 脚本通过；Clumsy MapTest 只通过 `PlanningMapFactory.Create` 完成建图与显示 |
|
||||
| P0-PLAN：Map Gate 之后 | CoarsePath 契约、保守碰撞、原语截断、Open List、Hybrid A*、输出校验和一次调用门面 | 6 个非 UI P0 脚本全部通过；固定场景可由 `CoarsePathPlanningService` 返回经最终复核的粗路径 |
|
||||
| P1：集成与优化 | CoarsePath MovementTest、性能基准、旧地图退役和 README | 7 个功能脚本及 Release 基准通过；UI 入口只做调试展示；旧地图类型不再成为规划运行时入口 |
|
||||
|
||||
执行顺序固定为“P0-MAP 独立闭环 → P0-PLAN → P1”。除了 Map 自己的 `MovementTest.MapTest` 和 PNG 调试辅助外，在 Map Gate 通过前不编写粗路径搜索或 CoarsePath UI，避免旧 `MovementTest.Trapmaptest.cs` 的传感器和渲染依赖进入核心模块。
|
||||
|
||||
## 当前首要里程碑：P0-MAP 独立闭环
|
||||
|
||||
当前阶段只处理 `Utils` 中被 Map 使用的无状态工具、完整 `Map` 目录、Map 自动化测试和 Map 实际调用。不得提前创建 `CoarsePath/Search`、`CoarsePath/Vehicle` 或 `CoarsePath/Output` 运行时代码。
|
||||
|
||||
### Map 实施顺序
|
||||
|
||||
1. 建立 `Map/Core`、`Map/Obstacles`、`Map/Sources`、`Map/Planning`、`Map/Test/Visualization` 目录和命名空间。
|
||||
2. 完成 `MapBoundsMm`、连续行优先 `EnvironmentGridMap`、圆/矩形 DTO 和唯一栅格化器。
|
||||
3. 完成统一 `IMapObstacleSource`、人工来源、TwoLeg 快照来源和事务式 `EnvironmentMapBuilder`。
|
||||
4. 完成 `PlanningGridMap`、精确 EDT、保守距离场和 mm→m 适配。
|
||||
5. 完成 `PlanningMapFactory`、容量 4 的两级快照缓存和地图变化判断。
|
||||
6. 将旧 `TrapMapImageExporter.cs` 能力拆到 `Map/Test/Visualization`，只消费只读快照。
|
||||
7. 完成三个 Map PowerShell 脚本和 `MovementTest.MapTest`，用真实入口验证建图、复用和显示。
|
||||
|
||||
对应详细任务的执行次序为：`Task 1 → Task 3 → Task 4 → Task 5 → Task 6 → Task 13 → Task 14 的 MapTest 部分 → Map Gate`。`Task 2` 和 `Task 7～12` 在 Map Gate 之后执行。
|
||||
|
||||
### Map 实际调用契约
|
||||
|
||||
`PlanningMapFactory` 应由长期存在的服务或测试对象持有，不能每次调用都重新 `new`，否则容量 4 的快照缓存无法跨调用复用：
|
||||
|
||||
```csharp
|
||||
private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();
|
||||
|
||||
public PlanningMapBuildResult CreateCurrentMap(
|
||||
MapBoundsMm bounds,
|
||||
long manualVersion,
|
||||
IReadOnlyList<IMapObstacle> manualObstacles,
|
||||
long twoLegVersion,
|
||||
TwoLegProjectionInput twoLegSnapshot)
|
||||
{
|
||||
return _mapFactory.Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = bounds,
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource(
|
||||
"manual", manualVersion, true, manualObstacles),
|
||||
new TwoLegObstacleSource(
|
||||
"two-leg", twoLegVersion, false, twoLegSnapshot),
|
||||
},
|
||||
AllowExplicitEmptyMap = false,
|
||||
});
|
||||
}
|
||||
```
|
||||
|
||||
调用方只判断统一结果，不接触 builder、rasterizer、投影器或 adapter：
|
||||
|
||||
```csharp
|
||||
PlanningMapBuildResult result = CreateCurrentMap(
|
||||
bounds, manualVersion, manualObstacles, twoLegVersion, twoLegSnapshot);
|
||||
|
||||
if (!result.Succeeded || result.Map == null || !result.Map.PlanningReady)
|
||||
return;
|
||||
|
||||
PlanningGridMap map = result.Map;
|
||||
bool worldOriginIsOccupied = map.IsOccupiedWorld(0d, 0d);
|
||||
double worldOriginClearance =
|
||||
map.GetConservativeObstacleDistanceMeters(0d, 0d);
|
||||
```
|
||||
|
||||
### Map Gate
|
||||
|
||||
以下条件必须全部满足，才开始 CoarsePath 契约和搜索：
|
||||
|
||||
- [ ] 新 `Map` 运行时代码不读取 `TwoLegDetect`、定位、Painter、Toast、UI 或系统时间。
|
||||
- [ ] 人工、TwoLeg 以及未来来源全部经 `IMapObstacleSource.ProjectToWorld()` 输出世界 mm 几何，再由唯一 rasterizer 写图。
|
||||
- [ ] 地图不写 AMR 自身，不使用旧默认 300 mm 膨胀。
|
||||
- [ ] `PlanningMapFactory.Create` 对完整输入命中、占据命中和占据变化给出可区分结果。
|
||||
- [ ] `MovementTest.MapTest` 只调用 `PlanningMapFactory` 和可选 PNG exporter，不复制地图构造逻辑。
|
||||
- [ ] 下列命令全部通过：
|
||||
|
||||
```powershell
|
||||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_adapter.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_image.ps1
|
||||
```
|
||||
|
||||
### P0-MAP 执行状态（2026-07-27）
|
||||
|
||||
- [x] 已完成 Task 1、Task 3～6、Task 13，以及 Task 14 的 `MovementTest.MapTest` 部分。
|
||||
- [x] 已建立新的 `Map` 目录结构、统一障碍物来源与 TwoLeg 检测时位姿投影；Map 核心不读取传感器、UI 或系统时钟。
|
||||
- [x] 已完成只读规划快照、保守 EDT 距离场、容量 4 的 LRU 缓存、PNG 可视化拆分和实际 MapTest 调用。
|
||||
- [x] 已通过上述 Map Gate 命令；另外 `verify_planning_utils.ps1` 也已通过。
|
||||
- [x] 未执行 Git 自检、暂存或提交。
|
||||
|
||||
## 最终目录与文件职责
|
||||
|
||||
```text
|
||||
ClumsyPilot/ParkrobTrajplanner/
|
||||
├── Initial_plan/ ------ 方案与实施文档，不放运行时代码
|
||||
├── Utils/ ------ 通用、无状态、确定性数值工具
|
||||
│ ├── AngleMath.cs ------ 角度归一化、最短角差和航向离散索引
|
||||
│ ├── UnitConverter.cs ------ mm/m、deg/rad 和半径/曲率转换
|
||||
│ ├── CoordinateTransform.cs ------ 车体坐标与世界坐标二维刚体变换
|
||||
│ ├── NumericGuard.cs ------ 有限值、正值和参数范围校验
|
||||
│ └── GridIndex.cs ------ 不可变行列索引值对象
|
||||
├── Map/ ------ MultiWheelC.TrajectoryPlanning.Mapping
|
||||
│ ├── Core/
|
||||
│ │ ├── EnvironmentGridMap.cs ------ 只保存外部障碍物的 mm 占据栅格
|
||||
│ │ ├── MapBoundsMm.cs ------ 有限、非退化的 mm 地图边界
|
||||
│ │ ├── MapBuildRequest.cs ------ 内部环境图构建请求
|
||||
│ │ ├── EnvironmentMapBuildResult.cs ------ 环境图、来源摘要和失败原因
|
||||
│ │ └── EnvironmentMapBuilder.cs ------ 校验来源并事务式合并障碍图层
|
||||
│ ├── Obstacles/
|
||||
│ │ ├── IMapObstacle.cs ------ 世界坐标障碍物公共几何契约
|
||||
│ │ ├── AxisAlignedRectangleObstacle.cs ------ 轴对齐矩形障碍 DTO
|
||||
│ │ ├── CircleObstacle.cs ------ 圆形障碍 DTO
|
||||
│ │ └── MapObstacleRasterizer.cs ------ 唯一占据栅格写入器
|
||||
│ ├── Sources/
|
||||
│ │ ├── IMapObstacleSource.cs ------ 纯快照障碍来源统一接口
|
||||
│ │ ├── ObstacleSourceStatus.cs ------ Applied/Empty/Unavailable/Invalid
|
||||
│ │ ├── ObstacleProjectionResult.cs ------ 来源版本、状态、诊断和障碍集合
|
||||
│ │ ├── ManualObstacleSource.cs ------ 输出人工圆和矩形
|
||||
│ │ ├── TwoLegProjectionInput.cs ------ 两腿端点、检测状态和检测位姿快照
|
||||
│ │ ├── TwoLegObstacleSource.cs ------ TwoLeg 统一来源适配器
|
||||
│ │ └── TwoLegObstacleProjector.cs ------ 车体系两腿端点投影到世界系
|
||||
│ ├── Planning/
|
||||
│ │ ├── PlanningGridMap.cs ------ 只读 m 占据图、距离场和快照元数据
|
||||
│ │ ├── EuclideanDistanceTransform.cs ------ 线性时间精确二维欧氏距离变换
|
||||
│ │ ├── ObstacleDistanceField.cs ------ 不高估真实净空的距离下界
|
||||
│ │ ├── PlanningMapCache.cs ------ 容量 4 的线程安全两级快照缓存
|
||||
│ │ └── PlanningMapAdapter.cs ------ 占据深拷贝、mm→m 和距离场生成
|
||||
│ ├── PlanningMapRequest.cs ------ Map 模块统一输入
|
||||
│ ├── PlanningMapBuildResult.cs ------ Map 模块统一输出和缓存命中类型
|
||||
│ ├── PlanningMapFactory.cs ------ Map 模块唯一公共创建入口
|
||||
│ └── Test/
|
||||
│ ├── MovementTest.MapTest.cs ------ Clumsy UI 地图构建/显示入口
|
||||
│ └── Visualization/
|
||||
│ ├── PlanningMapImageExportRequest.cs ------ 快照、叠加层和输出选项
|
||||
│ ├── PlanningMapImageExportResult.cs ------ PNG 状态、路径、尺寸和诊断
|
||||
│ ├── PlanningMapImageExporter.cs ------ 校验、渲染编排和原子发布
|
||||
│ ├── PlanningMapImageRenderer.cs ------ 地图/车辆/路径绘制到 RGBA
|
||||
│ └── ValidatedPngWriter.cs ------ Stb 编码、PNG 结构和 CRC 校验
|
||||
├── CoarsePath/ ------ MultiWheelC.TrajectoryPlanning.CoarsePath
|
||||
│ ├── Contracts/
|
||||
│ │ ├── Pose2D.cs ------ m/rad 不可变二维位姿
|
||||
│ │ ├── TravelDirection.cs ------ Forward/Reverse
|
||||
│ │ ├── GoalDirectionConstraint.cs ------ Any/Forward/Reverse
|
||||
│ │ ├── VehicleParameters.cs ------ 尺寸、安全余量和最大曲率
|
||||
│ │ ├── PlanningRequest.cs ------ 纯规划器输入
|
||||
│ │ ├── HybridAStarConfiguration.cs ------ 原语、离散、代价、限额和容差
|
||||
│ │ ├── PlanningResult.cs ------ 状态、诊断、稠密路径和分段
|
||||
│ │ ├── PlanningStatus.cs ------ 输入、碰撞、搜索和验证状态
|
||||
│ │ ├── PlanningDiagnostics.cs ------ 节点、堆、耗时、路径和终止统计
|
||||
│ │ ├── CoarsePathPoint.cs ------ 位姿、弧长、方向、曲率和净空
|
||||
│ │ ├── CoarsePathPointSource.cs ------ Start/MotionPrimitive/GoalTruncation
|
||||
│ │ └── PathSegment.cs ------ 包含式方向分段索引
|
||||
│ ├── Vehicle/
|
||||
│ │ ├── VehicleKinematics.cs ------ 解析保守最大曲率
|
||||
│ │ ├── VehicleFootprint.cs ------ 扩大矩形、AABB 和外接圆
|
||||
│ │ ├── OrientedRectangleCellIntersection.cs ------ 旋转矩形与格矩形精确相交
|
||||
│ │ └── FootprintCollisionChecker.cs ------ 边界、快速放行、精确和扫掠检查
|
||||
│ ├── Search/
|
||||
│ │ ├── MotionPrimitive.cs ------ 恒曲率原语和实际截断长度
|
||||
│ │ ├── MotionPrimitiveGenerator.cs ------ 解析积分并保留内部采样点
|
||||
│ │ ├── BinaryMinHeap.cs ------ netstandard2.0 确定性 Open List
|
||||
│ │ ├── SearchCostCalculator.cs ------ 统一计算等效米搜索代价
|
||||
│ │ ├── HybridAStarNode.cs ------ 连续状态、代价、父索引和原语
|
||||
│ │ ├── HybridAStarNodeKey.cs ------ 位置/航向/方向/曲率离散键
|
||||
│ │ ├── GoalToleranceChecker.cs ------ 位置、航向和进入方向判断
|
||||
│ │ ├── GridDijkstraHeuristic.cs ------ 八邻域、禁止切角的二维启发
|
||||
│ │ └── HybridAStarSearch.cs ------ 扩展、重开、限额和候选管理
|
||||
│ ├── Output/
|
||||
│ │ ├── PathBacktracker.cs ------ 根据父索引重建原语内部点
|
||||
│ │ ├── CoarsePathAssembler.cs ------ 弧长、换向点和方向分段
|
||||
│ │ └── CoarsePathValidator.cs ------ 数值、碰撞、曲率、终点和分段复核
|
||||
│ ├── HybridAStarPlanner.cs ------ 只消费 PlanningGridMap 的下层门面
|
||||
│ ├── Facade/
|
||||
│ │ ├── CoarsePathPlanningJob.cs ------ 一次调用所需全部输入
|
||||
│ │ ├── CoarsePathPlanningJobResult.cs ------ 地图结果与粗路径结果
|
||||
│ │ ├── PlanningDebugOptions.cs ------ 地图、路径、碰撞调试开关
|
||||
│ │ ├── IPlanningDebugSink.cs ------ 不改变规划状态的调试消费接口
|
||||
│ │ └── CoarsePathPlanningService.cs ------ 建图、搜索和调试的一次调用入口
|
||||
│ └── Test/
|
||||
│ ├── CoarsePathScenarioFactory.cs ------ 固定、可复现的地图与规划场景
|
||||
│ └── MovementTest.CoarsePathTest.cs ------ 后台规划并在 Clumsy UI 显示
|
||||
│ └── README.md ------ 粗规划模块边界、调用示例和文档链接
|
||||
|
||||
ClumsyPilot/tests/
|
||||
├── verify_planning_utils.ps1 ------ 工具与公共契约
|
||||
├── verify_planning_map_factory.ps1 ------ 来源、事务、缓存与快照
|
||||
├── verify_planning_map_adapter.ps1 ------ 栅格、适配和距离场
|
||||
├── verify_planning_map_image.ps1 ------ PNG 渲染与原子发布
|
||||
├── verify_coarse_path_collision.ps1 ------ 亚栅格、擦边和扫掠碰撞
|
||||
├── verify_coarse_path_search.ps1 ------ 原语截断、堆、代价和搜索
|
||||
├── verify_coarse_path_integration.ps1 ------ 一次调用端到端验证
|
||||
└── benchmark_coarse_path.ps1 ------ Release 性能与内存门槛
|
||||
```
|
||||
|
||||
## 固定公共调用方式
|
||||
|
||||
常规业务代码只保留一个长期存在的服务实例：
|
||||
|
||||
```csharp
|
||||
var result = planningService.Plan(new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = bounds,
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource(
|
||||
"manual", manualVersion, true, manualObstacles),
|
||||
new TwoLegObstacleSource("two-leg", twoLegVersion, false, twoLegSnapshot),
|
||||
},
|
||||
AllowExplicitEmptyMap = true,
|
||||
},
|
||||
Start = startPose,
|
||||
Goal = goalPose,
|
||||
Vehicle = vehicle,
|
||||
Configuration = configuration,
|
||||
StartVehicleCurvature = 0d,
|
||||
StartDirection = null,
|
||||
GoalDirection = GoalDirectionConstraint.Any,
|
||||
Debug = new PlanningDebugOptions
|
||||
{
|
||||
VisualizeMap = true,
|
||||
VisualizePath = true,
|
||||
VisualizeCollisionChecks = false,
|
||||
},
|
||||
}, cancellationToken);
|
||||
```
|
||||
|
||||
已有地图快照复用或纯搜索测试才直接调用 `HybridAStarPlanner.Plan(PlanningRequest, CancellationToken)`。业务调用方不得直接拼装 builder、rasterizer、碰撞器、运动原语或搜索节点。
|
||||
|
||||
## P0：正确可用
|
||||
|
||||
### Task 1：建立 Utils 与测试基座
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/AngleMath.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/UnitConverter.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/CoordinateTransform.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/NumericGuard.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/GridIndex.cs`
|
||||
- Create: `ClumsyPilot/tests/verify_planning_utils.ps1`
|
||||
|
||||
**产出接口：**
|
||||
|
||||
```csharp
|
||||
double AngleMath.NormalizeRadians(double radians);
|
||||
double AngleMath.ShortestSignedDifference(double from, double to);
|
||||
int AngleMath.ToHeadingIndex(double heading, double resolution, int binCount);
|
||||
double UnitConverter.MillimetersToMeters(double value);
|
||||
double UnitConverter.DegreesToRadians(double value);
|
||||
bool NumericGuard.IsFinite(double value);
|
||||
readonly struct GridIndex { int Row; int Col; }
|
||||
```
|
||||
|
||||
- [ ] 写失败测试：断言 `2π→0`、`179°→-179°` 最短角差为 `2°`、`1250 mm→1.25 m`、车体系 `(1000,0)` 在世界位姿 `(2000,3000,90°)` 后得到 `(2,4) m`，并拒绝 NaN/Infinity。
|
||||
- [ ] 执行以下命令，预期构建成功、脚本因新类型不存在返回非零。
|
||||
|
||||
```powershell
|
||||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
|
||||
```
|
||||
|
||||
- [ ] 实现：角度统一到 `[-π,π)`；航向索引先归一化再 `floor`；刚体变换使用 `world=origin+R(heading)×local`；`GridIndex` 实现值相等和稳定哈希。
|
||||
- [ ] 重跑脚本，预期输出 `Planning utility checks passed.`。
|
||||
|
||||
### Task 2：锁定 CoarsePath 公共契约与默认参数
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `CoarsePath/Contracts/` 下目录树列出的 12 个契约文件
|
||||
- Modify: `ClumsyPilot/tests/verify_planning_utils.ps1`
|
||||
|
||||
**固定默认值：**
|
||||
|
||||
```csharp
|
||||
PrimitiveLengthMeters = 0.50;
|
||||
IntegrationStepMeters = 0.05;
|
||||
MaximumCollisionCheckStepMeters = 0.025;
|
||||
HeadingResolutionRadians = Math.PI / 36d;
|
||||
CurvatureLevelCount = 5;
|
||||
GoalPositionToleranceMeters = 0.15;
|
||||
GoalHeadingToleranceRadians = Math.PI / 36d;
|
||||
MaximumExpandedNodes = 200000;
|
||||
SearchTimeout = TimeSpan.FromSeconds(5);
|
||||
HeuristicWeight = 1.0;
|
||||
ReverseCostMultiplier = 1.5;
|
||||
GearSwitchPenaltyMeters = 1.0;
|
||||
CurvatureMagnitudeWeight = 0.10;
|
||||
CurvatureChangePenaltyMetersPerLevel = 0.05;
|
||||
ClearanceCostWeight = 0.20;
|
||||
ClearanceCostDistanceMeters = 0.50;
|
||||
```
|
||||
|
||||
- [ ] 扩展失败测试：断言上述默认值，三个方向/来源枚举，以及 `PlanningRequest` 的起始曲率、起始方向、目标进入方向。
|
||||
- [ ] 断言 `PlanningStatus` 包含：`Success`、`Cancelled`、`InvalidRequest`、`InvalidMap`、`MapNotReady`、`InvalidVehicleParameters`、`InvalidCurvatureConfiguration`、`StartOutsideMap`、`StartInCollision`、`GoalOutsideMap`、`GoalInCollision`、`SearchTimeout`、`SearchNodeLimitExceeded`、`NoFeasiblePath`、`BacktrackingFailed`、`FinalValidationFailed`、`InternalError`。
|
||||
- [ ] 实现不可变 `Pose2D`；结果工厂只允许成功结果携带路径，失败结果路径为空。
|
||||
- [ ] `PlanningDiagnostics` 固定记录扩展节点数、生成节点数、重新打开节点数、陈旧堆条目数、Open List 峰值、总路径长度、最小保守净空、耗时和终止原因。
|
||||
- [ ] `CoarsePathPoint` 固定包含 `X/Y`、`Heading/UnwrappedHeading`、`ArcLength`、`Direction`、`VehicleCurvature`、`BodyClearance`、`IsGearSwitchPoint` 和 `Source`；`PathSegment` 固定包含方向和包含式 `StartIndex/EndIndex`。
|
||||
- [ ] 重跑工具脚本，预期契约和默认值全部通过。
|
||||
|
||||
### Task 3：实现环境栅格、边界与统一栅格化
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `Map/Core/MapBoundsMm.cs`
|
||||
- Create: `Map/Core/EnvironmentGridMap.cs`
|
||||
- Create: `Map/Obstacles/IMapObstacle.cs`
|
||||
- Create: `Map/Obstacles/AxisAlignedRectangleObstacle.cs`
|
||||
- Create: `Map/Obstacles/CircleObstacle.cs`
|
||||
- Create: `Map/Obstacles/MapObstacleRasterizer.cs`
|
||||
- Create: `ClumsyPilot/tests/verify_planning_map_adapter.ps1`
|
||||
|
||||
上述 Map/CoarsePath 相对路径均位于 `ClumsyPilot/ParkrobTrajplanner/`。
|
||||
|
||||
- [ ] 写边界和栅格失败测试：20/200 mm 合法，范围外失败；4,000,001 格分配前失败；`XMax/YMax` 排他；非完整末格裁剪；地图外占据；圆/矩形与格边或格角接触时保守占据；完全在地图外的合法障碍不写格。
|
||||
- [ ] 运行脚本，预期新 Map 类型不存在。
|
||||
- [ ] 实现私有行优先 `byte[]`，索引固定为 `row*Cols+col`；世界转格使用 `floor((value-min)/resolution)`；不得返回内部缓冲区。
|
||||
- [ ] 实现唯一栅格化器：圆使用“圆心到格矩形最近点距离”，轴对齐矩形使用闭区间相交，只遍历裁剪后的候选包围盒。
|
||||
- [ ] 确认不存在 `MarkVehicleFootprint` 或安全距离参数，重跑脚本通过。
|
||||
|
||||
### Task 4：统一人工与 TwoLeg 障碍来源
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `Map/Sources/` 下目录树列出的 7 个文件
|
||||
- Create: `Map/Core/MapBuildRequest.cs`
|
||||
- Create: `Map/Core/EnvironmentMapBuildResult.cs`
|
||||
- Create: `Map/Core/EnvironmentMapBuilder.cs`
|
||||
- Create: `ClumsyPilot/tests/verify_planning_map_factory.ps1`
|
||||
|
||||
**固定接口：**
|
||||
|
||||
```csharp
|
||||
public interface IMapObstacleSource
|
||||
{
|
||||
string SourceId { get; }
|
||||
long SourceVersion { get; }
|
||||
bool IsRequired { get; }
|
||||
ObstacleProjectionResult ProjectToWorld();
|
||||
}
|
||||
```
|
||||
|
||||
- [ ] 构造函数统一为 `ManualObstacleSource(string sourceId, long sourceVersion, bool isRequired, IReadOnlyList<IMapObstacle> obstacles)` 和 `TwoLegObstacleSource(string sourceId, long sourceVersion, bool isRequired, TwoLegProjectionInput input)`；计划内所有实际调用均使用这两个签名。
|
||||
- [ ] 写来源事务失败测试：人工 `Applied/Empty`、TwoLeg 两个圆、可选来源 `Unavailable/Invalid` 保留人工图层、必需来源失败导致整图失败、重复 `SourceId` 失败、输入顺序不同但结果相同。
|
||||
- [ ] 校验 `SourceId` 非空且一次请求内唯一，`SourceVersion` 非负；来源快照内容发生变化时，上层必须递增版本。
|
||||
- [ ] 写 TwoLeg 坐标测试：使用检测时车辆位姿完成车体系 mm 到世界系 mm 变换，不得使用规划开始时位姿。
|
||||
- [ ] 实现纯快照投影：`ProjectToWorld()` 不能调用 `TwoLegDetect`、定位、UI 或系统时间；过期判断由上层采集适配器在构造 DTO 前完成。
|
||||
- [ ] builder 按 `SourceId` 排序；可选失败只记诊断；必需失败不发布地图；全部成功几何交给唯一 rasterizer。
|
||||
- [ ] 重跑工厂脚本，预期来源状态、投影和事务断言通过。
|
||||
|
||||
### Task 5：生成不可变 PlanningGridMap 和保守距离场
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `Map/Planning/EuclideanDistanceTransform.cs`
|
||||
- Create: `Map/Planning/ObstacleDistanceField.cs`
|
||||
- Create: `Map/Planning/PlanningGridMap.cs`
|
||||
- Create: `Map/Planning/PlanningMapAdapter.cs`
|
||||
- Modify: `ClumsyPilot/tests/verify_planning_map_adapter.ps1`
|
||||
|
||||
- [ ] 扩展失败测试：mm→m、占据深拷贝、地图外距离为零、障碍格距离为零、空图距离正无穷、末格裁剪，以及所有样本距离不高于暴力几何距离。
|
||||
- [ ] 实现两次一维平方距离变换，复杂度 `O(Rows×Cols)`；不得逐自由格遍历全部障碍格。
|
||||
- [ ] 对精确栅格中心距离应用：
|
||||
|
||||
```csharp
|
||||
Math.Max(0d, centerDistanceMeters - Math.Sqrt(2d) * resolutionMeters)
|
||||
```
|
||||
|
||||
- [ ] 查询使用点所在格的保守值，不做可能抬高结果的插值；空图跳过 EDT；边界检查独立执行。
|
||||
- [ ] `PlanningGridMap` 私有保存连续占据/距离数组，不提供写入口和缓冲区引用。
|
||||
- [ ] 重跑地图适配脚本，预期栅格、深拷贝、空图和保守距离全部通过。
|
||||
|
||||
### Task 6：实现 PlanningMapFactory 与两级快照缓存
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `Map/Planning/PlanningMapCache.cs`
|
||||
- Create: `Map/PlanningMapRequest.cs`
|
||||
- Create: `Map/PlanningMapBuildResult.cs`
|
||||
- Create: `Map/PlanningMapFactory.cs`
|
||||
- Modify: `Map/Planning/PlanningGridMap.cs`
|
||||
- Modify: `ClumsyPilot/tests/verify_planning_map_factory.ps1`
|
||||
|
||||
```csharp
|
||||
public sealed class PlanningMapFactory
|
||||
{
|
||||
public PlanningMapBuildResult Create(PlanningMapRequest request);
|
||||
}
|
||||
```
|
||||
|
||||
`PlanningMapRequest` 固定包含 `MapBoundsMm Bounds`、`float ResolutionMm`、`IReadOnlyList<IMapObstacleSource> ObstacleSources`、`bool AllowExplicitEmptyMap`。
|
||||
|
||||
- [ ] 写缓存测试：完全相同请求返回同一 `PlanningGridMap`；来源版本变化但最终占据未变化时产生新 `SnapshotId` 并复用占据/距离缓冲；占据变化时重建距离场。
|
||||
- [ ] 写规划可用性测试：至少一个成功来源提供有效障碍语义时 `PlanningReady=true`；所有来源均为空时仅 `AllowExplicitEmptyMap=true` 可用；必需来源失败或未明确空图语义时 `PlanningReady=false` 并填写 `PlanningBlockReason`，规划器随后返回 `MapNotReady`。
|
||||
- [ ] 验证失效边界：起终点、车辆、规划参数和可视化开关不进入地图指纹；边界、分辨率、空图策略、来源状态/版本或规范化几何变化必须进入。
|
||||
- [ ] 实现确定性 `InputFingerprint`：来源先按 ID 排序，字段固定顺序，浮点按 IEEE 位模式；命中后仍比较规范化结构。
|
||||
- [ ] 栅格化后计算 `OccupancyHash`；命中后仍比较地图几何、数组长度和逐字节内容，不能只信任哈希。
|
||||
- [ ] 实现容量 4 的线程安全 LRU；完整命中返回同一快照，占据命中共享不可变缓冲但生成新元数据，只有占据变化才运行 EDT。
|
||||
- [ ] `PlanningMapBuildResult` 固定返回 `Succeeded`、失败原因、来源摘要、缓存命中类型和成功时的 `PlanningGridMap`；快照固定保存 `PlanningReady`、`PlanningBlockReason`、来源版本摘要、`InputFingerprint`、`OccupancyHash`、单调 `SnapshotId`。
|
||||
- [ ] 增加 16 个并发相同请求测试和 LRU 淘汰测试，重跑脚本通过。
|
||||
|
||||
### Task 7：实现连续车体足迹和保守碰撞检查
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `CoarsePath/Vehicle/` 下目录树列出的 4 个文件
|
||||
- Create: `ClumsyPilot/tests/verify_coarse_path_collision.ps1`
|
||||
|
||||
**固定接口：**
|
||||
|
||||
```csharp
|
||||
bool IsPoseCollisionFree(
|
||||
Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
|
||||
double additionalMarginMeters, out double bodyClearanceMeters);
|
||||
|
||||
bool IsSweptMotionCollisionFree(
|
||||
Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
|
||||
double maximumCenterStepMeters, out double minimumBodyClearanceMeters);
|
||||
```
|
||||
|
||||
- [ ] 写失败测试：正交、45°、任意航向、栅格中心/亚栅格中心、边角接触、薄障碍、地图边界、距离场快速放行，以及两个无碰撞端点之间有障碍的扫掠场景。
|
||||
- [ ] 实现以几何中心为参考的扩大车体；安全余量加到长度和宽度两侧；最大曲率与最小转弯半径并存时取更保守限制。
|
||||
- [ ] 使用分离轴定理精确判断连续旋转矩形与占据格矩形相交，接触视为碰撞；不得使用离散航向模板。
|
||||
- [ ] 检查顺序：扩大车体边界 → 保守距离严格大于外接圆时快速放行 → AABB 内占据格 SAT。
|
||||
- [ ] 扫掠采样中心步长不超过 `min(configuredStep,map.Resolution/2)`；每段临时附加余量为：
|
||||
|
||||
```text
|
||||
0.5 × (centerDisplacement
|
||||
+ circumscribedRadius × abs(headingDelta))
|
||||
```
|
||||
|
||||
- [ ] 重跑碰撞脚本，预期所有亚栅格、擦边和扫掠案例通过。
|
||||
|
||||
### Task 8：实现解析恒曲率原语和内部终点截断
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `CoarsePath/Search/MotionPrimitive.cs`
|
||||
- Create: `CoarsePath/Search/MotionPrimitiveGenerator.cs`
|
||||
- Create: `CoarsePath/Search/GoalToleranceChecker.cs`
|
||||
- Create/Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1`
|
||||
|
||||
**解析积分：**
|
||||
|
||||
```csharp
|
||||
double signedDistance = direction == TravelDirection.Forward ? step : -step;
|
||||
double nextHeading = AngleMath.NormalizeRadians(
|
||||
heading + curvature * signedDistance);
|
||||
|
||||
if (Math.Abs(curvature) < 1e-12)
|
||||
{
|
||||
nextX = x + signedDistance * Math.Cos(heading);
|
||||
nextY = y + signedDistance * Math.Sin(heading);
|
||||
}
|
||||
else
|
||||
{
|
||||
nextX = x + (Math.Sin(nextHeading) - Math.Sin(heading)) / curvature;
|
||||
nextY = y - (Math.Cos(nextHeading) - Math.Cos(heading)) / curvature;
|
||||
}
|
||||
```
|
||||
|
||||
- [ ] 写原语测试：直行、圆弧、倒车、五级曲率、相邻曲率最多变化一级、最大长度 0.50 m、实际采样步长不超过 `min(IntegrationStep,CollisionStep,MapResolution/2)`。
|
||||
- [ ] 写用户提出的案例：目标距起点 0.30 m、原语最大 0.50 m、收紧容差；断言第一个满足目标的内部点截断，后续点不生成，来源为 `GoalTruncation`。
|
||||
- [ ] 每个内部点严格按“有限值 → 扫掠碰撞 → 目标条件”检查；碰撞必须先于目标。
|
||||
- [ ] 起点已满足目标时创建零长度候选，不生成原语。
|
||||
- [ ] 重跑搜索脚本，预期几何、碰撞采样和 0.30 m 截断案例通过。
|
||||
|
||||
### Task 9：实现 Open List、统一代价和二维启发
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `CoarsePath/Search/BinaryMinHeap.cs`
|
||||
- Create: `CoarsePath/Search/SearchCostCalculator.cs`
|
||||
- Create: `CoarsePath/Search/GridDijkstraHeuristic.cs`
|
||||
- Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1`
|
||||
|
||||
**固定代价：**
|
||||
|
||||
```text
|
||||
primitiveCost =
|
||||
lengthMeters
|
||||
× directionMultiplier
|
||||
× (1
|
||||
+ CurvatureMagnitudeWeight × abs(curvature / maximumCurvature)
|
||||
+ ClearanceCostWeight × max(0, 1 - clearance / ClearanceCostDistanceMeters))
|
||||
+ gearSwitchPenalty
|
||||
+ CurvatureChangePenaltyMetersPerLevel × abs(curvatureLevelDelta)
|
||||
```
|
||||
|
||||
- [ ] 写堆顺序测试：较小 `F`、较小 `H`、较大 `G`、较小插入序号；相同输入重复运行顺序一致。
|
||||
- [ ] 写代价测试：前进、倒车、换向、曲率幅值、曲率变化和净空项；拒绝负数及非有限权重。
|
||||
- [ ] 写 Dijkstra 测试：八邻域直/斜代价；两个正交邻格任一占据时禁止对角切角；二维不可达返回明确状态。
|
||||
- [ ] 实现专用二叉最小堆，不依赖 `PriorityQueue`；允许旧条目由搜索层惰性丢弃。
|
||||
- [ ] `HeuristicWeight=1` 使用 `F=G+H`；大于 1 时只承诺可行性。
|
||||
- [ ] 重跑搜索脚本，预期顺序、公式和切角限制通过。
|
||||
|
||||
### Task 10：实现 Hybrid A* 节点、重开和终点候选管理
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `CoarsePath/Search/HybridAStarNode.cs`
|
||||
- Create: `CoarsePath/Search/HybridAStarNodeKey.cs`
|
||||
- Create: `CoarsePath/Search/HybridAStarSearch.cs`
|
||||
- Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1`
|
||||
|
||||
- [ ] 写搜索测试：空图前进、单矩形绕行、允许倒车的狭窄场景、起始曲率、目标进入方向、`±π` 容差、无解、取消、超时、节点上限、重开和确定性。
|
||||
- [ ] 写候选排序测试：先生成较大 `F` 的终点候选时不得结束；较小 `F` 普通节点先出队；候选成为最佳有效条目后才成功。
|
||||
- [ ] 离散键固定为位置格、航向格、方向和曲率等级；连续位姿保留在节点中。
|
||||
- [ ] `Dictionary<HybridAStarNodeKey,double>` 保存普通状态最佳 `G`；更小 `G` 允许重开；旧普通堆条目惰性丢弃。
|
||||
- [ ] 终点候选放入同一堆，但不能仅因另一个连续位姿落入相同离散键且 `G` 更低而被删除；候选出队时重新验证目标和末段碰撞。
|
||||
- [ ] 搜索循环在每次扩展前按顺序检查取消、5 s 超时、200,000 节点上限，再弹出有效条目；Open List 为空返回 `NoFeasiblePath`。
|
||||
- [ ] 重跑搜索脚本，预期候选顺序、重开、限额和所有场景通过。
|
||||
|
||||
### Task 11：回溯、装配、最终复核和 HybridAStarPlanner
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `CoarsePath/Output/PathBacktracker.cs`
|
||||
- Create: `CoarsePath/Output/CoarsePathAssembler.cs`
|
||||
- Create: `CoarsePath/Output/CoarsePathValidator.cs`
|
||||
- Create: `CoarsePath/HybridAStarPlanner.cs`
|
||||
- Create: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
||||
|
||||
```csharp
|
||||
public sealed class HybridAStarPlanner
|
||||
{
|
||||
public PlanningResult Plan(
|
||||
PlanningRequest request,
|
||||
CancellationToken cancellationToken = default(CancellationToken));
|
||||
}
|
||||
```
|
||||
|
||||
- [ ] 写失败状态测试：请求、地图就绪、车辆参数、曲率配置、起终点越界/碰撞，以及搜索失败到结果的无异常映射。
|
||||
- [ ] 写输出测试：首点弧长零；弧长不递减；`UnwrappedHeading` 连续；终点截断来源正确；除换向对外无相邻重复点。
|
||||
- [ ] 写换向分段测试：换向处保留两个坐标/航向/弧长相同而方向不同的点；新方向点标记 `IsGearSwitchPoint`；包含式分段完整覆盖路径。
|
||||
- [ ] 搜索节点只存父索引、方向、曲率和实际原语长度；成功后使用相同解析积分和有效步长重建内部点。
|
||||
- [ ] 最终复核有限数值、曲率、扫掠碰撞、目标容差/方向、弧长、换向对和分段；失败返回 `FinalValidationFailed` 且不发布部分路径。
|
||||
- [ ] 重跑碰撞、搜索和集成脚本，预期全部通过。
|
||||
|
||||
### Task 12：实现一次调用 CoarsePathPlanningService
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `CoarsePath/Facade/` 下目录树列出的 5 个文件
|
||||
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
||||
|
||||
```csharp
|
||||
public sealed class CoarsePathPlanningService
|
||||
{
|
||||
public CoarsePathPlanningJobResult Plan(
|
||||
CoarsePathPlanningJob job,
|
||||
CancellationToken cancellationToken = default(CancellationToken));
|
||||
}
|
||||
```
|
||||
|
||||
- [ ] 写一次调用测试：固定执行 `PlanningMapFactory.Create → HybridAStarPlanner.Plan → debug sink`；地图失败不启动搜索；结果同时保留地图和规划结果。
|
||||
- [ ] 写旁路隔离测试：可视化开关不改变地图指纹、占据哈希、规划状态或路径；debug sink 异常只写调试诊断。
|
||||
- [ ] 服务持有同一 `PlanningMapFactory`，多次调用共享容量 4 缓存；默认 debug sink 为空行为。
|
||||
- [ ] 重跑以下 P0 验收，预期构建成功且 6 个脚本退出码为 0。
|
||||
|
||||
```powershell
|
||||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_adapter.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_collision.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
||||
```
|
||||
|
||||
## P0-MAP 收尾与 P1：集成、迁移和性能
|
||||
|
||||
### Task 13（P0-MAP）：拆分迁移 TrapMapImageExporter
|
||||
|
||||
**现有来源：**
|
||||
|
||||
- Read/Migrate: `ClumsyPilot/ParkrobTrajplanner/Occupancygird_Map/Map_test/TrapMapImageExporter.cs`
|
||||
|
||||
**新文件：**
|
||||
|
||||
- Create: `Map/Test/Visualization/PlanningMapImageExportRequest.cs`
|
||||
- Create: `Map/Test/Visualization/PlanningMapImageExportResult.cs`
|
||||
- Create: `Map/Test/Visualization/PlanningMapImageExporter.cs`
|
||||
- Create: `Map/Test/Visualization/PlanningMapImageRenderer.cs`
|
||||
- Create: `Map/Test/Visualization/ValidatedPngWriter.cs`
|
||||
- Create: `ClumsyPilot/tests/verify_planning_map_image.ps1`
|
||||
|
||||
- [ ] 写新图片测试：输入只允许 `PlanningGridMap`；覆盖关闭导出、非法尺寸、唯一命名、临时文件清理、PNG 签名/IHDR/IEND 和每个 chunk CRC。
|
||||
- [ ] 保留现有有效限制：`PixelsPerCell=4`、`MaximumImageEdgePixels=4000`、`MaximumFileSizeBytes=50 MiB`、`OutputDpi=300`、最多 1024 次重名重试、StbImageWriteSharp 1.16.7。
|
||||
- [ ] 按职责迁移：Request/Result 只放 DTO；Renderer 生成 RGBA；Writer 负责编码/CRC；Exporter 校验、独占临时文件和原子发布。
|
||||
- [ ] 新导出器不得依赖 `GridMapData`、`TrapMapVehiclePose`、TwoLeg 状态、传感器或地图构造器；车辆、起终点和路径只作可选叠加层。
|
||||
- [ ] 运行图片脚本通过后先保留旧文件，Task 16 确认等价覆盖后再退役。
|
||||
|
||||
### Task 14：增加地图与粗路径 MovementTest
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `Map/Test/MovementTest.MapTest.cs`
|
||||
- Create: `CoarsePath/Test/CoarsePathScenarioFactory.cs`
|
||||
- Create: `CoarsePath/Test/MovementTest.CoarsePathTest.cs`
|
||||
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
||||
|
||||
- [ ] **P0-MAP 部分：** 先创建 `MovementTest.MapTest.cs`。它只调用长期持有的 `PlanningMapFactory`，显示来源状态、栅格、快照 ID 和缓存命中，并可选调用新 PNG 导出器；通过 Map Gate 后即可结束当前首要里程碑。
|
||||
- [ ] **P1 部分：** Map Gate 和 P0-PLAN 均通过后，再创建 `CoarsePathScenarioFactory.cs` 与 `MovementTest.CoarsePathTest.cs`。
|
||||
- [ ] 场景工厂提供显式空图、单矩形绕行、人工圆+矩形+TwoLeg、相同地图缓存命中、倒车换向和无解案例。
|
||||
- [ ] CoarsePath 案例使用同一 `CoarsePathPlanningService`；测试类不得直接实例化 rasterizer、碰撞器、原语生成器或搜索节点。
|
||||
- [ ] `MovementTest.CoarsePathTest` 在后台任务调用同步 `Plan`，绘制起点、目标、路径、换向点和扩大车体检查点。
|
||||
- [ ] `TestStop` 先取消专用 `CancellationTokenSource`，再清理任务和 Painter；两个入口不得发送底盘运动命令。
|
||||
- [ ] Clumsy UI 手动运行时不阻塞界面，停止后无后台规划残留，调试开关不改变结果。
|
||||
|
||||
### Task 15（P1）：性能、资源和确定性验收
|
||||
|
||||
**文件：**
|
||||
|
||||
- Create: `ClumsyPilot/tests/benchmark_coarse_path.ps1`
|
||||
- Modify: `Map/Planning/PlanningMapCache.cs`
|
||||
- Modify: `Map/Planning/EuclideanDistanceTransform.cs`
|
||||
- Modify: `CoarsePath/Search/BinaryMinHeap.cs`
|
||||
- Modify: `CoarsePath/Search/HybridAStarSearch.cs`
|
||||
- Modify: `CoarsePath/Contracts/PlanningDiagnostics.cs`
|
||||
|
||||
- [ ] 基准脚本输出地图规模、缓存命中、状态、耗时、扩展/生成/重开节点、陈旧堆条目、Open List 峰值和托管内存增量，超限返回非零。
|
||||
- [ ] 地图参考：20 m×20 m、0.05 m、160,000 格、100 障碍；20 次后完整构建 P95≤200 ms，完整缓存命中 P95≤5 ms。
|
||||
- [ ] 地图极限：4,000,000 格、100 障碍；3 s 内成功或明确失败；成功时内存增量≤160 MB，无溢出和部分快照。
|
||||
- [ ] 规划参考：12 m×8 m、0.05 m、矩形阻断直线、距离≥8 m；20 次 P95≤2 s，内存增量≤256 MB。
|
||||
- [ ] 规划压力：20 m×20 m、0.05 m；成功或无解均在 5 s、200,000 节点和 512 MB 增量内返回。
|
||||
- [ ] 优化只针对查询分配、重复 EDT、堆扩容和稠密点保存；不得降低碰撞保守性、跳过最终复核或放宽失败状态。
|
||||
- [ ] 运行：
|
||||
|
||||
```powershell
|
||||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj -c Release --no-restore
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\benchmark_coarse_path.ps1 -Configuration Release
|
||||
```
|
||||
|
||||
预期：全部指标达标，脚本退出码为 0。
|
||||
|
||||
### Task 16（P1）：退役旧地图入口并完成文档
|
||||
|
||||
**文件：**
|
||||
|
||||
- Modify/Delete after equivalent coverage: `ClumsyPilot/ParkrobTrajplanner/Occupancygird_Map/Map_test/MovementTest.Trapmaptest.cs`
|
||||
- Delete after equivalent coverage: `ClumsyPilot/ParkrobTrajplanner/Occupancygird_Map/Map_test/TrapMapImageExporter.cs`
|
||||
- Update/Delete after equivalent coverage: `ClumsyPilot/tests/verify_trapmap_grid.ps1`
|
||||
- Update/Delete after equivalent coverage: `ClumsyPilot/tests/verify_trapmap_inputs.ps1`
|
||||
- Update/Delete after equivalent coverage: `ClumsyPilot/tests/verify_trapmap_lifecycle.ps1`
|
||||
- Update/Delete after equivalent coverage: `ClumsyPilot/tests/verify_trapmap_image.ps1`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md`
|
||||
|
||||
- [ ] 建立覆盖表：`TrapMapBounds→MapBoundsMm`、`GridMapData→EnvironmentGridMap/PlanningGridMap`、`TrapMapLayerComposer→EnvironmentMapBuilder`、`TrapMapBuilder.Get→上层采集+CoarsePathPlanningService`、旧 exporter→五个 Visualization 文件。
|
||||
- [ ] 等价脚本全部通过后再删除旧实现；不保留车辆写图、默认 300 mm 膨胀或地图构建器直接调用 `TwoLegDetect` 的兼容开关。
|
||||
- [ ] README 写明一次调用、下层门面、mm/m-rad 边界、快照变化判断、调试开关不参与指纹、P0/P1 命令和非目标。
|
||||
- [ ] 执行旧引用搜索：
|
||||
|
||||
```powershell
|
||||
rg -n "GridMapData|TrapMapBuilder|TrapMapLayerComposer|TrapMapImageExporter|MarkVehicleFootprint" .\ClumsyPilot
|
||||
```
|
||||
|
||||
预期：仅迁移说明或历史文档可命中；运行时代码和新测试不得命中旧类型。
|
||||
|
||||
- [ ] 运行最终回归：
|
||||
|
||||
```powershell
|
||||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_adapter.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_image.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_collision.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
||||
```
|
||||
|
||||
预期：Debug 构建成功，7 个功能脚本退出码均为 0；随后重跑 Task 15 Release 基准并通过。
|
||||
|
||||
## 最终验收清单
|
||||
|
||||
- [ ] `CoarsePathPlanningService.Plan(job)` 是推荐的一次调用入口。
|
||||
- [ ] `PlanningMapFactory` 和 `HybridAStarPlanner` 仅作为可独立测试的下层门面。
|
||||
- [ ] 人工、TwoLeg 和未来障碍通过同一个 `IMapObstacleSource` 进入唯一栅格化器。
|
||||
- [ ] 地图未变化时复用完整快照或占据/距离缓冲；可视化、起终点和车辆参数不参与地图变化判断。
|
||||
- [ ] 地图不包含车辆自身和安全膨胀，碰撞检查使用扩大车辆矩形。
|
||||
- [ ] 距离场是净空下界，不能因高估而跳过精确碰撞。
|
||||
- [ ] 0.50 m 原语可在任意内部采样点截断；终点候选按 Open List 顺序出队后才终止。
|
||||
- [ ] best-G、重开、陈旧条目、候选保护和堆排序均有自动化测试。
|
||||
- [ ] 输出包含稠密点、保守净空、实际曲率、换向点和完整方向分段。
|
||||
- [ ] PNG 和 MovementTest 只消费只读快照，不成为核心规划依赖。
|
||||
- [ ] 失败、取消、超时和限额均返回空路径及明确状态，不发布部分结果。
|
||||
- [ ] Debug 与 Release 验收全部通过，且未执行 Git 自检或提交。
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,92 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 建图阶段使用的可变环境占据栅格。
|
||||
///
|
||||
/// 单位：边界、世界查询和栅格边长均为 mm。
|
||||
/// 注意：只有 <see cref="MapObstacleRasterizer"/> 可以写入占据状态；规划阶段应改用不可变的 <see cref="PlanningGridMap"/>。
|
||||
/// </summary>
|
||||
public sealed class EnvironmentGridMap
|
||||
{
|
||||
private readonly byte[] _cells;
|
||||
private int _occupiedCount;
|
||||
|
||||
/// <summary>
|
||||
/// 创建空的环境占据栅格。
|
||||
///
|
||||
/// 参数：bounds 为左闭右开的世界边界，单位 mm；resolutionMm 为格边长，单位 mm。
|
||||
/// 返回：无；边界为空或分辨率不合法时抛出异常。
|
||||
/// </summary>
|
||||
public EnvironmentGridMap(MapBoundsMm bounds, float resolutionMm)
|
||||
{
|
||||
if (bounds == null) throw new ArgumentNullException(nameof(bounds));
|
||||
bounds.GetDimensions(resolutionMm, out int rows, out int cols);
|
||||
Bounds = bounds; ResolutionMm = resolutionMm; Rows = rows; Cols = cols;
|
||||
_cells = new byte[checked(rows * cols)];
|
||||
}
|
||||
|
||||
/// <summary>地图世界边界，单位 mm，采用左闭右开规则。</summary>
|
||||
public MapBoundsMm Bounds { get; }
|
||||
/// <summary>单个栅格边长，单位 mm。</summary>
|
||||
public float ResolutionMm { get; }
|
||||
/// <summary>栅格行数，Y 方向从下限向上递增。</summary>
|
||||
public int Rows { get; }
|
||||
/// <summary>栅格列数，X 方向从下限向右递增。</summary>
|
||||
public int Cols { get; }
|
||||
/// <summary>当前已被标记为障碍的格数。</summary>
|
||||
public int OccupiedCount { get { return _occupiedCount; } }
|
||||
|
||||
/// <summary>判断行列索引是否有效。参数 row、col 分别为从零开始的行和列；有效时返回 true。</summary>
|
||||
public bool IsInBounds(int row, int col) { return row >= 0 && row < Rows && col >= 0 && col < Cols; }
|
||||
/// <summary>判断世界坐标是否位于地图内。参数 xMm、yMm 单位为 mm；上边界与右边界返回 false。</summary>
|
||||
public bool IsWorldInBounds(float xMm, float yMm) { return Bounds.Contains(xMm, yMm); }
|
||||
|
||||
/// <summary>
|
||||
/// 将世界坐标转换为栅格索引。
|
||||
///
|
||||
/// 参数：xMm、yMm 为世界坐标，单位 mm；row、col 为输出索引。
|
||||
/// 返回：坐标在地图内时为 true 并写入索引；否则返回 false，两个输出均为 -1。
|
||||
/// </summary>
|
||||
public bool TryWorldToGrid(float xMm, float yMm, out int row, out int col)
|
||||
{
|
||||
row = -1; col = -1;
|
||||
if (!IsWorldInBounds(xMm, yMm)) return false;
|
||||
col = (int)Math.Floor(((double)xMm - Bounds.XMin) / ResolutionMm);
|
||||
row = (int)Math.Floor(((double)yMm - Bounds.YMin) / ResolutionMm);
|
||||
return IsInBounds(row, col);
|
||||
}
|
||||
|
||||
/// <summary>查询栅格是否占据。越界索引按障碍处理，返回 true。</summary>
|
||||
public bool IsOccupied(int row, int col) { return !IsInBounds(row, col) || _cells[row * Cols + col] != 0; }
|
||||
/// <summary>按世界坐标查询占据状态。参数 xMm、yMm 单位为 mm；坐标越界时保守地返回 true。</summary>
|
||||
public bool IsOccupiedWorld(float xMm, float yMm)
|
||||
{
|
||||
return !TryWorldToGrid(xMm, yMm, out int row, out int col) || IsOccupied(row, col);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取一个栅格的世界坐标范围。
|
||||
///
|
||||
/// 参数：row、col 为有效索引；xMin、xMax、yMin、yMax 为输出边界，单位 mm。
|
||||
/// 返回：无；索引越界时抛出 <see cref="ArgumentOutOfRangeException"/>。
|
||||
/// </summary>
|
||||
public void GetCellBounds(int row, int col, out float xMin, out float xMax, out float yMin, out float yMax)
|
||||
{
|
||||
if (!IsInBounds(row, col)) throw new ArgumentOutOfRangeException();
|
||||
xMin = Bounds.XMin + col * ResolutionMm;
|
||||
yMin = Bounds.YMin + row * ResolutionMm;
|
||||
xMax = Math.Min(Bounds.XMax, xMin + ResolutionMm);
|
||||
yMax = Math.Min(Bounds.YMax, yMin + ResolutionMm);
|
||||
}
|
||||
|
||||
internal void MarkOccupied(int row, int col)
|
||||
{
|
||||
if (!IsInBounds(row, col)) return;
|
||||
int index = row * Cols + col;
|
||||
if (_cells[index] == 0) { _cells[index] = 1; _occupiedCount++; }
|
||||
}
|
||||
internal byte[] CopyCells() { return (byte[])_cells.Clone(); }
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 环境占据图构建结果。
|
||||
/// 返回：成功时提供可供适配的 EnvironmentGridMap；失败时提供失败原因和已处理来源状态。
|
||||
/// </summary>
|
||||
public sealed class EnvironmentMapBuildResult
|
||||
{
|
||||
private EnvironmentMapBuildResult(bool succeeded, EnvironmentGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults, string failureReason, PlanningOperationStopReason stopReason)
|
||||
{
|
||||
Succeeded = succeeded; Map = map; SourceResults = sourceResults ?? Array.Empty<ObstacleProjectionResult>(); FailureReason = failureReason ?? string.Empty; StopReason = stopReason;
|
||||
}
|
||||
/// <summary>构建是否成功。true 时 Map 非空；false 时读取 FailureReason。</summary>
|
||||
public bool Succeeded { get; }
|
||||
/// <summary>成功生成的构建期环境栅格；失败时为 null。</summary>
|
||||
public EnvironmentGridMap Map { get; }
|
||||
/// <summary>已尝试来源的投影结果，用于记录已应用、空或失败状态。</summary>
|
||||
public IReadOnlyList<ObstacleProjectionResult> SourceResults { get; }
|
||||
/// <summary>失败原因。成功时为空字符串。</summary>
|
||||
public string FailureReason { get; }
|
||||
/// <summary>内部预算停止原因；普通构建成功或失败时为 None。</summary>
|
||||
internal PlanningOperationStopReason StopReason { get; }
|
||||
/// <summary>创建成功结果。参数 map 为已完成栅格，sourceResults 为来源投影记录。</summary>
|
||||
public static EnvironmentMapBuildResult Success(EnvironmentGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults) { return new EnvironmentMapBuildResult(true, map, sourceResults, null, PlanningOperationStopReason.None); }
|
||||
/// <summary>创建失败结果。参数 reason 为诊断文本，sourceResults 可包含失败前已处理的来源。</summary>
|
||||
public static EnvironmentMapBuildResult Failure(string reason, IReadOnlyList<ObstacleProjectionResult> sourceResults) { return new EnvironmentMapBuildResult(false, null, sourceResults, reason, PlanningOperationStopReason.None); }
|
||||
/// <summary>创建已取消或超时结果；不发布构建期可写地图。</summary>
|
||||
internal static EnvironmentMapBuildResult Stopped(PlanningOperationStopReason stopReason, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.None) throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
return new EnvironmentMapBuildResult(false, null, sourceResults,
|
||||
stopReason == PlanningOperationStopReason.Cancelled ? "地图构建已取消。" : "地图构建已超时。", stopReason);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 从排序后的纯障碍物快照事务性构建环境占据图。
|
||||
///
|
||||
/// 注意：必需来源返回不可用或无效状态时，构建整体失败；可选来源仅记录其状态并继续构建。
|
||||
/// </summary>
|
||||
public sealed class EnvironmentMapBuilder
|
||||
{
|
||||
/// <summary>
|
||||
/// 投影所有障碍物来源并栅格化为环境地图。
|
||||
///
|
||||
/// 参数：request 包含 mm 世界边界、分辨率和来源列表；每个来源 ID 必须唯一且版本非负。
|
||||
/// 返回：成功时包含 <see cref="EnvironmentGridMap"/> 和全部来源状态；必需来源失败时返回失败结果而不产生可用地图。
|
||||
/// </summary>
|
||||
public EnvironmentMapBuildResult Build(MapBuildRequest request)
|
||||
{
|
||||
return Build(request, PlanningOperationBudget.Unlimited(CancellationToken.None));
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算投影来源并栅格化；停止时不发布可写环境地图。</summary>
|
||||
internal EnvironmentMapBuildResult Build(MapBuildRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, null);
|
||||
if (request == null || request.Bounds == null) return EnvironmentMapBuildResult.Failure("Map request and bounds are required.", null);
|
||||
if (request.ObstacleSources == null) return EnvironmentMapBuildResult.Failure("Obstacle source collection is required.", null);
|
||||
var sources = request.ObstacleSources.OrderBy(s => s == null ? string.Empty : s.SourceId, StringComparer.Ordinal).ToArray();
|
||||
var results = new List<ObstacleProjectionResult>();
|
||||
string previousId = null;
|
||||
for (int i = 0; i < sources.Length; i++)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, results);
|
||||
IMapObstacleSource source = sources[i];
|
||||
if (source == null || string.IsNullOrWhiteSpace(source.SourceId) || source.SourceVersion < 0)
|
||||
return EnvironmentMapBuildResult.Failure("Each source needs a non-empty id and non-negative version.", results);
|
||||
if (string.Equals(previousId, source.SourceId, StringComparison.Ordinal))
|
||||
return EnvironmentMapBuildResult.Failure("Obstacle source ids must be unique.", results);
|
||||
previousId = source.SourceId;
|
||||
ObstacleProjectionResult result;
|
||||
try { result = source.ProjectToWorld() ?? ObstacleProjectionResult.Invalid("Source returned no projection result."); }
|
||||
catch (Exception exception) { result = ObstacleProjectionResult.Invalid(exception.Message); }
|
||||
results.Add(result);
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, results);
|
||||
if (source.IsRequired && (result.Status == ObstacleSourceStatus.Invalid || result.Status == ObstacleSourceStatus.Unavailable))
|
||||
return EnvironmentMapBuildResult.Failure("A required obstacle source failed: " + source.SourceId, results);
|
||||
}
|
||||
var map = new EnvironmentGridMap(request.Bounds, request.ResolutionMm);
|
||||
for (int i = 0; i < results.Count; i++)
|
||||
if (results[i].Status == ObstacleSourceStatus.Applied)
|
||||
for (int j = 0; j < results[i].Obstacles.Count; j++)
|
||||
{
|
||||
if (!MapObstacleRasterizer.TryRasterize(map, results[i].Obstacles[j], budget, out stopReason))
|
||||
return EnvironmentMapBuildResult.Stopped(stopReason, results);
|
||||
}
|
||||
return EnvironmentMapBuildResult.Success(map, results);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 有限的世界地图边界。
|
||||
/// 单位：mm；范围采用左闭右开 [XMin, XMax) × [YMin, YMax)。
|
||||
/// </summary>
|
||||
public sealed class MapBoundsMm : IEquatable<MapBoundsMm>
|
||||
{
|
||||
/// <summary>单张地图允许的最大栅格数，超过该值会拒绝创建地图。</summary>
|
||||
public const int MaximumCellCount = 4000000;
|
||||
|
||||
/// <summary>
|
||||
/// 创建地图世界边界。
|
||||
///
|
||||
/// 参数：
|
||||
/// - xMin、xMax：世界 X 轴下限和上限，单位 mm，且 xMax 必须大于 xMin。
|
||||
/// - yMin、yMax：世界 Y 轴下限和上限，单位 mm，且 yMax 必须大于 yMin。
|
||||
///
|
||||
/// 注意：边界采用左闭右开规则，上限坐标不属于地图。
|
||||
/// </summary>
|
||||
public MapBoundsMm(float xMin, float xMax, float yMin, float yMax)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(xMin) || !NumericGuard.IsFinite(xMax) ||
|
||||
!NumericGuard.IsFinite(yMin) || !NumericGuard.IsFinite(yMax) ||
|
||||
xMax <= xMin || yMax <= yMin)
|
||||
throw new ArgumentOutOfRangeException(nameof(xMax), "Map bounds must be finite and non-degenerate.");
|
||||
XMin = xMin; XMax = xMax; YMin = yMin; YMax = yMax;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 轴下限，单位 mm，包含在地图内。</summary>
|
||||
public float XMin { get; }
|
||||
/// <summary>世界 X 轴上限，单位 mm，不包含在地图内。</summary>
|
||||
public float XMax { get; }
|
||||
/// <summary>世界 Y 轴下限，单位 mm，包含在地图内。</summary>
|
||||
public float YMin { get; }
|
||||
/// <summary>世界 Y 轴上限，单位 mm，不包含在地图内。</summary>
|
||||
public float YMax { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 判断世界坐标是否属于地图边界。
|
||||
///
|
||||
/// 参数：xMm、yMm 为世界坐标，单位 mm。
|
||||
/// 返回：坐标位于 [XMin, XMax) × [YMin, YMax) 时为 true，否则为 false。
|
||||
/// </summary>
|
||||
public bool Contains(float xMm, float yMm)
|
||||
{
|
||||
return xMm >= XMin && xMm < XMax && yMm >= YMin && yMm < YMax;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据栅格分辨率计算行列数。
|
||||
///
|
||||
/// 参数：resolutionMm 为每个方格的边长，单位 mm，取值必须在 [20, 200]；rows、cols 为输出行数和列数。
|
||||
/// 返回：无；当分辨率无效或总格数超过 <see cref="MaximumCellCount"/> 时抛出异常。
|
||||
/// </summary>
|
||||
public void GetDimensions(float resolutionMm, out int rows, out int cols)
|
||||
{
|
||||
if (!NumericGuard.IsInRange(resolutionMm, 20f, 200f))
|
||||
throw new ArgumentOutOfRangeException(nameof(resolutionMm), "ResolutionMm must be within [20, 200].");
|
||||
double columnCount = Math.Ceiling(((double)XMax - XMin) / resolutionMm);
|
||||
double rowCount = Math.Ceiling(((double)YMax - YMin) / resolutionMm);
|
||||
if (columnCount > int.MaxValue || rowCount > int.MaxValue || columnCount <= 0d || rowCount <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(resolutionMm), "Map dimensions are invalid.");
|
||||
cols = (int)columnCount; rows = (int)rowCount;
|
||||
long cellCount = checked((long)rows * cols);
|
||||
if (cellCount > MaximumCellCount)
|
||||
throw new ArgumentOutOfRangeException(nameof(resolutionMm), "Map cell count exceeds 4,000,000.");
|
||||
}
|
||||
|
||||
/// <summary>比较两个边界的四个 mm 坐标是否完全相同。</summary>
|
||||
public bool Equals(MapBoundsMm other)
|
||||
{
|
||||
return other != null && XMin.Equals(other.XMin) && XMax.Equals(other.XMax) &&
|
||||
YMin.Equals(other.YMin) && YMax.Equals(other.YMax);
|
||||
}
|
||||
/// <summary>比较当前边界与指定对象是否表示相同的世界范围。</summary>
|
||||
public override bool Equals(object obj) { return Equals(obj as MapBoundsMm); }
|
||||
/// <summary>返回由四个边界坐标组成的哈希值，用于缓存键比较。</summary>
|
||||
public override int GetHashCode()
|
||||
{
|
||||
unchecked { int hash = XMin.GetHashCode(); hash = hash * 31 + XMax.GetHashCode(); hash = hash * 31 + YMin.GetHashCode(); return hash * 31 + YMax.GetHashCode(); }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 环境占据图构建器的输入数据。
|
||||
/// 注意：通常由 PlanningMapFactory 从公开请求转换得到，调用者无需直接使用。
|
||||
/// </summary>
|
||||
public sealed class MapBuildRequest
|
||||
{
|
||||
/// <summary>环境图世界边界。单位：mm；不能为空。</summary>
|
||||
public MapBoundsMm Bounds { get; set; }
|
||||
/// <summary>环境栅格边长。单位：mm；必须满足 MapBoundsMm 的分辨率限制。</summary>
|
||||
public float ResolutionMm { get; set; }
|
||||
/// <summary>待投影的障碍物来源列表；每个来源 ID 必须唯一。</summary>
|
||||
public IReadOnlyList<IMapObstacleSource> ObstacleSources { get; set; } = Array.Empty<IMapObstacleSource>();
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>边与世界 X/Y 轴平行的矩形障碍物，坐标单位为 mm。</summary>
|
||||
public sealed class AxisAlignedRectangleObstacle : IMapObstacle
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建轴对齐矩形障碍物。
|
||||
///
|
||||
/// 参数：xMin、xMax、yMin、yMax 分别为矩形世界坐标边界，单位 mm。
|
||||
/// 注意：构造不校验边界顺序，请通过 <see cref="IsValid"/> 判断后再使用。
|
||||
/// </summary>
|
||||
public AxisAlignedRectangleObstacle(float xMin, float xMax, float yMin, float yMax)
|
||||
{
|
||||
XMin = xMin; XMax = xMax; YMin = yMin; YMax = yMax;
|
||||
}
|
||||
/// <summary>矩形世界 X 下边界，单位 mm。</summary>
|
||||
public float XMin { get; }
|
||||
/// <summary>矩形世界 X 上边界，单位 mm。</summary>
|
||||
public float XMax { get; }
|
||||
/// <summary>矩形世界 Y 下边界，单位 mm。</summary>
|
||||
public float YMin { get; }
|
||||
/// <summary>矩形世界 Y 上边界，单位 mm。</summary>
|
||||
public float YMax { get; }
|
||||
/// <summary>四个边界均为有限数且 XMax≥XMin、YMax≥YMin 时为 true；否则为 false。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return NumericGuard.IsFinite(XMin) && NumericGuard.IsFinite(XMax) && NumericGuard.IsFinite(YMin) && NumericGuard.IsFinite(YMax) && XMax >= XMin && YMax >= YMin; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>以世界 mm 坐标表示的圆形障碍物。</summary>
|
||||
public sealed class CircleObstacle : IMapObstacle
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建圆形障碍物。
|
||||
///
|
||||
/// 参数：centerX、centerY 为圆心世界坐标，单位 mm；radiusMm 为半径，单位 mm。
|
||||
/// 注意：构造不抛出几何校验异常，请通过 <see cref="IsValid"/> 判断后再使用。
|
||||
/// </summary>
|
||||
public CircleObstacle(float centerX, float centerY, float radiusMm)
|
||||
{
|
||||
CenterX = centerX; CenterY = centerY; RadiusMm = radiusMm;
|
||||
}
|
||||
/// <summary>圆心世界 X 坐标，单位 mm。</summary>
|
||||
public float CenterX { get; }
|
||||
/// <summary>圆心世界 Y 坐标，单位 mm。</summary>
|
||||
public float CenterY { get; }
|
||||
/// <summary>圆的半径，单位 mm。</summary>
|
||||
public float RadiusMm { get; }
|
||||
/// <summary>圆心和半径均为有限数且半径不小于零时为 true；否则为 false。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return NumericGuard.IsFinite(CenterX) && NumericGuard.IsFinite(CenterY) && NumericGuard.IsFinite(RadiusMm) && RadiusMm >= 0f; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 世界坐标中的不可变障碍物几何。
|
||||
///
|
||||
/// 单位：所有几何坐标与尺寸均为 mm。实现类型必须能由 <see cref="MapObstacleRasterizer"/> 栅格化。
|
||||
/// </summary>
|
||||
public interface IMapObstacle
|
||||
{
|
||||
/// <summary>几何数据是否有效。true 表示数值有限且尺寸满足该几何类型的约束；false 表示不得投影到地图。</summary>
|
||||
bool IsValid { get; }
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 将障碍物几何保守投影为栅格占据状态的唯一写入入口。
|
||||
///
|
||||
/// 注意：调用方不能直接改写 <see cref="EnvironmentGridMap"/>；相交或贴边的栅格均按占据处理。
|
||||
/// </summary>
|
||||
public static class MapObstacleRasterizer
|
||||
{
|
||||
/// <summary>
|
||||
/// 将一个有效障碍物栅格化到环境地图。
|
||||
///
|
||||
/// 参数：map 为待写入的环境栅格；obstacle 为世界 mm 坐标的圆形或轴对齐矩形障碍物。
|
||||
/// 返回：无。地图或障碍物为空、障碍物无效、几何类型不受支持时抛出异常。
|
||||
/// 注意：该方法只增加占据格，不会清除已有障碍。
|
||||
/// </summary>
|
||||
public static void Rasterize(EnvironmentGridMap map, IMapObstacle obstacle)
|
||||
{
|
||||
if (!TryRasterize(map, obstacle, PlanningOperationBudget.Unlimited(CancellationToken.None), out _))
|
||||
throw new InvalidOperationException("Unbounded rasterization unexpectedly stopped.");
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算将障碍物写入环境栅格；停止时返回 false 且不发布环境地图。</summary>
|
||||
internal static bool TryRasterize(EnvironmentGridMap map, IMapObstacle obstacle, PlanningOperationBudget budget,
|
||||
out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
if (obstacle == null || !obstacle.IsValid) throw new ArgumentException("Obstacle must be valid.", nameof(obstacle));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
int workItemCount = 0;
|
||||
var circle = obstacle as CircleObstacle;
|
||||
if (circle != null) return TryRasterizeCircle(map, circle, budget, ref workItemCount, out stopReason);
|
||||
var rectangle = obstacle as AxisAlignedRectangleObstacle;
|
||||
if (rectangle != null) return TryRasterizeRectangle(map, rectangle, budget, ref workItemCount, out stopReason);
|
||||
throw new NotSupportedException("Unsupported map obstacle geometry.");
|
||||
}
|
||||
|
||||
private static bool TryRasterizeCircle(EnvironmentGridMap map, CircleObstacle circle, PlanningOperationBudget budget,
|
||||
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
GetCandidateRange(map, circle.CenterX - circle.RadiusMm, circle.CenterX + circle.RadiusMm,
|
||||
circle.CenterY - circle.RadiusMm, circle.CenterY + circle.RadiusMm,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
||||
double radiusSquared = (double)circle.RadiusMm * circle.RadiusMm;
|
||||
for (int row = firstRow; row <= lastRow; row++)
|
||||
for (int col = firstCol; col <= lastCol; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
|
||||
double nearestX = Math.Max(xMin, Math.Min(circle.CenterX, xMax));
|
||||
double nearestY = Math.Max(yMin, Math.Min(circle.CenterY, yMax));
|
||||
double dx = circle.CenterX - nearestX;
|
||||
double dy = circle.CenterY - nearestY;
|
||||
if (dx * dx + dy * dy <= radiusSquared) map.MarkOccupied(row, col);
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryRasterizeRectangle(EnvironmentGridMap map, AxisAlignedRectangleObstacle rectangle, PlanningOperationBudget budget,
|
||||
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
GetCandidateRange(map, rectangle.XMin, rectangle.XMax, rectangle.YMin, rectangle.YMax,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
||||
for (int row = firstRow; row <= lastRow; row++)
|
||||
for (int col = firstCol; col <= lastCol; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
|
||||
if (rectangle.XMax >= xMin && rectangle.XMin <= xMax && rectangle.YMax >= yMin && rectangle.YMin <= yMax)
|
||||
map.MarkOccupied(row, col);
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void GetCandidateRange(EnvironmentGridMap map, float xMin, float xMax, float yMin, float yMax,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol)
|
||||
{
|
||||
if (xMax < map.Bounds.XMin || xMin > map.Bounds.XMax || yMax < map.Bounds.YMin || yMin > map.Bounds.YMax)
|
||||
{ firstRow = 1; lastRow = 0; firstCol = 1; lastCol = 0; return; }
|
||||
// Geometry is closed for conservative rasterisation. Include the cell on
|
||||
// the lower side when a boundary lies exactly on a grid line.
|
||||
firstCol = Clamp((int)Math.Floor(((double)xMin - map.Bounds.XMin) / map.ResolutionMm) - 1, 0, map.Cols - 1);
|
||||
lastCol = Clamp((int)Math.Floor(((double)xMax - map.Bounds.XMin) / map.ResolutionMm), 0, map.Cols - 1);
|
||||
firstRow = Clamp((int)Math.Floor(((double)yMin - map.Bounds.YMin) / map.ResolutionMm) - 1, 0, map.Rows - 1);
|
||||
lastRow = Clamp((int)Math.Floor(((double)yMax - map.Bounds.YMin) / map.ResolutionMm), 0, map.Rows - 1);
|
||||
}
|
||||
private static int Clamp(int value, int min, int max) { return value < min ? min : value > max ? max : value; }
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>针对行主序二值栅格计算精确欧氏距离平方的内部算法。</summary>
|
||||
internal static class EuclideanDistanceTransform
|
||||
{
|
||||
/// <summary>
|
||||
/// 计算每个栅格到最近障碍栅格的距离平方。
|
||||
///
|
||||
/// 参数：occupied 为行主序占据数组，非零表示障碍；rows、cols 为数组尺寸。
|
||||
/// 返回：行主序距离平方数组，单位为栅格边长的平方；不含任何 mm 或 m 换算。
|
||||
/// </summary>
|
||||
public static double[] ComputeSquaredDistances(byte[] occupied, int rows, int cols)
|
||||
{
|
||||
if (!TryComputeSquaredDistances(occupied, rows, cols, PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
out double[] squared, out _))
|
||||
throw new InvalidOperationException("Unbounded Euclidean distance transform unexpectedly stopped.");
|
||||
return squared;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算计算距离平方；停止时不返回部分数组。</summary>
|
||||
internal static bool TryComputeSquaredDistances(byte[] occupied, int rows, int cols, PlanningOperationBudget budget,
|
||||
out double[] squaredDistances, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (occupied == null) throw new ArgumentNullException(nameof(occupied));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
squaredDistances = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
double noObstacleDistanceSquared = (double)rows * rows + (double)cols * cols + 1d;
|
||||
var intermediate = new double[occupied.Length];
|
||||
var result = new double[occupied.Length];
|
||||
var input = new double[Math.Max(rows, cols)];
|
||||
var output = new double[Math.Max(rows, cols)];
|
||||
int workItemCount = 0;
|
||||
for (int row = 0; row < rows; row++)
|
||||
{
|
||||
int offset = row * cols;
|
||||
for (int col = 0; col < cols; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
input[col] = occupied[offset + col] == 0 ? noObstacleDistanceSquared : 0d;
|
||||
}
|
||||
if (!TryTransform1D(input, cols, output, budget, ref workItemCount, out stopReason)) return false;
|
||||
for (int col = 0; col < cols; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
intermediate[offset + col] = output[col];
|
||||
}
|
||||
}
|
||||
for (int col = 0; col < cols; col++)
|
||||
{
|
||||
for (int row = 0; row < rows; row++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
input[row] = intermediate[row * cols + col];
|
||||
}
|
||||
if (!TryTransform1D(input, rows, output, budget, ref workItemCount, out stopReason)) return false;
|
||||
for (int row = 0; row < rows; row++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
result[row * cols + col] = output[row];
|
||||
}
|
||||
}
|
||||
squaredDistances = result;
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryTransform1D(double[] f, int length, double[] result, PlanningOperationBudget budget,
|
||||
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
var locations = new int[length];
|
||||
var boundaries = new double[length + 1];
|
||||
int k = 0;
|
||||
locations[0] = 0;
|
||||
boundaries[0] = double.NegativeInfinity;
|
||||
boundaries[1] = double.PositiveInfinity;
|
||||
for (int q = 1; q < length; q++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
double intersection;
|
||||
do
|
||||
{
|
||||
int p = locations[k];
|
||||
intersection = ((f[q] + (double)q * q) - (f[p] + (double)p * p)) / (2d * (q - p));
|
||||
if (intersection <= boundaries[k])
|
||||
{
|
||||
k--;
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
}
|
||||
} while (k >= 0 && intersection <= boundaries[k]);
|
||||
if (k < 0)
|
||||
{
|
||||
k = 0; locations[0] = q; boundaries[0] = double.NegativeInfinity; boundaries[1] = double.PositiveInfinity;
|
||||
}
|
||||
else
|
||||
{
|
||||
k++; locations[k] = q; boundaries[k] = intersection; boundaries[k + 1] = double.PositiveInfinity;
|
||||
}
|
||||
}
|
||||
k = 0;
|
||||
for (int q = 0; q < length; q++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
while (boundaries[k + 1] < q) k++;
|
||||
double delta = q - locations[k];
|
||||
result[q] = delta * delta + f[locations[k]];
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>以 m 表示的障碍物净距离保守下界。</summary>
|
||||
internal sealed class ObstacleDistanceField
|
||||
{
|
||||
private readonly double[] _conservativeDistances;
|
||||
private ObstacleDistanceField(double[] conservativeDistances) { _conservativeDistances = conservativeDistances; }
|
||||
/// <summary>
|
||||
/// 从占据栅格创建距离场。
|
||||
///
|
||||
/// 参数：occupied 为行主序占据数组；rows、cols 为其尺寸；resolutionMeters 为格边长，单位 m。
|
||||
/// 返回：每个格到最近障碍物的保守净距离下界，单位 m；全空地图中的每项为正无穷。
|
||||
/// </summary>
|
||||
public static ObstacleDistanceField Create(byte[] occupied, int rows, int cols, double resolutionMeters)
|
||||
{
|
||||
if (!TryCreate(occupied, rows, cols, resolutionMeters, PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
out ObstacleDistanceField field, out _))
|
||||
throw new InvalidOperationException("Unbounded distance-field creation unexpectedly stopped.");
|
||||
return field;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建距离场；停止时不返回部分距离数据。</summary>
|
||||
internal static bool TryCreate(byte[] occupied, int rows, int cols, double resolutionMeters,
|
||||
PlanningOperationBudget budget, out ObstacleDistanceField field, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (occupied == null) throw new ArgumentNullException(nameof(occupied));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
field = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
bool hasObstacle = false;
|
||||
int workItemCount = 0;
|
||||
for (int i = 0; i < occupied.Length; i++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
if (occupied[i] != 0) { hasObstacle = true; break; }
|
||||
}
|
||||
var distances = new double[occupied.Length];
|
||||
if (!hasObstacle)
|
||||
{
|
||||
for (int i = 0; i < distances.Length; i++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
distances[i] = double.PositiveInfinity;
|
||||
}
|
||||
field = new ObstacleDistanceField(distances);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
if (!EuclideanDistanceTransform.TryComputeSquaredDistances(occupied, rows, cols, budget, out double[] squared, out stopReason))
|
||||
return false;
|
||||
double conservativeOffset = Math.Sqrt(2d) * resolutionMeters;
|
||||
for (int i = 0; i < distances.Length; i++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
distances[i] = Math.Max(0d, Math.Sqrt(squared[i]) * resolutionMeters - conservativeOffset);
|
||||
}
|
||||
field = new ObstacleDistanceField(distances);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
internal double[] CopyDistances() { return (double[])_conservativeDistances.Clone(); }
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 供粗路径规划使用的不可变地图快照。
|
||||
///
|
||||
/// 单位：世界查询方法使用 m；<see cref="Bounds"/> 和 <see cref="ResolutionMm"/> 保留原始 mm 数据。
|
||||
/// 注意：世界坐标越界一律按占据处理，净距离为零。
|
||||
/// </summary>
|
||||
public sealed class PlanningGridMap
|
||||
{
|
||||
private readonly byte[] _occupied;
|
||||
private readonly double[] _conservativeDistances;
|
||||
|
||||
internal PlanningGridMap(MapBoundsMm bounds, float resolutionMm, int rows, int cols, byte[] occupied, double[] conservativeDistances,
|
||||
long snapshotId, bool planningReady, string planningBlockReason, string inputFingerprint, string occupancyHash)
|
||||
{
|
||||
Bounds = bounds; ResolutionMm = resolutionMm; Rows = rows; Cols = cols;
|
||||
_occupied = occupied; _conservativeDistances = conservativeDistances;
|
||||
SnapshotId = snapshotId; PlanningReady = planningReady; PlanningBlockReason = planningBlockReason ?? string.Empty;
|
||||
InputFingerprint = inputFingerprint ?? string.Empty; OccupancyHash = occupancyHash ?? string.Empty;
|
||||
}
|
||||
/// <summary>源环境图的世界边界，单位 mm，采用左闭右开规则。</summary>
|
||||
public MapBoundsMm Bounds { get; }
|
||||
/// <summary>源环境图的栅格边长，单位 mm。</summary>
|
||||
public float ResolutionMm { get; }
|
||||
/// <summary>规划世界查询对应的栅格边长，单位 m。</summary>
|
||||
public double ResolutionMeters { get { return ResolutionMm / 1000d; } }
|
||||
/// <summary>栅格行数。</summary>
|
||||
public int Rows { get; }
|
||||
/// <summary>栅格列数。</summary>
|
||||
public int Cols { get; }
|
||||
/// <summary>工厂为本次返回快照分配的单调编号，用于区分不同构建结果。</summary>
|
||||
public long SnapshotId { get; }
|
||||
/// <summary>地图是否允许进入粗路径规划。true 时可直接查询；false 时应先处理 <see cref="PlanningBlockReason"/>。</summary>
|
||||
public bool PlanningReady { get; }
|
||||
/// <summary>禁止规划的原因。<see cref="PlanningReady"/> 为 true 时为空字符串。</summary>
|
||||
public string PlanningBlockReason { get; }
|
||||
/// <summary>完整建图输入的稳定指纹，用于识别精确输入缓存命中。</summary>
|
||||
public string InputFingerprint { get; }
|
||||
/// <summary>占据栅格内容哈希，用于识别可复用的占据与距离数组。</summary>
|
||||
public string OccupancyHash { get; }
|
||||
|
||||
/// <summary>查询世界位置是否占据。参数 xMeters、yMeters 单位为 m；位置越界时保守地返回 true。</summary>
|
||||
public bool IsOccupiedWorld(double xMeters, double yMeters)
|
||||
{
|
||||
return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) || _occupied[row * Cols + col] != 0;
|
||||
}
|
||||
/// <summary>
|
||||
/// 查询到最近障碍物的保守净距离下界。
|
||||
///
|
||||
/// 参数：xMeters、yMeters 为世界坐标，单位 m。
|
||||
/// 返回：单位 m 的非负距离下界；地图内无障碍物时为正无穷，越界时为零。
|
||||
/// </summary>
|
||||
public double GetConservativeObstacleDistanceMeters(double xMeters, double yMeters)
|
||||
{
|
||||
return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) ? 0d : _conservativeDistances[row * Cols + col];
|
||||
}
|
||||
/// <summary>
|
||||
/// 将规划世界坐标转换为栅格索引。
|
||||
///
|
||||
/// 参数：xMeters、yMeters 为世界坐标，单位 m；row、col 为输出索引。
|
||||
/// 返回：位置在地图内时为 true 并写入索引；否则返回 false，两个输出均为 -1。
|
||||
/// </summary>
|
||||
public bool TryWorldToGrid(double xMeters, double yMeters, out int row, out int col)
|
||||
{
|
||||
row = -1; col = -1;
|
||||
double xMm = xMeters * 1000d, yMm = yMeters * 1000d;
|
||||
if (xMm < Bounds.XMin || xMm >= Bounds.XMax || yMm < Bounds.YMin || yMm >= Bounds.YMax) return false;
|
||||
col = (int)Math.Floor((xMm - Bounds.XMin) / ResolutionMm);
|
||||
row = (int)Math.Floor((yMm - Bounds.YMin) / ResolutionMm);
|
||||
return row >= 0 && row < Rows && col >= 0 && col < Cols;
|
||||
}
|
||||
/// <summary>按行列索引查询占据状态。参数从零开始；任一索引越界时返回 true。</summary>
|
||||
public bool IsOccupied(int row, int col) { return row < 0 || row >= Rows || col < 0 || col >= Cols || _occupied[row * Cols + col] != 0; }
|
||||
internal byte[] CopyOccupied() { return (byte[])_occupied.Clone(); }
|
||||
internal bool OccupancyEquals(PlanningGridMap other)
|
||||
{
|
||||
if (other == null || Rows != other.Rows || Cols != other.Cols || ResolutionMm != other.ResolutionMm || !Bounds.Equals(other.Bounds) || _occupied.Length != other._occupied.Length) return false;
|
||||
for (int i = 0; i < _occupied.Length; i++) if (_occupied[i] != other._occupied[i]) return false;
|
||||
return true;
|
||||
}
|
||||
internal PlanningGridMap WithMetadata(long snapshotId, bool planningReady, string blockReason, string inputFingerprint, string occupancyHash)
|
||||
{
|
||||
return new PlanningGridMap(Bounds, ResolutionMm, Rows, Cols, _occupied, _conservativeDistances, snapshotId, planningReady, blockReason, inputFingerprint, occupancyHash);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>将建图阶段的 mm 环境栅格适配为规划阶段的不可变 m 查询快照。</summary>
|
||||
public static class PlanningMapAdapter
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建规划地图快照及其保守距离场。
|
||||
///
|
||||
/// 参数：environmentMap 为已经完成障碍物栅格化的环境图，坐标与分辨率单位均为 mm。
|
||||
/// 返回：不可变的 <see cref="PlanningGridMap"/>；其世界查询使用 m，初始元数据由工厂随后分配。
|
||||
/// </summary>
|
||||
public static PlanningGridMap Create(EnvironmentGridMap environmentMap)
|
||||
{
|
||||
if (!TryCreate(environmentMap, PlanningOperationBudget.Unlimited(CancellationToken.None), out PlanningGridMap map, out _))
|
||||
throw new InvalidOperationException("Unbounded planning-map adaptation unexpectedly stopped.");
|
||||
return map;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建规划快照；停止时不返回部分地图。</summary>
|
||||
internal static bool TryCreate(EnvironmentGridMap environmentMap, PlanningOperationBudget budget,
|
||||
out PlanningGridMap map, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (environmentMap == null) throw new ArgumentNullException(nameof(environmentMap));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
map = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
byte[] occupied = environmentMap.CopyCells();
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
if (!ObstacleDistanceField.TryCreate(occupied, environmentMap.Rows, environmentMap.Cols,
|
||||
environmentMap.ResolutionMm / 1000d, budget, out ObstacleDistanceField field, out stopReason))
|
||||
return false;
|
||||
map = new PlanningGridMap(environmentMap.Bounds, environmentMap.ResolutionMm, environmentMap.Rows, environmentMap.Cols,
|
||||
occupied, field.CopyDistances(), 0, false, "Map metadata has not been assigned.", string.Empty, string.Empty);
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason == PlanningOperationStopReason.None) return true;
|
||||
map = null;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user