feat: 发布 EM 轨迹规划首个版本
This commit is contained in:
@@ -22,6 +22,7 @@
|
||||
<Compile Remove="tests\EMPlannerVerificationHost\**\*.cs" />
|
||||
<Compile Remove="ParkrobTrajplanner\Trajplanner_output\**\*.cs" />
|
||||
<Compile Remove="ParkrobTrajplanner\Trajplanner_guide\**\*.cs" />
|
||||
<Compile Remove="Shared\**\*.cs" />
|
||||
<Compile Remove="ParkrobTrajplanner\auto_avoidance\**\*.cs"
|
||||
Condition="'$(ExcludeLegacyAutoAvoidance)' == 'true'" />
|
||||
<!-- 嵌套验证项目的旧输出不能参与主插件的程序集解析。 -->
|
||||
@@ -29,6 +30,11 @@
|
||||
<None Remove="**\obj\**\*" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Compile Include="..\Shared\**\*.cs"
|
||||
Link="Shared\%(RecursiveDir)%(Filename)%(Extension)" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="TrajectoryPlanningVisualization\TrajectoryPlanningVisualization.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
-812
@@ -1,812 +0,0 @@
|
||||
<!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>
|
||||
@@ -1,13 +1,34 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 静态走廊采样、偏移和净空配置；距离单位均为 m。
|
||||
/// </summary>
|
||||
public sealed class CorridorConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// 沿参考弧长的走廊采样间距；默认值 0.10 m，必须为正有限值，见 <see cref="EmPlannerConfiguration.CreateDefault"/>。
|
||||
/// </summary>
|
||||
public double LongitudinalSampleSpacingMeters { get; set; }
|
||||
/// <summary>
|
||||
/// 每个采样站横向搜索的间距；默认值 0.025 m，必须为正有限值，见 <see cref="EmPlannerConfiguration.CreateDefault"/>。
|
||||
/// </summary>
|
||||
public double LateralSampleSpacingMeters { get; set; }
|
||||
/// <summary>
|
||||
/// 相对参考线允许搜索的最大横向偏移；单位 m，默认值 0.30，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumLateralOffsetMeters { get; set; }
|
||||
/// <summary>
|
||||
/// 碰撞检测之外预留的附加净空；单位 m，默认值 0.02，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double AdditionalClearanceReserveMeters { get; set; }
|
||||
/// <summary>
|
||||
/// 足迹碰撞检测的最大行进步长；单位 m，默认值 0.025，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 复制当前走廊配置；输入为当前五个标量，输出为无共享可变状态的可修改快照，不对数值作校验且不产生失败状态。
|
||||
/// </summary>
|
||||
internal CorridorConfiguration Copy()
|
||||
{
|
||||
return new CorridorConfiguration
|
||||
|
||||
@@ -1,15 +1,42 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// EM 规划器的完整可变配置树；所有子配置均须非空，建议通过 <see cref="CreateDefault"/> 创建协调且彼此独立的默认快照。
|
||||
/// </summary>
|
||||
public sealed partial class EmPlannerConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// 重规划节奏、时间窗口和资源上限配置；无直接单位，必须为非空的 <see cref="SchedulingConfiguration"/>，默认值为新的调度配置对象。
|
||||
/// </summary>
|
||||
public SchedulingConfiguration Scheduling { get; set; }
|
||||
/// <summary>
|
||||
/// 世界坐标至 Frenet 坐标的投影容差配置；无直接单位，必须为非空的 <see cref="FrenetConfiguration"/>，默认值为新的 Frenet 配置对象。
|
||||
/// </summary>
|
||||
public FrenetConfiguration Frenet { get; set; }
|
||||
/// <summary>
|
||||
/// 静态无碰撞走廊的采样与净空配置；无直接单位，必须为非空的 <see cref="CorridorConfiguration"/>，默认值为新的走廊配置对象。
|
||||
/// </summary>
|
||||
public CorridorConfiguration Corridor { get; set; }
|
||||
/// <summary>
|
||||
/// LS 横向优化的约束与权重配置；无直接单位，必须为非空的 <see cref="LateralConfiguration"/>，默认值为新的横向配置对象。
|
||||
/// </summary>
|
||||
public LateralConfiguration Lateral { get; set; }
|
||||
/// <summary>
|
||||
/// ST 纵向优化的速度、舒适性与权重配置；无直接单位，必须为非空的 <see cref="LongitudinalConfiguration"/>，默认值为新的纵向配置对象。
|
||||
/// </summary>
|
||||
public LongitudinalConfiguration Longitudinal { get; set; }
|
||||
/// <summary>
|
||||
/// QP 求解器的迭代次数、容差和运行选项；无直接单位，必须为非空的 <see cref="SolverConfiguration"/>，默认值为新的求解器配置对象。
|
||||
/// </summary>
|
||||
public SolverConfiguration Solver { get; set; }
|
||||
/// <summary>
|
||||
/// 发布前几何、运动学和终端姿态校验容差；无直接单位，必须为非空的 <see cref="ValidationConfiguration"/>，默认值为新的校验配置对象。
|
||||
/// </summary>
|
||||
public ValidationConfiguration Validation { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 创建用于低速泊车的全套默认配置；每次调用均构造新的子配置和权重对象，因此调用方可修改返回值而不影响其他默认快照。
|
||||
/// </summary>
|
||||
public static EmPlannerConfiguration CreateDefault()
|
||||
{
|
||||
return new EmPlannerConfiguration
|
||||
@@ -17,8 +44,8 @@ public sealed partial class EmPlannerConfiguration
|
||||
Scheduling = new SchedulingConfiguration
|
||||
{
|
||||
ReplanPeriodSeconds = 0.20d,
|
||||
TimeHorizonSeconds = 6d, //窗口的规划时间长度。单位s
|
||||
DistanceHorizonMeters = 500d, //最大探索s里程距离，单位m
|
||||
TimeHorizonSeconds = 6d, // 单次规划的时间窗口，单位 s。
|
||||
DistanceHorizonMeters = 500d, // 单次规划的最大前向探索距离，单位 m。
|
||||
OutputTimeStepSeconds = 0.05d,
|
||||
SolverTimeoutSeconds = 0.10d,
|
||||
HandoffLookaheadSeconds = 0.30d,
|
||||
@@ -103,6 +130,9 @@ public sealed partial class EmPlannerConfiguration
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 深复制当前配置树；输入为当前实例状态，输出与当前实例不共享非空嵌套对象的可修改快照，原本为 <c>null</c> 的子配置保持为 <c>null</c> 且不产生失败状态。
|
||||
/// </summary>
|
||||
internal EmPlannerConfiguration Copy()
|
||||
{
|
||||
return new EmPlannerConfiguration
|
||||
|
||||
@@ -1,11 +1,26 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 世界坐标与 Frenet 参考之间投影和边界判定的容差配置。
|
||||
/// </summary>
|
||||
public sealed class FrenetConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// 世界姿态投影到方向段所允许的最大欧氏距离；单位 m，默认值 0.50，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumProjectionDistanceMeters { get; set; }
|
||||
/// <summary>
|
||||
/// Frenet 重建中 <c>1-kappa*l</c> 的最小绝对安全余量；无量纲，默认值 0.20，必须为大于 0 且小于 1 的有限值。
|
||||
/// </summary>
|
||||
public double MinimumFrenetDenominator { get; set; }
|
||||
/// <summary>
|
||||
/// 判断投影是否锚定在段边界的距离容差；单位 m，默认值 1e-8，必须为正有限值。
|
||||
/// </summary>
|
||||
public double BoundaryAnchorToleranceMeters { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 复制当前 Frenet 配置；输入为当前三个标量，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
|
||||
/// </summary>
|
||||
internal FrenetConfiguration Copy()
|
||||
{
|
||||
return new FrenetConfiguration
|
||||
|
||||
@@ -1,13 +1,34 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// LS 横向优化的离散、收敛、曲率和边界限制配置。
|
||||
/// </summary>
|
||||
public sealed class LateralConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// 相邻外层迭代横向解之间允许的最大偏移量；单位 m，默认值 0.05，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumLateralStepPerIterationMeters { get; set; }
|
||||
/// <summary>
|
||||
/// 横向偏移对弧长的一阶导数上限；无量纲，默认值 0.50，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumLateralSlope { get; set; }
|
||||
/// <summary>
|
||||
/// 横向偏移对弧长的二阶导数上限；单位 1/m，默认值 1，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumLateralSecondDerivativePerMeter { get; set; }
|
||||
/// <summary>
|
||||
/// 横向偏移对弧长的三阶导数上限；单位 1/m²，默认值 2，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumLateralThirdDerivativePerSquareMeter { get; set; }
|
||||
/// <summary>
|
||||
/// 横向 QP 目标函数的权重集合；无直接单位，必须非空且其每项为非负有限值，默认值为新的 <see cref="LateralWeights"/> 对象。
|
||||
/// </summary>
|
||||
public LateralWeights Weights { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 复制当前横向配置；输入为当前标量和权重引用，输出为不共享非空权重对象的可修改快照，权重为 <c>null</c> 时保持为空且不产生失败状态。
|
||||
/// </summary>
|
||||
internal LateralConfiguration Copy()
|
||||
{
|
||||
return new LateralConfiguration
|
||||
|
||||
@@ -1,16 +1,46 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 横向 QP 目标函数的权重快照；权重只影响偏好，不放宽安全约束。
|
||||
/// </summary>
|
||||
public sealed class LateralWeights
|
||||
{
|
||||
/// <summary>
|
||||
/// 惩罚偏离参考线横向偏移的权重；无量纲，默认值 10，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double ReferenceOffset { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚横向偏移一阶导数的权重；无量纲，默认值 1，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double HeadingDeviation { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚横向偏移二阶导数的权重；无量纲，默认值 5，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double SecondDerivative { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚横向偏移三阶导数的权重；无量纲，默认值 10，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double ThirdDerivative { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚由横向解导出的车辆曲率的权重；无量纲，默认值 5，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double Curvature { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚相邻站曲率变化的权重；无量纲，默认值 20，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double CurvatureVariation { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚偏离上一轮横向轨迹的权重；无量纲，默认值 5，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double PreviousTrajectory { get; set; }
|
||||
/// <summary>
|
||||
/// 滚动规划末端横向状态的稳定权重；无量纲，默认值 10，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double RollingTerminal { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 复制当前横向权重；输入为当前八个标量，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
|
||||
/// </summary>
|
||||
internal LateralWeights Copy()
|
||||
{
|
||||
return new LateralWeights
|
||||
|
||||
@@ -1,20 +1,62 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// ST 纵向优化的速度、舒适性和终端保持限制配置；所有数值由 <see cref="EmPlannerConfiguration.CreateDefault"/> 提供低速泊车默认值。
|
||||
/// </summary>
|
||||
public sealed class LongitudinalConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// 前进方向允许的最大速度；单位 m/s，默认值 1，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumForwardSpeedMetersPerSecond { get; set; }
|
||||
/// <summary>
|
||||
/// 倒车方向允许的最大速度；单位 m/s，默认值 0.5，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumReverseSpeedMetersPerSecond { get; set; }
|
||||
/// <summary>
|
||||
/// 前进方向的目标巡航速度；单位 m/s，默认值 1，必须为正有限值且不得超过 <see cref="MaximumForwardSpeedMetersPerSecond"/>。
|
||||
/// </summary>
|
||||
public double DesiredForwardSpeedMetersPerSecond { get; set; }
|
||||
/// <summary>
|
||||
/// 倒车方向的目标巡航速度；单位 m/s，默认值 0.5，必须为正有限值且不得超过 <see cref="MaximumReverseSpeedMetersPerSecond"/>。
|
||||
/// </summary>
|
||||
public double DesiredReverseSpeedMetersPerSecond { get; set; }
|
||||
/// <summary>
|
||||
/// 允许的最大纵向加速度；单位 m/s²，默认值 0.20，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumAccelerationMetersPerSecondSquared { get; set; }
|
||||
/// <summary>
|
||||
/// 允许的最大纵向减速度幅值；单位 m/s²，默认值 0.30，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumDecelerationMetersPerSecondSquared { get; set; }
|
||||
/// <summary>
|
||||
/// 允许的最大纵向加加速度幅值；单位 m/s³，默认值 0.50，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumJerkMetersPerSecondCubed { get; set; }
|
||||
/// <summary>
|
||||
/// 由速度和曲率共同施加的最大横向加速度；单位 m/s²，默认值 0.20，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumLateralAccelerationMetersPerSecondSquared { get; set; }
|
||||
/// <summary>
|
||||
/// 车辆曲率随时间变化的上限；单位 1/(m·s)，默认值 0.50，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumCurvatureRatePerMeterPerSecond { get; set; }
|
||||
/// <summary>
|
||||
/// 将进度速度视为停止的容差；单位 m/s，默认值 0.01，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double StopSpeedToleranceMetersPerSecond { get; set; }
|
||||
/// <summary>
|
||||
/// 到达零速度后需保持的时长；单位 s，默认值 0.20，必须为正有限值。
|
||||
/// </summary>
|
||||
public double ZeroSpeedHoldSeconds { get; set; }
|
||||
/// <summary>
|
||||
/// 纵向 QP 目标函数的权重集合；无直接单位，必须非空且其每项为非负有限值，默认值为新的 <see cref="LongitudinalWeights"/> 对象。
|
||||
/// </summary>
|
||||
public LongitudinalWeights Weights { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 复制当前纵向配置；输入为当前标量和权重引用，输出为不共享非空权重对象的可修改快照，权重为 <c>null</c> 时保持为空且不产生失败状态。
|
||||
/// </summary>
|
||||
internal LongitudinalConfiguration Copy()
|
||||
{
|
||||
return new LongitudinalConfiguration
|
||||
|
||||
@@ -1,13 +1,34 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 纵向 QP 目标函数的权重快照；权重只改变偏好而不放宽硬约束。
|
||||
/// </summary>
|
||||
public sealed class LongitudinalWeights
|
||||
{
|
||||
/// <summary>
|
||||
/// 惩罚偏离参考速度的权重；无量纲，默认值 10，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double ReferenceSpeed { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚纵向加速度的权重；无量纲，默认值 1，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double Acceleration { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚纵向加加速度的权重；无量纲，默认值 10，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double Jerk { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚偏离上一条轨迹速度解的权重；无量纲，默认值 5，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double PreviousTrajectory { get; set; }
|
||||
/// <summary>
|
||||
/// 惩罚末端加速度的权重；无量纲，默认值 1，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double TerminalAcceleration { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 复制当前纵向权重；输入为当前五个标量，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
|
||||
/// </summary>
|
||||
internal LongitudinalWeights Copy()
|
||||
{
|
||||
return new LongitudinalWeights
|
||||
|
||||
@@ -1,19 +1,58 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// EM 规划的触发节奏、预测窗口、离散上限和发布样本上限配置；默认值由 <see cref="EmPlannerConfiguration.CreateDefault"/> 为低速泊车协调。
|
||||
/// </summary>
|
||||
public sealed class SchedulingConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// 两次规划触发之间的周期；单位 s，默认值 0.20，必须为正有限值。
|
||||
/// </summary>
|
||||
public double ReplanPeriodSeconds { get; set; }
|
||||
/// <summary>
|
||||
/// 单次优化覆盖的预测时间窗口；单位 s，默认值 6，必须为正有限值。
|
||||
/// </summary>
|
||||
public double TimeHorizonSeconds { get; set; }
|
||||
/// <summary>
|
||||
/// 单次优化覆盖的最大参考线距离；单位 m，默认值 500，必须为正有限值；滚动规划时还须覆盖制动距离及一个重规划周期内的行程。
|
||||
/// </summary>
|
||||
public double DistanceHorizonMeters { get; set; }
|
||||
/// <summary>
|
||||
/// 发布轨迹相邻样本的时间间隔；单位 s，默认值 0.05，必须为正有限值。
|
||||
/// </summary>
|
||||
public double OutputTimeStepSeconds { get; set; }
|
||||
/// <summary>
|
||||
/// 单次求解允许使用的超时预算；单位 s，默认值 0.10，必须为正有限值。
|
||||
/// </summary>
|
||||
public double SolverTimeoutSeconds { get; set; }
|
||||
/// <summary>
|
||||
/// 与已发布轨迹交接时向前查看的时长；单位 s，默认值 0.30，必须为正有限值。
|
||||
/// </summary>
|
||||
public double HandoffLookaheadSeconds { get; set; }
|
||||
/// <summary>
|
||||
/// 接受车辆状态输入的最大时效；单位 s，默认值 0.20，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumVehicleStateAgeSeconds { get; set; }
|
||||
/// <summary>
|
||||
/// 纵向优化结点允许的最大时间间距；单位 s，默认值 0.20，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumOptimizationTimeStepSeconds { get; set; }
|
||||
/// <summary>
|
||||
/// 横向或走廊优化结点允许的最大空间间距；单位 m，默认值 0.10，必须为正有限值。
|
||||
/// </summary>
|
||||
public double MaximumOptimizationSpatialStepMeters { get; set; }
|
||||
/// <summary>
|
||||
/// 一次优化允许的最大结点数量；单位为结点数，默认值 401，必须为不小于 3 的整数。
|
||||
/// </summary>
|
||||
public int MaximumOptimizationKnotCount { get; set; }
|
||||
/// <summary>
|
||||
/// 一条发布轨迹允许的最大样本数量；单位为样本数，默认值 5001，必须为不小于 2 的整数。
|
||||
/// </summary>
|
||||
public int MaximumPublishedSampleCount { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 复制当前调度配置；输入为当前标量限制，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
|
||||
/// </summary>
|
||||
internal SchedulingConfiguration Copy()
|
||||
{
|
||||
return new SchedulingConfiguration
|
||||
|
||||
@@ -1,16 +1,46 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// LS/ST QP 求解器的迭代预算、收敛容差和运行选项。
|
||||
/// </summary>
|
||||
public sealed class SolverConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// 外层顺序凸化最大迭代次数；默认值 5，必须为正整数。
|
||||
/// </summary>
|
||||
public int MaximumOuterIterations { get; set; }
|
||||
/// <summary>
|
||||
/// 单次 OSQP 求解的最大迭代次数；默认值 4000，必须为正整数。
|
||||
/// </summary>
|
||||
public int MaximumOsqpIterations { get; set; }
|
||||
/// <summary>
|
||||
/// 求解器绝对残差容差；默认值 1e-5，必须为正有限值。
|
||||
/// </summary>
|
||||
public double AbsoluteTolerance { get; set; }
|
||||
/// <summary>
|
||||
/// 求解器相对残差容差；默认值 1e-5，必须为正有限值。
|
||||
/// </summary>
|
||||
public double RelativeTolerance { get; set; }
|
||||
/// <summary>
|
||||
/// 发布前接受解的严格残差容差；默认值 1e-5，必须为正有限值。
|
||||
/// </summary>
|
||||
public double StrictResidualTolerance { get; set; }
|
||||
/// <summary>
|
||||
/// 是否将上一轮可用解作为初值；无单位，默认值 <c>true</c>，仅接受布尔值。
|
||||
/// </summary>
|
||||
public bool WarmStart { get; set; }
|
||||
/// <summary>
|
||||
/// 是否请求 OSQP 进行结果修正；无单位，默认值 <c>true</c>，仅接受布尔值。
|
||||
/// </summary>
|
||||
public bool Polish { get; set; }
|
||||
/// <summary>
|
||||
/// 是否启用原生求解器诊断输出；无单位，默认值 <c>false</c>，仅接受布尔值。
|
||||
/// </summary>
|
||||
public bool NativeVerbose { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 复制当前求解器配置；输入为当前迭代预算、容差和布尔选项，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
|
||||
/// </summary>
|
||||
internal SolverConfiguration Copy()
|
||||
{
|
||||
return new SolverConfiguration
|
||||
|
||||
@@ -1,12 +1,30 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 发布前轨迹几何、运动学和终端误差的验收容差配置；默认值由 <see cref="EmPlannerConfiguration.CreateDefault"/> 提供。
|
||||
/// </summary>
|
||||
public sealed class ValidationConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// 几何位置、弧长和采样比较使用的空间容差；单位 m，默认值 1e-8，必须为正有限值。
|
||||
/// </summary>
|
||||
public double SpatialToleranceMeters { get; set; }
|
||||
/// <summary>
|
||||
/// 速度、加速度和曲率等无量纲相对比较的容差；无量纲，默认值 1e-5，必须为正有限值。
|
||||
/// </summary>
|
||||
public double KinematicTolerance { get; set; }
|
||||
/// <summary>
|
||||
/// 精确终止模式下终端位置允许的误差；单位 m，默认值 0，必须为非负有限值。
|
||||
/// </summary>
|
||||
public double TerminalPositionToleranceMeters { get; set; }
|
||||
/// <summary>
|
||||
/// 精确终止模式下终端航向允许的误差；单位 rad，默认值 0，必须为 [0, π] 内的有限值。
|
||||
/// </summary>
|
||||
public double TerminalYawToleranceRadians { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 复制当前校验配置；输入为当前四个容差，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
|
||||
/// </summary>
|
||||
internal ValidationConfiguration Copy()
|
||||
{
|
||||
return new ValidationConfiguration
|
||||
|
||||
@@ -2,8 +2,14 @@ using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 验证规划契约中必须为有限数的标量输入。
|
||||
/// </summary>
|
||||
internal static class ContractNumeric
|
||||
{
|
||||
/// <summary>
|
||||
/// 拒绝 NaN 或无穷数值。<paramref name="value"/> 的单位由调用方语境决定；<paramref name="parameterName"/> 原样传给异常构造器，方法本身不验证它。
|
||||
/// </summary>
|
||||
public static void RequireFinite(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
|
||||
@@ -1,10 +1,28 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 EM 规划中参考段终端或滚动窗口终端的边界类型。
|
||||
/// </summary>
|
||||
public enum EmBoundaryType
|
||||
{
|
||||
/// <summary>
|
||||
/// 非边界采样点；不携带终端或换挡边界语义。
|
||||
/// </summary>
|
||||
None,
|
||||
/// <summary>
|
||||
/// 滚动窗口因安全约束形成的停车终点，而非方向段的真实终点。
|
||||
/// </summary>
|
||||
RollingSafetyStop,
|
||||
/// <summary>
|
||||
/// 到达换挡位置前的真实停车边界。
|
||||
/// </summary>
|
||||
GearSwitchApproach,
|
||||
/// <summary>
|
||||
/// 换挡后新方向段开始时的离开边界。
|
||||
/// </summary>
|
||||
GearSwitchDeparture,
|
||||
/// <summary>
|
||||
/// 整条参考路径或当前任务目标的真实终点边界。
|
||||
/// </summary>
|
||||
Goal,
|
||||
}
|
||||
|
||||
@@ -1,8 +1,20 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 指定纵向规划面对当前窗口末端时采用的速度终端语义。
|
||||
/// </summary>
|
||||
public enum EmLongitudinalMode
|
||||
{
|
||||
/// <summary>
|
||||
/// 普通滚动窗口末端；保持连续行驶，不要求在本窗口内停止。
|
||||
/// </summary>
|
||||
RollingContinuation,
|
||||
/// <summary>
|
||||
/// 将真实停车边界纳入窗口，但在可达性不足时仅以可停车方式接近。
|
||||
/// </summary>
|
||||
ApproachStopBoundary,
|
||||
/// <summary>
|
||||
/// 要求在当前窗口内到达真实边界并保持零速。
|
||||
/// </summary>
|
||||
ExactStopAtBoundary,
|
||||
}
|
||||
|
||||
@@ -1,8 +1,20 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 请求允许的车辆运动模型；服务依据此值选择或拒绝相应规划分支。
|
||||
/// </summary>
|
||||
public enum EmMotionModel
|
||||
{
|
||||
/// <summary>
|
||||
/// 常规非完整约束车辆，可前进或倒车但不能侧移。
|
||||
/// </summary>
|
||||
NonholonomicForwardReverse,
|
||||
/// <summary>
|
||||
/// 蟹行平移模型；当前 EM 管线不支持时返回明确状态。
|
||||
/// </summary>
|
||||
CrabTranslation,
|
||||
/// <summary>
|
||||
/// 原地旋转模型；当前 EM 管线不支持时返回明确状态。
|
||||
/// </summary>
|
||||
InPlaceRotation,
|
||||
}
|
||||
|
||||
@@ -1,12 +1,136 @@
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal interface IEmPlanningPublicationAuthorization
|
||||
{
|
||||
TimeSpan Remaining { get; }
|
||||
|
||||
bool IsExpired { get; }
|
||||
|
||||
EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
|
||||
CancellationToken coordinatorCallerCancellationToken, Action publish);
|
||||
}
|
||||
|
||||
internal enum EmPlanningPublicationDecision
|
||||
{
|
||||
Published = 0,
|
||||
CallerCancelled = 1,
|
||||
DeadlineExpired = 2,
|
||||
}
|
||||
|
||||
internal sealed class EmPlanningRequestPublicationAuthorization : IEmPlanningPublicationAuthorization
|
||||
{
|
||||
private readonly object gate = new object();
|
||||
private readonly TimeSpan? initialRemaining;
|
||||
private readonly Stopwatch stopwatch;
|
||||
private readonly CancellationToken deadlineToken;
|
||||
private bool callerCancelled;
|
||||
private bool deadlineExpired;
|
||||
|
||||
public EmPlanningRequestPublicationAuthorization(TimeSpan? remaining, CancellationToken deadlineToken)
|
||||
{
|
||||
initialRemaining = remaining;
|
||||
this.deadlineToken = deadlineToken;
|
||||
stopwatch = remaining.HasValue ? Stopwatch.StartNew() : null;
|
||||
deadlineExpired = remaining.HasValue && remaining.Value <= TimeSpan.Zero ||
|
||||
deadlineToken.IsCancellationRequested;
|
||||
}
|
||||
|
||||
public TimeSpan Remaining
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (gate)
|
||||
{
|
||||
ObserveDeadlineInsideGate();
|
||||
return ComputeRemainingInsideGate();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public bool IsExpired
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (gate)
|
||||
{
|
||||
ObserveDeadlineInsideGate();
|
||||
return deadlineExpired;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
|
||||
CancellationToken coordinatorCallerCancellationToken, Action publish)
|
||||
{
|
||||
if (publish == null)
|
||||
throw new ArgumentNullException(nameof(publish));
|
||||
|
||||
using CancellationTokenRegistration requestCallerRegistration =
|
||||
requestCallerCancellationToken.Register(ObserveCallerCancellation);
|
||||
using CancellationTokenRegistration coordinatorCallerRegistration =
|
||||
coordinatorCallerCancellationToken.Register(ObserveCallerCancellation);
|
||||
using CancellationTokenRegistration deadlineRegistration =
|
||||
deadlineToken.Register(ObserveDeadlineCancellation);
|
||||
lock (gate)
|
||||
{
|
||||
if (requestCallerCancellationToken.IsCancellationRequested ||
|
||||
coordinatorCallerCancellationToken.IsCancellationRequested)
|
||||
{
|
||||
callerCancelled = true;
|
||||
}
|
||||
ObserveDeadlineInsideGate();
|
||||
if (callerCancelled)
|
||||
return EmPlanningPublicationDecision.CallerCancelled;
|
||||
if (deadlineExpired)
|
||||
return EmPlanningPublicationDecision.DeadlineExpired;
|
||||
publish();
|
||||
return EmPlanningPublicationDecision.Published;
|
||||
}
|
||||
}
|
||||
|
||||
private void ObserveCallerCancellation()
|
||||
{
|
||||
lock (gate)
|
||||
callerCancelled = true;
|
||||
}
|
||||
|
||||
private void ObserveDeadlineCancellation()
|
||||
{
|
||||
lock (gate)
|
||||
deadlineExpired = true;
|
||||
}
|
||||
|
||||
private void ObserveDeadlineInsideGate()
|
||||
{
|
||||
if (deadlineToken.IsCancellationRequested || ComputeRemainingInsideGate() <= TimeSpan.Zero)
|
||||
deadlineExpired = true;
|
||||
}
|
||||
|
||||
private TimeSpan ComputeRemainingInsideGate()
|
||||
{
|
||||
if (!initialRemaining.HasValue)
|
||||
return deadlineExpired ? TimeSpan.Zero : TimeSpan.MaxValue;
|
||||
TimeSpan remaining = initialRemaining.Value - stopwatch.Elapsed;
|
||||
return remaining > TimeSpan.Zero && !deadlineExpired ? remaining : TimeSpan.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// EM 单次规划的只读输入引用快照，绑定平滑参考路径、地图、车辆状态、目标方向段和输出身份。
|
||||
/// 坐标使用 m，航向使用 rad，时间使用 UTC；调用方必须在进入服务前保证这些输入属于同一业务版本。
|
||||
/// </summary>
|
||||
public sealed class EmPlanningRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存一次规划所需的引用快照和发布身份。构造器只赋值而不校验、复制或取得对象所有权；调用方仍拥有传入引用，服务随后负责验证并复制配置。
|
||||
/// </summary>
|
||||
public EmPlanningRequest(
|
||||
PathSmoothingResult referencePath,
|
||||
PlanningGridMap map,
|
||||
@@ -21,7 +145,10 @@ public sealed class EmPlanningRequest
|
||||
string referencePathId,
|
||||
string previousTrajectoryId,
|
||||
EmMotionModel motionModel,
|
||||
EmPlanningScope planningScope)
|
||||
EmPlanningScope planningScope,
|
||||
TimeSpan? cycleDeadlineRemaining = null,
|
||||
CancellationToken callerCancellationToken = default,
|
||||
CancellationToken cycleDeadlineToken = default)
|
||||
{
|
||||
ReferencePath = referencePath;
|
||||
Map = map;
|
||||
@@ -37,25 +164,166 @@ public sealed class EmPlanningRequest
|
||||
PreviousTrajectoryId = previousTrajectoryId;
|
||||
MotionModel = motionModel;
|
||||
PlanningScope = planningScope;
|
||||
CycleDeadlineRemaining = cycleDeadlineRemaining;
|
||||
CallerCancellationToken = callerCancellationToken;
|
||||
CycleDeadlineToken = cycleDeadlineToken;
|
||||
PublicationAuthorization = new EmPlanningRequestPublicationAuthorization(
|
||||
cycleDeadlineRemaining, cycleDeadlineToken);
|
||||
}
|
||||
|
||||
internal EmPlanningRequest(
|
||||
PathSmoothingResult referencePath,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
VehicleMotionState vehicleState,
|
||||
EmPlannerConfiguration configuration,
|
||||
int segmentIndex,
|
||||
EmTrajectory previousTrajectory,
|
||||
DateTimeOffset requestedAtUtc,
|
||||
DateTimeOffset effectiveAtUtc,
|
||||
string outputTrajectoryId,
|
||||
string referencePathId,
|
||||
string previousTrajectoryId,
|
||||
EmMotionModel motionModel,
|
||||
EmPlanningScope planningScope,
|
||||
TimeSpan? cycleDeadlineRemaining,
|
||||
CancellationToken callerCancellationToken,
|
||||
CancellationToken cycleDeadlineToken,
|
||||
IEmPlanningPublicationAuthorization publicationAuthorization)
|
||||
: this(referencePath, map, vehicle, vehicleState, configuration, segmentIndex, previousTrajectory,
|
||||
requestedAtUtc, effectiveAtUtc, outputTrajectoryId, referencePathId, previousTrajectoryId,
|
||||
motionModel, planningScope, cycleDeadlineRemaining, callerCancellationToken, cycleDeadlineToken)
|
||||
{
|
||||
PublicationAuthorization = publicationAuthorization ??
|
||||
new EmPlanningRequestPublicationAuthorization(cycleDeadlineRemaining, cycleDeadlineToken);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 平滑后的参考路径结果引用；服务要求其为可消费的成功结果，且调用方负责保持其与其他输入版本一致。
|
||||
/// </summary>
|
||||
public PathSmoothingResult ReferencePath { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 规划使用的栅格地图快照引用；地图坐标单位和快照身份由地图契约定义，构造器不检查 null 或就绪状态。
|
||||
/// </summary>
|
||||
public PlanningGridMap Map { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 车辆几何与运动限制引用；调用方拥有该对象，服务在请求验证后使用其约束。
|
||||
/// </summary>
|
||||
public VehicleParameters Vehicle { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 一次性捕获的车辆运动状态；其位置为世界坐标 m、航向为 rad，时效和数值有效性由服务验证。
|
||||
/// </summary>
|
||||
public VehicleMotionState VehicleState { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 本次规划的配置引用；请求不深拷贝它，验证通过后服务取得配置副本供本次规划使用。
|
||||
/// </summary>
|
||||
public EmPlannerConfiguration Configuration { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 参考路径中待规划的方向段索引；必须由服务验证为有效的非负范围。
|
||||
/// </summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 可选的上一条已发布轨迹引用，用于衔接或诊断；构造器允许为 null，调用方保有其所有权。
|
||||
/// </summary>
|
||||
public EmTrajectory PreviousTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 本次规划请求发起的 UTC 时刻；用于车辆状态时效判断和生成轨迹元数据，构造器不验证其时间关系。
|
||||
/// </summary>
|
||||
public DateTimeOffset RequestedAtUtc { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 成功发布轨迹开始生效的 UTC 时刻；由下游消费者依据其调度语义使用。
|
||||
/// </summary>
|
||||
public DateTimeOffset EffectiveAtUtc { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 成功轨迹的发布标识；构造器不校验，轨迹元数据构造时要求为非空白字符串。
|
||||
/// </summary>
|
||||
public string OutputTrajectoryId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 输入参考路径的业务标识，用于成功轨迹的来源追踪；构造器不校验其 null 或空白值。
|
||||
/// </summary>
|
||||
public string ReferencePathId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 上一条轨迹的业务标识；允许为 null，成功轨迹元数据会将 null 规范化为空字符串。
|
||||
/// </summary>
|
||||
public string PreviousTrajectoryId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 请求的车辆运动模型；不支持或无效的枚举值由服务转换为失败状态。
|
||||
/// </summary>
|
||||
public EmMotionModel MotionModel { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 本次请求覆盖滚动窗口或整个方向段的范围；服务负责验证其枚举值和资源约束。
|
||||
/// </summary>
|
||||
public EmPlanningScope PlanningScope { get; }
|
||||
|
||||
/// <summary>Optional remaining wall-clock budget for the complete outer planning cycle.</summary>
|
||||
public TimeSpan? CycleDeadlineRemaining { get; }
|
||||
|
||||
/// <summary>Dynamic caller-cancellation origin used to preserve terminal-status precedence.</summary>
|
||||
public CancellationToken CallerCancellationToken { get; }
|
||||
|
||||
/// <summary>Dynamic shared-cycle deadline origin checked again at atomic publication.</summary>
|
||||
public CancellationToken CycleDeadlineToken { get; }
|
||||
|
||||
internal IEmPlanningPublicationAuthorization PublicationAuthorization { get; }
|
||||
|
||||
internal bool IsCycleDeadlineExpired()
|
||||
{
|
||||
try
|
||||
{
|
||||
return PublicationAuthorization.IsExpired;
|
||||
}
|
||||
catch
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
internal TimeSpan? DynamicCycleDeadlineRemaining()
|
||||
{
|
||||
try
|
||||
{
|
||||
TimeSpan remaining = PublicationAuthorization.Remaining;
|
||||
return remaining > TimeSpan.Zero ? remaining : TimeSpan.Zero;
|
||||
}
|
||||
catch
|
||||
{
|
||||
return TimeSpan.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
internal EmPlanningPublicationDecision TryAuthorizePublication(
|
||||
CancellationToken coordinatorCallerCancellationToken, Action publish)
|
||||
{
|
||||
if (publish == null)
|
||||
throw new ArgumentNullException(nameof(publish));
|
||||
try
|
||||
{
|
||||
return PublicationAuthorization.TryPublish(CallerCancellationToken,
|
||||
coordinatorCallerCancellationToken, publish);
|
||||
}
|
||||
catch
|
||||
{
|
||||
return EmPlanningPublicationDecision.DeadlineExpired;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// The request owns this DTO contract; Task 2 adds its request-bound settings.
|
||||
/// <summary>
|
||||
/// 为请求契约保留的 <see cref="EmPlannerConfiguration"/> 部分声明；实际配置成员由其他同名分部提供。
|
||||
/// </summary>
|
||||
public sealed partial class EmPlannerConfiguration
|
||||
{
|
||||
}
|
||||
|
||||
@@ -2,8 +2,14 @@ using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 一次 EM 规划的不可变结果。只有成功状态才携带可发布的完整轨迹；其余状态仅提供诊断，不能作为部分执行轨迹消费。
|
||||
/// </summary>
|
||||
public sealed class EmPlanningResult
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建规划结果并强制成功状态与轨迹发布的一致性：<see cref="EmPlanningStatus.Success"/> 和 <see cref="EmPlanningStatus.SuccessWithFallback"/> 必须提供非 null 轨迹，其他状态必须提供 null 轨迹；null 失败原因规范化为空字符串。
|
||||
/// </summary>
|
||||
public EmPlanningResult(EmPlanningStatus status, EmTrajectory trajectory, string failureReason)
|
||||
{
|
||||
if (!Enum.IsDefined(typeof(EmPlanningStatus), status))
|
||||
@@ -20,9 +26,18 @@ public sealed class EmPlanningResult
|
||||
FailureReason = failureReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 本次规划的最终状态；消费者必须先判断其是否为成功状态，再访问可发布轨迹。
|
||||
/// </summary>
|
||||
public EmPlanningStatus Status { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 仅在成功或成功降级状态下存在的完整可发布轨迹；失败、取消和无效输入结果始终为 null，消费者不得把失败结果当作部分轨迹执行。
|
||||
/// </summary>
|
||||
public EmTrajectory Trajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 面向诊断的失败或降级原因；null 输入已规范化为空字符串，不替代 <see cref="Status"/> 的机器可读状态。
|
||||
/// </summary>
|
||||
public string FailureReason { get; }
|
||||
}
|
||||
|
||||
@@ -1,7 +1,16 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 定义一次 EM 规划覆盖当前滚动窗口还是完整方向段。
|
||||
/// </summary>
|
||||
public enum EmPlanningScope
|
||||
{
|
||||
/// <summary>
|
||||
/// 仅覆盖有限的滚动规划窗口；段末之外保留给后续重规划。
|
||||
/// </summary>
|
||||
RollingHorizon,
|
||||
/// <summary>
|
||||
/// 覆盖选定方向段直至真实停车边界，并受完整段资源上限保护。
|
||||
/// </summary>
|
||||
FullDirectionSegment,
|
||||
}
|
||||
|
||||
@@ -1,26 +1,96 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 EM 规划请求的最终处理状态；仅两个成功状态允许发布轨迹，其余状态要求消费者保留现有安全策略并读取诊断。
|
||||
/// </summary>
|
||||
public enum EmPlanningStatus
|
||||
{
|
||||
/// <summary>
|
||||
/// 所有规划和验证阶段成功，结果可按其生效时间发布。
|
||||
/// </summary>
|
||||
Success,
|
||||
/// <summary>
|
||||
/// 已发布经过验证的降级或回退轨迹；消费者仍可使用轨迹，并应记录诊断原因。
|
||||
/// </summary>
|
||||
SuccessWithFallback,
|
||||
/// <summary>
|
||||
/// 请求对象或其成员未满足服务输入要求。
|
||||
/// </summary>
|
||||
InvalidInput,
|
||||
/// <summary>
|
||||
/// 请求的车辆运动模型未被当前 EM 管线支持。
|
||||
/// </summary>
|
||||
UnsupportedMotionMode,
|
||||
/// <summary>
|
||||
/// 车辆状态采集时刻相对于请求时刻过旧。
|
||||
/// </summary>
|
||||
StaleVehicleState,
|
||||
/// <summary>
|
||||
/// 车辆状态的行驶方向与选定参考路径方向段不一致。
|
||||
/// </summary>
|
||||
StateDirectionMismatch,
|
||||
/// <summary>
|
||||
/// 参考路径结果、方向段或其状态不能用于规划。
|
||||
/// </summary>
|
||||
InvalidReferencePath,
|
||||
/// <summary>
|
||||
/// 无法将车辆状态投影到选定参考路径或方向段。
|
||||
/// </summary>
|
||||
ProjectionFailed,
|
||||
/// <summary>
|
||||
/// 地图和车辆约束下未能构造可行的行驶走廊。
|
||||
/// </summary>
|
||||
CorridorInfeasible,
|
||||
/// <summary>
|
||||
/// 横向优化未得到满足约束的候选解。
|
||||
/// </summary>
|
||||
LateralInfeasible,
|
||||
/// <summary>
|
||||
/// 纵向优化未得到满足时空与动力学约束的候选解。
|
||||
/// </summary>
|
||||
LongitudinalInfeasible,
|
||||
/// <summary>
|
||||
/// 当前速度、距离或限制不足以在所需边界前完成停车。
|
||||
/// </summary>
|
||||
StoppingDistanceInsufficient,
|
||||
/// <summary>
|
||||
/// 所需二次规划求解器不可用。
|
||||
/// </summary>
|
||||
SolverUnavailable,
|
||||
/// <summary>
|
||||
/// 求解在共享时间预算或迭代限制内未完成。
|
||||
/// </summary>
|
||||
SolverTimedOut,
|
||||
/// <summary>
|
||||
/// 调用方在可发布结果生成前取消了请求。
|
||||
/// </summary>
|
||||
Cancelled,
|
||||
/// <summary>
|
||||
/// 候选轨迹未通过最终世界空间、动力学或终端语义验证。
|
||||
/// </summary>
|
||||
ValidationFailed,
|
||||
/// <summary>
|
||||
/// 请求在处理期间被更新版本的规划工作替代。
|
||||
/// </summary>
|
||||
Superseded,
|
||||
/// <summary>
|
||||
/// 候选轨迹相对于当前状态未形成足够的有效进展。
|
||||
/// </summary>
|
||||
NoProgress,
|
||||
/// <summary>
|
||||
/// 轨迹末端位姿未满足要求的真实终端位姿。
|
||||
/// </summary>
|
||||
TerminalPoseMismatch,
|
||||
/// <summary>
|
||||
/// 完整方向段规划超过配置的时间、采样或求解资源上限。
|
||||
/// </summary>
|
||||
FullSegmentResourceLimitExceeded,
|
||||
/// <summary>
|
||||
/// 未被其他状态细分的规划失败。
|
||||
/// </summary>
|
||||
Failed,
|
||||
/// <summary>
|
||||
/// The outer planning cycle exhausted its shared bootstrap-to-publication deadline.
|
||||
/// </summary>
|
||||
CycleDeadlineExpired,
|
||||
}
|
||||
|
||||
@@ -1,8 +1,20 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 表示已发布轨迹末端所对应的规划终端语义。
|
||||
/// </summary>
|
||||
public enum EmTerminalType
|
||||
{
|
||||
/// <summary>
|
||||
/// 因滚动窗口安全约束形成的停车终端，不代表方向段完成。
|
||||
/// </summary>
|
||||
RollingSafetyStop,
|
||||
/// <summary>
|
||||
/// 用于在方向改变前后执行换挡的终端。
|
||||
/// </summary>
|
||||
GearSwitch,
|
||||
/// <summary>
|
||||
/// 最终任务目标的终端。
|
||||
/// </summary>
|
||||
Goal,
|
||||
}
|
||||
|
||||
@@ -4,8 +4,14 @@ using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 供成功规划结果发布的时间参数化轨迹；本类型只施加结构约束，世界空间复核由上游服务完成，点序列、元数据和终端语义在发布后保持不可变。
|
||||
/// </summary>
|
||||
public sealed class EmTrajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建可发布的轨迹并复制点列表容器。元数据和每个点对象按引用保存、调用方仍拥有它们；传入列表可随后修改而不影响 <see cref="Points"/>，且 null 或空列表会抛出异常。
|
||||
/// </summary>
|
||||
public EmTrajectory(EmTrajectoryMetadata metadata, IReadOnlyList<EmTrajectoryPoint> points)
|
||||
{
|
||||
if (metadata == null)
|
||||
@@ -27,7 +33,13 @@ public sealed class EmTrajectory
|
||||
Points = new ReadOnlyCollection<EmTrajectoryPoint>(copy);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 轨迹的不可变发布元数据引用；构造时必须非 null，未在本类中深拷贝。
|
||||
/// </summary>
|
||||
public EmTrajectoryMetadata Metadata { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 按调用方提供顺序保存的只读点列表；列表容器为构造时复制的快照，至少包含一个非 null 点，时间单调性由上游验证保证而非本构造器检查。
|
||||
/// </summary>
|
||||
public IReadOnlyList<EmTrajectoryPoint> Points { get; }
|
||||
}
|
||||
|
||||
@@ -3,8 +3,14 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 轨迹发布身份、来源版本、有效时间、方向段和终端边界的不可变元数据。
|
||||
/// </summary>
|
||||
public sealed class EmTrajectoryMetadata
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建轨迹的发布和来源元数据。验证标识、序列号、索引和枚举值；<paramref name="previousTrajectoryId"/> 可为 null 并会规范化为空字符串，两个 UTC 时刻的先后关系不在此处验证。
|
||||
/// </summary>
|
||||
public EmTrajectoryMetadata(
|
||||
string trajectoryId,
|
||||
DateTimeOffset generatedAtUtc,
|
||||
@@ -52,16 +58,63 @@ public sealed class EmTrajectoryMetadata
|
||||
PlanningScope = planningScope;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 非空白的本次发布轨迹标识，由消费者用于去重、替换和追踪。
|
||||
/// </summary>
|
||||
public string TrajectoryId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 生成此元数据的 UTC 时刻；值原样保存，构造器不与生效时刻比较。
|
||||
/// </summary>
|
||||
public DateTimeOffset GeneratedAtUtc { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 轨迹计划开始生效的 UTC 时刻；执行消费者负责按其调度策略解释。
|
||||
/// </summary>
|
||||
public DateTimeOffset EffectiveAtUtc { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 地图快照的非负版本标识；用于确认轨迹依赖的环境版本。
|
||||
/// </summary>
|
||||
public long MapSnapshotId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 非空白的输入参考路径标识；用于关联轨迹的几何来源。
|
||||
/// </summary>
|
||||
public string ReferencePathId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 车辆状态的非负序列版本；用于判断轨迹是否基于当前状态快照。
|
||||
/// </summary>
|
||||
public long VehicleStateSequenceId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 前一轨迹的可选标识；null 输入已规范化为空字符串，空字符串表示没有可关联的前轨迹标识。
|
||||
/// </summary>
|
||||
public string PreviousTrajectoryId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 参考路径中此轨迹所属的非负方向段索引。
|
||||
/// </summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 所属方向段的已验证行驶方向。
|
||||
/// </summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 轨迹最后一个终端的已验证业务类型。
|
||||
/// </summary>
|
||||
public EmTerminalType TerminalType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 生成轨迹时采用的已验证纵向终端速度语义。
|
||||
/// </summary>
|
||||
public EmLongitudinalMode LongitudinalMode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 生成轨迹时采用的已验证规划覆盖范围。
|
||||
/// </summary>
|
||||
public EmPlanningScope PlanningScope { get; }
|
||||
}
|
||||
|
||||
@@ -3,8 +3,14 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 轨迹中的单个时间样本。世界位置单位 m、航向单位 rad、速度单位 m/s、曲率单位 1/m。
|
||||
/// </summary>
|
||||
public sealed class EmTrajectoryPoint
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个轨迹时间样本。所有浮点输入必须有限；时间、段内弧长和路径弧长必须非负，段索引和枚举值必须有效。派生的绝对速度、世界速度分量和偏航角速度由有符号纵向速度、航向和曲率计算。
|
||||
/// </summary>
|
||||
public EmTrajectoryPoint(
|
||||
double x,
|
||||
double y,
|
||||
@@ -62,23 +68,88 @@ public sealed class EmTrajectoryPoint
|
||||
LongitudinalJerk = longitudinalJerk;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 世界坐标系 X 位置，单位 m。
|
||||
/// </summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 世界坐标系 Y 位置，单位 m。
|
||||
/// </summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 车辆在世界坐标系中的航向，单位 rad；构造器仅要求有限，不归一化角度。
|
||||
/// </summary>
|
||||
public double Yaw { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 沿车辆前向轴的有符号纵向速度，单位 m/s；符号表示前进或倒车。
|
||||
/// </summary>
|
||||
public double SignedLongitudinalVelocity { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 有符号纵向速度的绝对值，单位 m/s。
|
||||
/// </summary>
|
||||
public double Speed { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 由有符号纵向速度和航向导出的世界坐标 X 速度分量，单位 m/s。
|
||||
/// </summary>
|
||||
public double VelocityX { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 由有符号纵向速度和航向导出的世界坐标 Y 速度分量，单位 m/s。
|
||||
/// </summary>
|
||||
public double VelocityY { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 由有符号纵向速度乘车辆曲率导出的偏航角速度，单位 rad/s。
|
||||
/// </summary>
|
||||
public double YawRate { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 从本条轨迹开始执行起累计的非负时间，单位 s。
|
||||
/// </summary>
|
||||
public double TimeFromStart { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 车辆路径曲率，单位 1/m；符号约定由上游几何计算定义。
|
||||
/// </summary>
|
||||
public double VehicleCurvature { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 此点所属的非负方向段索引。
|
||||
/// </summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 从该方向段开始沿参考路径累计的非负弧长，单位 m。
|
||||
/// </summary>
|
||||
public double SegmentLocalS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 从整条参考路径开始累计的非负弧长，单位 m。
|
||||
/// </summary>
|
||||
public double PathS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 此点的已验证行驶方向。
|
||||
/// </summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 此点的已验证边界语义；普通内部点使用 <see cref="EmBoundaryType.None"/>。
|
||||
/// </summary>
|
||||
public EmBoundaryType BoundaryType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 沿车辆前向轴的纵向加速度，单位 m/s²；仅供同程序集的轨迹验证和执行逻辑读取。
|
||||
/// </summary>
|
||||
internal double LongitudinalAcceleration { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 沿车辆前向轴的纵向加加速度，单位 m/s³；仅供同程序集的轨迹验证和执行逻辑读取。
|
||||
/// </summary>
|
||||
internal double LongitudinalJerk { get; }
|
||||
}
|
||||
|
||||
@@ -3,8 +3,14 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 调用方一次性捕获的车辆运动状态；EMPlanner 只消费此快照，不直接读取定位、轮速或系统时钟。
|
||||
/// </summary>
|
||||
public sealed class VehicleMotionState
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存调用方捕获的车辆状态。构造器不校验 null、数值有限性、UTC 时效或序列号；服务在接受请求前负责验证，传入 <paramref name="pose"/> 的所有权仍归调用方。
|
||||
/// </summary>
|
||||
public VehicleMotionState(
|
||||
Pose2D pose,
|
||||
double signedLongitudinalSpeedMetersPerSecond,
|
||||
@@ -19,13 +25,28 @@ public sealed class VehicleMotionState
|
||||
SequenceId = sequenceId;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 车辆世界位姿引用，其中位置单位为 m、航向单位为 rad；构造器允许 null，但服务要求有效位姿。
|
||||
/// </summary>
|
||||
public Pose2D Pose { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 沿车辆前向轴的有符号纵向速度，单位 m/s；正负方向约定由上游状态生产者负责，服务要求其为有限数。
|
||||
/// </summary>
|
||||
public double SignedLongitudinalSpeedMetersPerSecond { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 可选的沿车辆前向轴纵向加速度，单位 m/s²；null 表示采集方未提供该量，非 null 值必须由服务验证为有限数。
|
||||
/// </summary>
|
||||
public double? LongitudinalAccelerationMetersPerSecondSquared { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 采集此状态的 UTC 时刻；服务用它相对请求发起时刻判断状态是否过期。
|
||||
/// </summary>
|
||||
public DateTimeOffset CapturedAtUtc { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 调用方提供的状态快照序列版本；服务要求其非负，成功轨迹将其写入元数据供消费者关联。
|
||||
/// </summary>
|
||||
public long SequenceId { get; }
|
||||
}
|
||||
|
||||
@@ -2,9 +2,17 @@ using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Connected free lateral interval at one exact reference-S station.</summary>
|
||||
/// <summary>
|
||||
/// 一个精确参考站 S 处与种子横向位置连通的自由横向区间。
|
||||
/// 参考站和横向偏移均以 m 计，横向正负号遵循该方向段的行驶坐标系；构造时拒绝非有限值、反向边界，或超出边界 1e-12 m 的种子。
|
||||
/// </summary>
|
||||
public sealed class LateralInterval
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个固定参考站上的闭合自由横向区间。
|
||||
/// 参数：referenceS、minimumL、maximumL 与 seedL 均为 m；seedL 必须在闭区间内（容许 1e-12 m 数值误差）。
|
||||
/// 返回：保存已验证边界的不可变区间；非法数值或不连通种子会引发 <see cref="ArgumentOutOfRangeException"/>。
|
||||
/// </summary>
|
||||
public LateralInterval(double referenceS, double minimumL, double maximumL, double seedL)
|
||||
{
|
||||
if (!IsFinite(referenceS) || !IsFinite(minimumL) || !IsFinite(maximumL) || !IsFinite(seedL) ||
|
||||
@@ -17,11 +25,30 @@ public sealed class LateralInterval
|
||||
SeedL = seedL;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 本区间所属方向段局部参考弧长 S，单位 m。
|
||||
/// </summary>
|
||||
public double ReferenceS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 可通行横向偏移闭区间的下界，单位 m。
|
||||
/// </summary>
|
||||
public double MinimumL { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 可通行横向偏移闭区间的上界，单位 m。
|
||||
/// </summary>
|
||||
public double MaximumL { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 用于保持拓扑连通性的种子横向偏移，单位 m。
|
||||
/// </summary>
|
||||
public double SeedL { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 判定标量能否参与走廊边界计算。
|
||||
/// 参数：value 为无单位或 m 制实数；返回：仅非 NaN 且非无穷大时为 true。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
@@ -4,9 +4,16 @@ using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable lateral hard bounds for one already-selected topological corridor.</summary>
|
||||
/// <summary>
|
||||
/// 一个已选择拓扑走廊在离散参考站上的不可变横向硬边界集合。
|
||||
/// 站点按方向段局部 S（m）非递减保存，调用方只能沿每个站点的种子连通自由区间优化。
|
||||
/// </summary>
|
||||
public sealed class StaticCorridor
|
||||
{
|
||||
/// <summary>
|
||||
/// 从站点序列创建走廊的防御性只读副本。
|
||||
/// 参数：stations 不能为空且至少含一个按 ReferenceS（m）非递减的非空区间；违反这些约束会被拒绝。
|
||||
/// </summary>
|
||||
public StaticCorridor(IReadOnlyList<LateralInterval> stations)
|
||||
{
|
||||
if (stations == null || stations.Count == 0)
|
||||
@@ -25,5 +32,8 @@ public sealed class StaticCorridor
|
||||
Stations = new ReadOnlyCollection<LateralInterval>(copy);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按方向段局部参考弧长 S（m）排序的横向硬边界只读列表。
|
||||
/// </summary>
|
||||
public IReadOnlyList<LateralInterval> Stations { get; }
|
||||
}
|
||||
|
||||
@@ -6,24 +6,49 @@ using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Builds only the seed-connected static free-space corridor of a direction segment.</summary>
|
||||
/// <summary>
|
||||
/// 为单一方向段构建仅与种子轨迹横向连通的静态自由空间走廊。
|
||||
/// 采样站和横向偏移使用 Frenet S/L（m），碰撞在世界 X/Y（m）车体几何中验证；任何无效、碰撞或断连站都会拒绝整个走廊。
|
||||
/// </summary>
|
||||
public sealed class StaticCorridorBuilder
|
||||
{
|
||||
/// <summary>
|
||||
/// 横向边界、采样去重和相邻区间重叠判定使用的数值容差，单位 m。
|
||||
/// </summary>
|
||||
private const double Epsilon = 1e-12d;
|
||||
|
||||
/// <summary>
|
||||
/// Frenet 重建时拒绝 1-κL 接近零的最小正分母，无量纲。
|
||||
/// </summary>
|
||||
private const double ReconstructionDenominator = 1e-12d;
|
||||
|
||||
/// <summary>
|
||||
/// 对未被保守净空快速放行的位置执行精确车体碰撞复核的依赖项。
|
||||
/// </summary>
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用默认连续车体碰撞检查器的静态走廊构建器。
|
||||
/// </summary>
|
||||
public StaticCorridorBuilder()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用指定车体碰撞检查器的静态走廊构建器。
|
||||
/// 参数：collisionChecker 不可为空；该检查器在世界坐标中按车辆尺寸和安全裕度验证采样位姿。
|
||||
/// </summary>
|
||||
public StaticCorridorBuilder(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从 S 起止锚点、种子轨迹和栅格地图构建种子连通的横向硬边界。
|
||||
/// 参数：startReferenceS/endReferenceS、种子 S/L 和配置采样尺度均为 m，map 使用世界 X/Y 栅格，vehicle 提供 m 制尺寸；区间限定在一个方向段内。
|
||||
/// 返回：每站均存在含种子的自由区间且相邻区间在 1e-12 m 内重叠时返回 true；否则 corridor 为 null 并写入拒绝原因。
|
||||
/// </summary>
|
||||
public bool TryBuild(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
|
||||
IReadOnlyList<FrenetProjection> seed, PlanningGridMap map, VehicleParameters vehicle,
|
||||
CorridorConfiguration configuration, out StaticCorridor corridor, out string failureReason)
|
||||
@@ -82,6 +107,10 @@ public sealed class StaticCorridorBuilder
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证构建走廊所需的段、S 范围、地图、车辆和采样配置。
|
||||
/// 参数：S 锚点和配置距离均为 m，车辆尺寸为 m；返回：地图未就绪、非有限量、越段锚点或非正采样尺度时为 false 并说明原因。
|
||||
/// </summary>
|
||||
private static bool TryValidateInput(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
|
||||
PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration, out string failureReason)
|
||||
{
|
||||
@@ -115,6 +144,10 @@ public sealed class StaticCorridorBuilder
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证并按局部参考 S 排序输入种子投影。
|
||||
/// 参数：input 可为 null（表示中心线 L=0 种子），各投影 S/L 为 m；返回：种子属于当前段且横向量有限时为 true，否则为 false。
|
||||
/// </summary>
|
||||
private static bool TryReadSeeds(IReadOnlyList<FrenetProjection> input, DirectionSegmentView segment,
|
||||
out List<SeedSample> seeds, out string failureReason)
|
||||
{
|
||||
@@ -139,6 +172,10 @@ public sealed class StaticCorridorBuilder
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成包含起止锚点的纵向采样站。
|
||||
/// 参数：起止 S 和 spacing 单位均为 m；返回：起点、严格内部等距站以及不同于起点超过 1e-12 m 的终点。
|
||||
/// </summary>
|
||||
private static IEnumerable<double> CreateStations(double startReferenceS, double endReferenceS, double spacing)
|
||||
{
|
||||
yield return startReferenceS;
|
||||
@@ -148,6 +185,10 @@ public sealed class StaticCorridorBuilder
|
||||
yield return endReferenceS;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 找出横向离散样本中包含种子的连续无碰撞区间。
|
||||
/// 参数：reference 为世界几何，seedL 及配置横向距离为 m；返回：种子样本自由时给出 [minimumL, maximumL]，碰撞或断连时返回 false。
|
||||
/// </summary>
|
||||
private bool TrySelectSeedConnectedInterval(FrenetReferencePoint reference, double seedL, PlanningGridMap map,
|
||||
VehicleParameters vehicle, CorridorConfiguration configuration, out double minimumL, out double maximumL)
|
||||
{
|
||||
@@ -190,6 +231,10 @@ public sealed class StaticCorridorBuilder
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在配置横向范围内生成有序且包含种子的唯一 L 采样值。
|
||||
/// 参数：seedL、maximumOffset 和 spacing 为 m；返回：包含两端和种子、以 1e-12 m 去重的升序列表。
|
||||
/// </summary>
|
||||
private static List<double> CreateLateralSamples(double seedL, double maximumOffset, double spacing)
|
||||
{
|
||||
var samples = new List<double>();
|
||||
@@ -202,6 +247,10 @@ public sealed class StaticCorridorBuilder
|
||||
return samples;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 重建给定 L 的世界车体位姿并检查其是否无碰撞。
|
||||
/// 参数：lateralOffset 为 m，reference 使用世界 X/Y（m）和 rad 航向，车辆与储备净空为 m；返回：重建奇异、越图或碰撞均为 false。
|
||||
/// </summary>
|
||||
private bool IsCollisionFree(FrenetReferencePoint reference, double lateralOffset, PlanningGridMap map,
|
||||
VehicleParameters vehicle, CorridorConfiguration configuration)
|
||||
{
|
||||
@@ -214,6 +263,10 @@ public sealed class StaticCorridorBuilder
|
||||
configuration.AdditionalClearanceReserveMeters, out _);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 用保守障碍距离和四个扩张车体角点快速确认明显净空。
|
||||
/// 参数：pose 为世界 X/Y（m）和 rad，车辆尺寸及额外储备为 m；返回：圆形下界安全且四角均在图内时为 true，否则交由精确检查器。
|
||||
/// </summary>
|
||||
private static bool IsObviouslyClear(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
|
||||
double additionalClearanceReserveMeters)
|
||||
{
|
||||
@@ -237,6 +290,10 @@ public sealed class StaticCorridorBuilder
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 以相邻种子 S 线性插值得到采样站的横向种子。
|
||||
/// 参数：seeds 已按局部 S（m）升序，referenceS 为 m；返回：区间外保持端点 L，重合 S 间隔不超过 1e-12 m 时取上端 L。
|
||||
/// </summary>
|
||||
private static double GetSeedL(IReadOnlyList<SeedSample> seeds, double referenceS)
|
||||
{
|
||||
if (seeds.Count == 0)
|
||||
@@ -257,6 +314,10 @@ public sealed class StaticCorridorBuilder
|
||||
return seeds[seeds.Count - 1].LateralOffset;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 向样本列表加入未在 1e-12 m 容差内出现过的横向值。
|
||||
/// 参数：samples 保存 m 制 L 值，value 为待加入 L（m）；返回：重复近似值被拒绝，唯一值追加后由调用方排序。
|
||||
/// </summary>
|
||||
private static void AddSortedUnique(List<double> samples, double value)
|
||||
{
|
||||
for (int index = 0; index < samples.Count; index++)
|
||||
@@ -265,25 +326,48 @@ public sealed class StaticCorridorBuilder
|
||||
samples.Add(value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判定采样步长或车辆尺寸是否为正的有限值。
|
||||
/// 参数：value 为对应单位的标量；返回：仅有限且严格大于零时为 true。
|
||||
/// </summary>
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0d;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判定走廊计算输入是否为有限实数。
|
||||
/// 参数：value 为任意标量；返回：NaN 和无穷时为 false。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 走廊种子在一个局部参考站上的轻量不可变记录。
|
||||
/// ReferenceS 与 LateralOffset 均以 m 计，仅用于站间种子插值。
|
||||
/// </summary>
|
||||
private sealed class SeedSample
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个种子记录。
|
||||
/// 参数：referenceS 为段局部 S（m），lateralOffset 为行驶坐标系中的 L（m）。
|
||||
/// </summary>
|
||||
public SeedSample(double referenceS, double lateralOffset)
|
||||
{
|
||||
ReferenceS = referenceS;
|
||||
LateralOffset = lateralOffset;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 种子所在方向段局部参考弧长 S，单位 m。
|
||||
/// </summary>
|
||||
public double ReferenceS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 种子相对中心线的横向偏移 L，单位 m。
|
||||
/// </summary>
|
||||
public double LateralOffset { get; }
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Globalization;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
@@ -9,28 +10,65 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
/// <summary>Runs one deterministic EM LS/ST planning pipeline and publishes only independently validated trajectories.</summary>
|
||||
public sealed class EmPlanningService : IEmPlanningService
|
||||
{
|
||||
private static readonly TimeSpan MaximumCycleDeadlineRemaining =
|
||||
TimeSpan.FromMilliseconds(int.MaxValue);
|
||||
private readonly IQpSolver qpSolver;
|
||||
private readonly IEmPlannerDebugSink defaultDebugSink;
|
||||
private readonly Func<TimeSpan> cycleElapsedForTesting;
|
||||
|
||||
public EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink = null)
|
||||
: this(qpSolver, defaultDebugSink, null)
|
||||
{
|
||||
}
|
||||
|
||||
internal EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink,
|
||||
Func<TimeSpan> cycleElapsedForTesting)
|
||||
{
|
||||
this.qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
|
||||
this.defaultDebugSink = defaultDebugSink;
|
||||
this.cycleElapsedForTesting = cycleElapsedForTesting;
|
||||
}
|
||||
|
||||
public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
Stopwatch cycleStopwatch = cycleElapsedForTesting == null ? Stopwatch.StartNew() : null;
|
||||
Func<TimeSpan> cycleElapsed = cycleElapsedForTesting ?? (() => cycleStopwatch.Elapsed);
|
||||
if (CallerCancellationRequested(request, cancellationToken))
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before request validation.");
|
||||
if (request?.CycleDeadlineRemaining is TimeSpan suppliedRemaining &&
|
||||
(suppliedRemaining < TimeSpan.Zero || suppliedRemaining > MaximumCycleDeadlineRemaining))
|
||||
{
|
||||
return Failure(EmPlanningStatus.InvalidInput, request,
|
||||
"CycleDeadlineRemaining must be between zero and " +
|
||||
MaximumCycleDeadlineRemaining.TotalMilliseconds + " milliseconds.");
|
||||
}
|
||||
if (DeadlineExpired(request, cycleElapsed))
|
||||
return CycleDeadlineFailure(request, "request", CycleRemaining(request, cycleElapsed));
|
||||
|
||||
EmPlanningRequestValidationResult requestValidation = EmPlanningRequestValidator.Validate(request);
|
||||
if (!requestValidation.IsValid)
|
||||
return Failure(requestValidation.Status, request, requestValidation.FailureReason);
|
||||
EmPlannerConfiguration configuration = requestValidation.Snapshot.Configuration;
|
||||
EmitDebug(request, "request/config validation succeeded");
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
||||
"request-validation", out EmPlanningResult validationFailure))
|
||||
{
|
||||
return validationFailure;
|
||||
}
|
||||
TimeSpan expirationRemaining = CycleRemaining(request, cycleElapsed);
|
||||
using var cycleExpiration = request.CycleDeadlineRemaining.HasValue
|
||||
? new CancellationTokenSource(expirationRemaining)
|
||||
: null;
|
||||
using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
||||
cancellationToken, request.CallerCancellationToken, request.CycleDeadlineToken,
|
||||
cycleExpiration?.Token ?? CancellationToken.None);
|
||||
CancellationToken planningCancellationToken = linkedCancellation.Token;
|
||||
|
||||
try
|
||||
{
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
||||
"projection", out EmPlanningResult deadlineFailure))
|
||||
return deadlineFailure;
|
||||
IReadOnlyList<DirectionSegmentView> segments = ReferencePathSegmenter.Create(request.ReferencePath);
|
||||
if (request.SegmentIndex < 0 || request.SegmentIndex >= segments.Count)
|
||||
return Failure(EmPlanningStatus.InvalidReferencePath, request, "The requested direction segment is unavailable.");
|
||||
@@ -59,7 +97,11 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
"Vehicle travel heading differs by at least 90 degrees from the selected direction segment.");
|
||||
}
|
||||
EmitDebug(request, "bounded ego projection succeeded");
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "projection", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
double initialProgressSpeed = Math.Abs(request.VehicleState.SignedLongitudinalSpeedMetersPerSecond);
|
||||
double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
|
||||
var horizonSelector = new PlanningHorizonSelector();
|
||||
@@ -84,18 +126,31 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
return Failure(EmPlanningStatus.CorridorInfeasible, request, corridorReason);
|
||||
}
|
||||
EmitDebug(request, "static connected corridor succeeded");
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
TimeSpan lateralBudget = SmallerBudget(
|
||||
TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds),
|
||||
CycleRemaining(request, cycleElapsed));
|
||||
EmPlannerConfiguration lateralConfiguration = configuration.Copy();
|
||||
lateralConfiguration.Scheduling.SolverTimeoutSeconds = lateralBudget.TotalSeconds;
|
||||
var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
|
||||
request.Vehicle, configuration, previousSeed);
|
||||
TimeSpan totalSolveBudget = TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds);
|
||||
request.Vehicle, lateralConfiguration, previousSeed);
|
||||
TimeSpan totalSolveBudget = lateralBudget;
|
||||
var solveBudgetStopwatch = Stopwatch.StartNew();
|
||||
LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
|
||||
LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, planningCancellationToken);
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
if (!IsSuccess(lateral.Status))
|
||||
return Failure(lateral.Status, request, lateral.FailureReason);
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after LS optimization.");
|
||||
EmitDebug(request, "LS optimization and validation succeeded");
|
||||
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
|
||||
initialProgressSpeed, initialAcceleration, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
|
||||
out string envelopeReason);
|
||||
@@ -119,9 +174,15 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
new LongitudinalPreviousTrajectorySeedBuilder().Build(
|
||||
request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
|
||||
segment.SegmentIndex, segment.Direction);
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
|
||||
remainingSolveBudget = SmallerBudget(remainingSolveBudget, CycleRemaining(request, cycleElapsed));
|
||||
if (remainingSolveBudget <= TimeSpan.Zero)
|
||||
{
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
||||
"longitudinal", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
return Failure(EmPlanningStatus.SolverTimedOut, request,
|
||||
"LS/ST optimization exhausted the shared solve budget before ST optimization.");
|
||||
}
|
||||
@@ -136,13 +197,22 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
return Failure(envelopeStatus, request, envelopeReason);
|
||||
EmitDebug(request, "PathS speed envelope succeeded");
|
||||
|
||||
LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(longitudinalInput, cancellationToken);
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
||||
"longitudinal", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(
|
||||
longitudinalInput, planningCancellationToken);
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
||||
"longitudinal", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
if (!IsSuccess(longitudinal.Status))
|
||||
return Failure(longitudinal.Status, request, longitudinal.FailureReason);
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after ST optimization.");
|
||||
EmitDebug(request, "ST optimization and validation succeeded");
|
||||
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
|
||||
request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
|
||||
segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode,
|
||||
@@ -151,10 +221,15 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
|
||||
if (assemblyStatus != EmPlanningStatus.Success)
|
||||
return Failure(assemblyStatus, request, assemblyFailure);
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
|
||||
EmitDebug(request, "trajectory assembly succeeded");
|
||||
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
||||
"world-validation", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
|
||||
Pose2D terminalPose = IsRealTerminalBoundary(terminalBoundary)
|
||||
? TerminalPose(lateral.Path)
|
||||
@@ -167,8 +242,13 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
return Failure(MapPublicationFailure(publication.Failure), request,
|
||||
publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
|
||||
}
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
||||
"world-validation", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
EmitDebug(request, "world-space publication validation succeeded");
|
||||
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before trajectory publication.");
|
||||
|
||||
@@ -176,11 +256,25 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
longitudinal.Status == EmPlanningStatus.SuccessWithFallback
|
||||
? EmPlanningStatus.SuccessWithFallback
|
||||
: EmPlanningStatus.Success;
|
||||
return new EmPlanningResult(finalStatus, trajectory,
|
||||
DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType + ";publication=validated");
|
||||
string publicationDiagnostic = DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType +
|
||||
";publication=validated";
|
||||
if (lateral.Status == EmPlanningStatus.SuccessWithFallback)
|
||||
publicationDiagnostic += ";lateralFallback=" + lateral.FailureReason;
|
||||
if (!string.IsNullOrWhiteSpace(longitudinal.FailureReason))
|
||||
{
|
||||
publicationDiagnostic += longitudinal.Status == EmPlanningStatus.SuccessWithFallback
|
||||
? ";longitudinalFallback=" + longitudinal.FailureReason
|
||||
: ";longitudinal=" + longitudinal.FailureReason;
|
||||
}
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
|
||||
return deadlineFailure;
|
||||
return new EmPlanningResult(finalStatus, trajectory, publicationDiagnostic);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
||||
"operation", out EmPlanningResult deadlineFailure))
|
||||
return deadlineFailure;
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled.");
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
@@ -193,6 +287,71 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
}
|
||||
}
|
||||
|
||||
private static bool TryTerminalFailure(EmPlanningRequest request, CancellationToken cancellationToken,
|
||||
Func<TimeSpan> elapsed, string phase, out EmPlanningResult failure)
|
||||
{
|
||||
if (CallerCancellationRequested(request, cancellationToken))
|
||||
{
|
||||
failure = Failure(EmPlanningStatus.Cancelled, request,
|
||||
"callerCancellation=true;phase=" + phase);
|
||||
return true;
|
||||
}
|
||||
TimeSpan remaining = CycleRemaining(request, elapsed);
|
||||
if (!DeadlineExpired(request, elapsed))
|
||||
{
|
||||
failure = null;
|
||||
return false;
|
||||
}
|
||||
failure = CycleDeadlineFailure(request, phase, remaining);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool CallerCancellationRequested(EmPlanningRequest request,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (request?.CallerCancellationToken.IsCancellationRequested == true)
|
||||
return true;
|
||||
if (!cancellationToken.IsCancellationRequested)
|
||||
return false;
|
||||
return request == null || !request.CycleDeadlineToken.IsCancellationRequested ||
|
||||
!request.CallerCancellationToken.CanBeCanceled;
|
||||
}
|
||||
|
||||
private static bool DeadlineExpired(EmPlanningRequest request, Func<TimeSpan> elapsed)
|
||||
{
|
||||
return request != null && (request.CycleDeadlineToken.IsCancellationRequested ||
|
||||
request.IsCycleDeadlineExpired() ||
|
||||
request.CycleDeadlineRemaining.HasValue && CycleRemaining(request, elapsed) <= TimeSpan.Zero);
|
||||
}
|
||||
|
||||
private static TimeSpan CycleRemaining(EmPlanningRequest request, Func<TimeSpan> elapsed)
|
||||
{
|
||||
TimeSpan snapshotRemaining = TimeSpan.MaxValue;
|
||||
if (request.CycleDeadlineRemaining.HasValue)
|
||||
{
|
||||
snapshotRemaining = request.CycleDeadlineRemaining.Value - elapsed();
|
||||
if (snapshotRemaining < TimeSpan.Zero)
|
||||
snapshotRemaining = TimeSpan.Zero;
|
||||
}
|
||||
TimeSpan? dynamicRemaining = request.DynamicCycleDeadlineRemaining();
|
||||
if (!dynamicRemaining.HasValue)
|
||||
return snapshotRemaining;
|
||||
return dynamicRemaining.Value < snapshotRemaining ? dynamicRemaining.Value : snapshotRemaining;
|
||||
}
|
||||
|
||||
private static TimeSpan SmallerBudget(TimeSpan configured, TimeSpan cycleRemaining)
|
||||
{
|
||||
return configured < cycleRemaining ? configured : cycleRemaining;
|
||||
}
|
||||
|
||||
private static EmPlanningResult CycleDeadlineFailure(EmPlanningRequest request, string phase,
|
||||
TimeSpan remaining)
|
||||
{
|
||||
return Failure(EmPlanningStatus.CycleDeadlineExpired, request,
|
||||
"cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
|
||||
remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
|
||||
}
|
||||
|
||||
private void EmitDebug(EmPlanningRequest request, string message)
|
||||
{
|
||||
if (defaultDebugSink == null || request == null || request.Configuration == null || request.Configuration.Solver == null ||
|
||||
|
||||
@@ -2,9 +2,17 @@ using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>World pose projected onto one bounded direction segment.</summary>
|
||||
/// <summary>
|
||||
/// 世界车辆位姿投影到单一受限方向段后的 Frenet 描述。
|
||||
/// S 与 L 单位均为 m，航向误差为 rad，距离以 m² 保存；对象不代表跨越换向边界的投影。
|
||||
/// </summary>
|
||||
public sealed class FrenetProjection
|
||||
{
|
||||
/// <summary>
|
||||
/// 从参考点和相对量创建不可变投影结果。
|
||||
/// 参数：lateralOffset 为沿行驶方向左法线的 L（m），headingError 为行驶航向差（rad），squaredDistanceMeters 为非负 m²。
|
||||
/// 返回：携带参考 S 的投影；空参考点、非有限值或负平方距离会被拒绝。
|
||||
/// </summary>
|
||||
public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
|
||||
double squaredDistanceMeters)
|
||||
{
|
||||
@@ -20,12 +28,35 @@ public sealed class FrenetProjection
|
||||
SquaredDistanceMeters = squaredDistanceMeters;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 投影命中的插值参考点，其坐标为世界 X/Y（m）。
|
||||
/// </summary>
|
||||
public FrenetReferencePoint ReferencePoint { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 命中点在当前方向段的局部参考弧长 S，单位 m。
|
||||
/// </summary>
|
||||
public double ReferenceS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 世界位姿相对参考行驶方向左法线的横向偏移 L，单位 m。
|
||||
/// </summary>
|
||||
public double LateralOffset { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 车辆行驶航向相对参考行驶航向的归一化误差，单位 rad。
|
||||
/// </summary>
|
||||
public double HeadingError { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 世界位置与命中参考位置的欧氏距离平方，单位 m²。
|
||||
/// </summary>
|
||||
public double SquaredDistanceMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 判定投影标量是否有限。
|
||||
/// 参数：value 为任意实数；返回：NaN 和正负无穷均返回 false。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
@@ -4,11 +4,21 @@ using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Deterministically projects a world pose only inside the supplied direction segment and S window.</summary>
|
||||
/// <summary>
|
||||
/// 在指定方向段及局部 S 窗口内确定性地投影世界位姿。
|
||||
/// 投影不跨越换向边界；世界 X/Y 与距离为 m，航向为 rad，并以种子 S 消除等距候选的拓扑歧义。
|
||||
/// </summary>
|
||||
public sealed class FrenetProjector
|
||||
{
|
||||
/// <summary>
|
||||
/// 等距候选比较和零长度线段识别使用的 S/距离平方数值容差 1e-14。
|
||||
/// </summary>
|
||||
private const double TieTolerance = 1e-14d;
|
||||
|
||||
/// <summary>
|
||||
/// 在窗口内投影世界位姿，并以窗口起点作为等距候选的种子 S。
|
||||
/// 参数：窗口与 maximumDistanceMeters 均为当前段局部 m 制距离；返回：命中距离不超过阈值时返回 true，否则 projection 为 null。
|
||||
/// </summary>
|
||||
public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
|
||||
double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection)
|
||||
{
|
||||
@@ -16,6 +26,10 @@ public sealed class FrenetProjector
|
||||
minimumReferenceS, out projection);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
|
||||
/// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
|
||||
/// </summary>
|
||||
public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
|
||||
double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection)
|
||||
{
|
||||
@@ -63,6 +77,10 @@ public sealed class FrenetProjector
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 考察被 S 窗口裁剪后的参考折线边，并将世界点的正交投影加入最优候选比较。
|
||||
/// 参数：端点为世界 X/Y（m）和局部 S（m），seedReferenceS 为 m；零长度（不超过 1e-14）边退化为两个端点比较。
|
||||
/// </summary>
|
||||
private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end,
|
||||
double seedReferenceS, ref Candidate best)
|
||||
{
|
||||
@@ -84,12 +102,20 @@ public sealed class FrenetProjector
|
||||
Consider(worldPose, referenceS, x, y, seedReferenceS, ref best);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将一个离散参考点作为投影候选参与比较。
|
||||
/// 参数：point 的位置为世界 m 制坐标，seedReferenceS 为局部 m 制种子；结果通过 best 原位更新。
|
||||
/// </summary>
|
||||
private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS,
|
||||
ref Candidate best)
|
||||
{
|
||||
Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 依据世界平面平方距离登记候选，并按确定性优先级替换当前最佳值。
|
||||
/// 参数：referenceS、x、y 与 seedReferenceS 分别为局部 S（m）和世界坐标（m）；平方距离由内部计算，单位 m²。
|
||||
/// </summary>
|
||||
private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS,
|
||||
ref Candidate best)
|
||||
{
|
||||
@@ -101,13 +127,25 @@ public sealed class FrenetProjector
|
||||
best = candidate;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判定投影计算的标量是否有限。
|
||||
/// 参数：value 为任意实数；返回：NaN 与无穷均返回 false。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 保存一个待比较的折线投影候选。
|
||||
/// ReferenceS 和 SeedDistance 使用 m，SquaredDistanceMeters 使用 m²，优先级由距离、种子距离及较小 S 依次决定。
|
||||
/// </summary>
|
||||
private sealed class Candidate
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建投影候选。
|
||||
/// 参数：referenceS 与 seedDistance 为 m，squaredDistanceMeters 为 m²；调用方仅传入有限的已计算值。
|
||||
/// </summary>
|
||||
public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance)
|
||||
{
|
||||
ReferenceS = referenceS;
|
||||
@@ -115,10 +153,25 @@ public sealed class FrenetProjector
|
||||
SeedDistance = seedDistance;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 候选在当前方向段的局部参考弧长 S，单位 m。
|
||||
/// </summary>
|
||||
public double ReferenceS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 候选世界位置到车辆位置的距离平方，单位 m²。
|
||||
/// </summary>
|
||||
public double SquaredDistanceMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 候选 S 与调用方种子 S 的绝对距离，单位 m。
|
||||
/// </summary>
|
||||
public double SeedDistance { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 比较两个候选的稳定优先级。
|
||||
/// 参数：other 为非空候选；返回：平方距离差超过 1e-14 时取较小者，随后取较近种子，仍相等时取较小 S。
|
||||
/// </summary>
|
||||
public bool IsPreferredTo(Candidate other)
|
||||
{
|
||||
if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true;
|
||||
@@ -129,3 +182,7 @@ public sealed class FrenetProjector
|
||||
}
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
|
||||
/// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
|
||||
/// </summary>
|
||||
|
||||
@@ -4,9 +4,17 @@ using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable interpolated reference sample in a single direction segment.</summary>
|
||||
/// <summary>
|
||||
/// 单一方向段内的不可变插值参考样本，连接世界几何与 Frenet 坐标。
|
||||
/// X/Y 与参考 S 为 m，航向为 rad，曲率为 1/m、曲率导数为 1/m²；倒车段仍以车辆航向保存几何。
|
||||
/// </summary>
|
||||
public sealed class FrenetReferencePoint
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建已验证的方向段参考样本。
|
||||
/// 参数：位置采用世界 X/Y（m），referenceS 为段局部弧长（m），航向为 rad，曲率量遵循 1/m 与 1/m²；所有数值必须有限。
|
||||
/// 返回：车辆航向会规范到 [-π, π]；任一非有限输入会引发 <see cref="ArgumentOutOfRangeException"/>。
|
||||
/// </summary>
|
||||
public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
|
||||
TravelDirection direction, double geometricCurvature, double vehicleCurvature,
|
||||
double vehicleCurvatureDerivative, double bodyClearance)
|
||||
@@ -33,23 +41,69 @@ public sealed class FrenetReferencePoint
|
||||
BodyClearance = bodyClearance;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 样本在当前方向段的局部参考弧长 S，单位 m。
|
||||
/// </summary>
|
||||
public double ReferenceS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 参考中心线点的世界 X 坐标，单位 m。
|
||||
/// </summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 参考中心线点的世界 Y 坐标，单位 m。
|
||||
/// </summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 归一化后的车辆车身航向，单位 rad。
|
||||
/// </summary>
|
||||
public double VehicleYaw { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 连续展开的车辆车身航向，单位 rad，供跨点几何插值使用。
|
||||
/// </summary>
|
||||
public double UnwrappedVehicleYaw { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 该参考样本的行驶方向，决定 Frenet 横向正负号和行驶航向。
|
||||
/// </summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 中心线几何曲率，单位 1/m。
|
||||
/// </summary>
|
||||
public double GeometricCurvature { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 满足车辆模型后的车辆曲率，单位 1/m。
|
||||
/// </summary>
|
||||
public double VehicleCurvature { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 车辆曲率相对弧长的导数，单位 1/m²。
|
||||
/// </summary>
|
||||
public double VehicleCurvatureDerivative { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 参考点记录的车体净空或可用裕度，单位 m。
|
||||
/// </summary>
|
||||
public double BodyClearance { get; }
|
||||
|
||||
/// <summary>Unwrapped direction of travel, used internally for geometry.</summary>
|
||||
/// <summary>
|
||||
/// 获取与当前行驶方向一致的连续航向。
|
||||
/// 返回：前进时为车辆展开航向，倒车时加 π；单位 rad，仅供 Frenet 几何计算，不重新归一化。
|
||||
/// </summary>
|
||||
public double TravelYaw
|
||||
{
|
||||
get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 拒绝不能安全保存为参考几何的数值。
|
||||
/// 参数：value 是待验证的任意单位标量，name 是异常参数名；NaN 或无穷会引发 <see cref="ArgumentOutOfRangeException"/>。
|
||||
/// </summary>
|
||||
private static void RequireFinite(double value, string name)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
|
||||
@@ -4,9 +4,17 @@ using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Coordinate conversion that keeps Frenet lateral sign relative to travel direction.</summary>
|
||||
/// <summary>
|
||||
/// 在世界坐标与 Frenet 横向约定间转换，并始终以实际行驶方向确定 L 的正负。
|
||||
/// 世界位置使用 X/Y（m）、航向使用 rad；倒车时车辆车身航向与行驶航向相差 π。
|
||||
/// </summary>
|
||||
public static class FrenetTransform
|
||||
{
|
||||
/// <summary>
|
||||
/// 将参考点及其横向状态重建为世界车辆中心位姿。
|
||||
/// 参数：lateralOffset 为 L（m），lateralDerivative 为 dL/dS（无量纲），minimumFrenetDenominator 为正的奇异性下界；参考点采用世界 X/Y（m）与 rad 航向。
|
||||
/// 返回：当 1-κL 有限且不小于下界、重建位姿有限时返回 true；空参考点、非法输入或接近 Frenet 奇异点时返回 false。
|
||||
/// </summary>
|
||||
public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
|
||||
double minimumFrenetDenominator, out Pose2D pose)
|
||||
{
|
||||
@@ -33,11 +41,19 @@ public static class FrenetTransform
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从车辆车身航向取得对应实际行驶的连续航向。
|
||||
/// 参数：vehicleYaw 为 rad，direction 指定前进或倒车；返回：前进原样、倒车加 π，结果不归一化。
|
||||
/// </summary>
|
||||
internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判定重建中间量是否为有限实数。
|
||||
/// 参数：value 为任意标量；返回：NaN 或正负无穷时为 false。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
@@ -4,11 +4,22 @@ using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Interpolates reference geometry inside exactly one direction segment.</summary>
|
||||
/// <summary>
|
||||
/// 在恰好一个方向段内按局部参考弧长插值参考几何。
|
||||
/// 输入和输出 S、X/Y 与净空均为 m，航向为 rad，曲率为 1/m；插值不会越过方向段边界。
|
||||
/// </summary>
|
||||
public static class ReferencePathInterpolator
|
||||
{
|
||||
/// <summary>
|
||||
/// 接受端点轻微 S 超差并判断精确节点的数值容差，单位 m。
|
||||
/// </summary>
|
||||
private const double Epsilon = 1e-12d;
|
||||
|
||||
/// <summary>
|
||||
/// 为给定局部 S 生成方向一致的 Frenet 参考样本。
|
||||
/// 参数：segment 不可为空，referenceS 为 m；[-1e-12, Length+1e-12] 内的端点超差会夹紧到边界。
|
||||
/// 返回：精确点直接转换，区间内对位置、展开航向和几何量线性插值；超界或退化跨度会引发异常。
|
||||
/// </summary>
|
||||
public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
|
||||
{
|
||||
if (segment == null)
|
||||
@@ -45,6 +56,10 @@ public static class ReferencePathInterpolator
|
||||
return FromPoint(segment.Points[segment.Points.Count - 1]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将原始平滑路径点转换为同一局部 S 的 Frenet 参考样本。
|
||||
/// 参数：point 已携带世界 X/Y（m）、航向（rad）和曲率量；返回：保持其方向及所有几何量的不可变副本。
|
||||
/// </summary>
|
||||
private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
|
||||
{
|
||||
return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
|
||||
@@ -52,11 +67,19 @@ public static class ReferencePathInterpolator
|
||||
point.BodyClearance);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在线性标量区间内计算插值值。
|
||||
/// 参数：lower、upper 为同单位端点，fraction 为无单位比例；返回：同单位的未夹紧线性结果。
|
||||
/// </summary>
|
||||
private static double Lerp(double lower, double upper, double fraction)
|
||||
{
|
||||
return lower + (upper - lower) * fraction;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判定插值输入是否为有限实数。
|
||||
/// 参数：value 为任意标量；返回：NaN 或无穷时为 false。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
+93
-10
@@ -1,5 +1,6 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
@@ -16,7 +17,15 @@ public sealed class LongitudinalConstraintBuilder
|
||||
public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
|
||||
out QuadraticProgram problem, out string failureReason)
|
||||
{
|
||||
return TryBuildCore(input, speedLimit, iterate, false, out problem, out failureReason);
|
||||
return TryBuildCore(input, speedLimit, iterate, false, null, null, 0d, out problem, out failureReason);
|
||||
}
|
||||
|
||||
internal bool TryBuildTrusted(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
||||
LongitudinalCandidate anchor, LongitudinalEnvelopeTrustRegion trustRegion, double strictTolerance,
|
||||
out QuadraticProgram problem, out string failureReason)
|
||||
{
|
||||
return TryBuildCore(input, speedLimit, anchor, false, trustRegion, anchor, strictTolerance,
|
||||
out problem, out failureReason);
|
||||
}
|
||||
|
||||
/// <summary>Builds the bounded full-scope feasibility projection before objective optimization.</summary>
|
||||
@@ -38,12 +47,14 @@ public sealed class LongitudinalConstraintBuilder
|
||||
failureReason = "Initial feasibility projection is only defined for full-direction exact-stop planning.";
|
||||
return false;
|
||||
}
|
||||
return TryBuildCore(input, speedLimit, linearizationIterate, true, out problem,
|
||||
out failureReason);
|
||||
return TryBuildCore(input, speedLimit, linearizationIterate, true, null, null, 0d,
|
||||
out problem, out failureReason);
|
||||
}
|
||||
|
||||
private bool TryBuildCore(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
|
||||
bool useScheduleReferenceObjective, out QuadraticProgram problem, out string failureReason)
|
||||
bool useScheduleReferenceObjective, LongitudinalEnvelopeTrustRegion trustRegion,
|
||||
LongitudinalCandidate trustedAnchor, double strictTolerance, out QuadraticProgram problem,
|
||||
out string failureReason)
|
||||
{
|
||||
problem = null;
|
||||
failureReason = string.Empty;
|
||||
@@ -63,6 +74,15 @@ public sealed class LongitudinalConstraintBuilder
|
||||
{
|
||||
throw new ArgumentException("The ST iterate does not match the configured knot layout.");
|
||||
}
|
||||
if ((trustRegion == null) != (trustedAnchor == null))
|
||||
throw new ArgumentException("Trusted QP construction requires both a trust region and anchor.");
|
||||
if (trustRegion != null && (trustRegion.MinimumPathS.Count != layout.KnotCount ||
|
||||
trustRegion.MaximumPathS.Count != layout.KnotCount ||
|
||||
trustRegion.SpeedSlope.Count != layout.KnotCount ||
|
||||
trustRegion.SpeedIntercept.Count != layout.KnotCount))
|
||||
{
|
||||
throw new ArgumentException("The trust region does not match the configured knot layout.");
|
||||
}
|
||||
if (!PathSpeedLimitBuilder.TryGetLimits(input, out double directionMaximum, out double maximumAcceleration,
|
||||
out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
|
||||
{
|
||||
@@ -84,12 +104,14 @@ public sealed class LongitudinalConstraintBuilder
|
||||
_objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
|
||||
int stabilizationStart = GetStabilizationStart(input, expectedTimes, layout.KnotCount);
|
||||
int stationaryKnotCount = layout.KnotCount - stabilizationStart;
|
||||
int expectedRows = 9 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
|
||||
int expectedRows = 10 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
|
||||
var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
|
||||
var lower = new List<double>(expectedRows);
|
||||
var upper = new List<double>(expectedRows);
|
||||
int row = 0;
|
||||
AddVariableBounds(input, speedLimit, iterate, layout, maximumAcceleration, maximumDeceleration, maximumJerk,
|
||||
AddVariableBounds(input, speedLimit, iterate, trustRegion, layout, maximumAcceleration,
|
||||
maximumDeceleration, maximumJerk, constraints, lower, upper, ref row);
|
||||
AddLowSpeedDecelerationReleaseEnvelope(layout, iterate, maximumJerk,
|
||||
constraints, lower, upper, ref row);
|
||||
AddMonotonicProgress(layout, constraints, lower, upper, ref row);
|
||||
AddExactDynamics(expectedTimes, layout, constraints, lower, upper, ref row);
|
||||
@@ -99,6 +121,20 @@ public sealed class LongitudinalConstraintBuilder
|
||||
if (row != expectedRows)
|
||||
throw new InvalidOperationException("ST constraint row accounting is inconsistent.");
|
||||
problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
|
||||
if (trustedAnchor != null)
|
||||
{
|
||||
LongitudinalQpAuditResult audit = LongitudinalQpFeasibilityAudit.Evaluate(problem, trustedAnchor,
|
||||
strictTolerance, layout, stabilizationStart);
|
||||
if (!audit.IsFeasible)
|
||||
{
|
||||
problem = null;
|
||||
failureReason = "Planner invariant failure: strict anchor is outside trusted QP" +
|
||||
";row=" + audit.WorstRow + ";category=" + audit.Category +
|
||||
";residual=" + audit.MaximumResidual.ToString("R", CultureInfo.InvariantCulture) +
|
||||
audit.Unit + ";tolerance=" + strictTolerance.ToString("R", CultureInfo.InvariantCulture);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
@@ -145,7 +181,8 @@ public sealed class LongitudinalConstraintBuilder
|
||||
}
|
||||
|
||||
private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
||||
LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration,
|
||||
LongitudinalCandidate iterate, LongitudinalEnvelopeTrustRegion trustRegion,
|
||||
LongitudinalVariableLayout layout, double maximumAcceleration,
|
||||
double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList<double> lower,
|
||||
IList<double> upper, ref int row)
|
||||
{
|
||||
@@ -153,14 +190,32 @@ public sealed class LongitudinalConstraintBuilder
|
||||
{
|
||||
if (iterate.S[index] < 0d || iterate.S[index] > input.PathUpperBoundS)
|
||||
throw new ArgumentException("The ST iterate progress lies outside actual PathS bounds.");
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
|
||||
if (trustRegion == null)
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
|
||||
else
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.S(index),
|
||||
trustRegion.MinimumPathS[index], trustRegion.MaximumPathS[index], ref row);
|
||||
double maximumSpeed = index == 0
|
||||
? input.DirectionMaximumSpeedMetersPerSecond
|
||||
: input.DirectionMaximumSpeedMetersPerSecond;
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, maximumSpeed, ref row);
|
||||
if (index > 0)
|
||||
AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
|
||||
constraints, lower, upper, ref row);
|
||||
{
|
||||
if (trustRegion == null)
|
||||
{
|
||||
AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
|
||||
constraints, lower, upper, ref row);
|
||||
}
|
||||
else
|
||||
{
|
||||
AddRow(constraints, lower, upper, row, new[]
|
||||
{
|
||||
new Coefficient(layout.U(index), 1d),
|
||||
new Coefficient(layout.S(index), -trustRegion.SpeedSlope[index]),
|
||||
}, -QuadraticProgram.MaximumFiniteBound, trustRegion.SpeedIntercept[index]);
|
||||
row++;
|
||||
}
|
||||
}
|
||||
AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
|
||||
ref row);
|
||||
}
|
||||
@@ -209,6 +264,34 @@ public sealed class LongitudinalConstraintBuilder
|
||||
}
|
||||
}
|
||||
|
||||
internal static void CalculateLowSpeedDecelerationReleaseTangent(
|
||||
double anchorAcceleration, double maximumJerk,
|
||||
out double accelerationCoefficient, out double lowerBound)
|
||||
{
|
||||
if (!IsFinite(anchorAcceleration) || !IsFinite(maximumJerk) || maximumJerk <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(anchorAcceleration));
|
||||
double a0 = Math.Min(0d, anchorAcceleration);
|
||||
accelerationCoefficient = -a0 / maximumJerk;
|
||||
lowerBound = -(a0 * a0) / (2d * maximumJerk);
|
||||
}
|
||||
|
||||
private static void AddLowSpeedDecelerationReleaseEnvelope(LongitudinalVariableLayout layout,
|
||||
LongitudinalCandidate iterate, double maximumJerk, SparseTripletBuilder constraints,
|
||||
IList<double> lower, IList<double> upper, ref int row)
|
||||
{
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
CalculateLowSpeedDecelerationReleaseTangent(iterate.A[index], maximumJerk,
|
||||
out double accelerationCoefficient, out double lowerBound);
|
||||
AddRow(constraints, lower, upper, row, new[]
|
||||
{
|
||||
new Coefficient(layout.U(index), 1d),
|
||||
new Coefficient(layout.A(index), accelerationCoefficient),
|
||||
}, lowerBound, QuadraticProgram.MaximumFiniteBound);
|
||||
row++;
|
||||
}
|
||||
}
|
||||
|
||||
private static void AddExactDynamics(IReadOnlyList<double> times, LongitudinalVariableLayout layout,
|
||||
SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
|
||||
{
|
||||
|
||||
+136
@@ -0,0 +1,136 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Proves the constant-jerk profile used by publication is continuously forward-progressing.</summary>
|
||||
internal static class LongitudinalContinuousProfileValidator
|
||||
{
|
||||
public static bool TryValidate(LongitudinalCandidate candidate, double tolerance, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
if (candidate == null)
|
||||
{
|
||||
failureReason = "A longitudinal candidate is required.";
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(tolerance) || tolerance < 0d)
|
||||
{
|
||||
failureReason = "A finite nonnegative continuous-profile tolerance is required.";
|
||||
return false;
|
||||
}
|
||||
|
||||
double highWater = candidate.S[0];
|
||||
for (int interval = 0; interval < candidate.J.Count; interval++)
|
||||
{
|
||||
double duration = candidate.KnotTimes[interval + 1] - candidate.KnotTimes[interval];
|
||||
double initialS = candidate.S[interval];
|
||||
double initialU = candidate.U[interval];
|
||||
double initialA = candidate.A[interval];
|
||||
double jerk = candidate.J[interval];
|
||||
var evaluationTimes = new List<double>(5) { 0d, duration };
|
||||
if (jerk != 0d)
|
||||
AddIfInside(evaluationTimes, -initialA / jerk, duration);
|
||||
AddSpeedRoots(evaluationTimes, initialU, initialA, jerk, duration);
|
||||
evaluationTimes.Sort();
|
||||
|
||||
double previousTime = double.NegativeInfinity;
|
||||
for (int point = 0; point < evaluationTimes.Count; point++)
|
||||
{
|
||||
double localTime = evaluationTimes[point];
|
||||
if (localTime == previousTime)
|
||||
continue;
|
||||
previousTime = localTime;
|
||||
Evaluate(initialS, initialU, initialA, jerk, localTime,
|
||||
out double progress, out double speed, out double acceleration);
|
||||
double regression = highWater - progress;
|
||||
if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) ||
|
||||
speed < -tolerance || regression > tolerance)
|
||||
{
|
||||
string kind = !IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration)
|
||||
? "non-finite"
|
||||
: speed < -tolerance ? "negative-speed" : "PathS-regression";
|
||||
failureReason = FormatFailure(kind, interval, localTime, progress, speed,
|
||||
acceleration, jerk, regression, string.Empty);
|
||||
return false;
|
||||
}
|
||||
if (progress > highWater)
|
||||
highWater = progress;
|
||||
}
|
||||
|
||||
Evaluate(initialS, initialU, initialA, jerk, duration,
|
||||
out double integratedS, out double integratedU, out double integratedA);
|
||||
double sMismatch = Math.Abs(integratedS - candidate.S[interval + 1]);
|
||||
double uMismatch = Math.Abs(integratedU - candidate.U[interval + 1]);
|
||||
double aMismatch = Math.Abs(integratedA - candidate.A[interval + 1]);
|
||||
if (!IsFinite(sMismatch) || !IsFinite(uMismatch) || !IsFinite(aMismatch) ||
|
||||
sMismatch > tolerance || uMismatch > tolerance || aMismatch > tolerance)
|
||||
{
|
||||
string detail = ";endpointMismatchS=" + Invariant(sMismatch) +
|
||||
";endpointMismatchU=" + Invariant(uMismatch) +
|
||||
";endpointMismatchA=" + Invariant(aMismatch);
|
||||
failureReason = FormatFailure("endpoint-mismatch", interval, duration, integratedS,
|
||||
integratedU, integratedA, jerk, highWater - integratedS, detail);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void AddSpeedRoots(ICollection<double> times, double initialU, double initialA,
|
||||
double jerk, double duration)
|
||||
{
|
||||
if (jerk == 0d)
|
||||
{
|
||||
if (initialA != 0d)
|
||||
AddIfInside(times, -initialU / initialA, duration);
|
||||
return;
|
||||
}
|
||||
|
||||
double discriminant = initialA * initialA - 2d * jerk * initialU;
|
||||
if (!IsFinite(discriminant) || discriminant < 0d)
|
||||
return;
|
||||
double rootTerm = Math.Sqrt(discriminant);
|
||||
AddIfInside(times, (-initialA - rootTerm) / jerk, duration);
|
||||
AddIfInside(times, (-initialA + rootTerm) / jerk, duration);
|
||||
}
|
||||
|
||||
private static void AddIfInside(ICollection<double> times, double localTime, double duration)
|
||||
{
|
||||
if (IsFinite(localTime) && localTime >= 0d && localTime <= duration)
|
||||
times.Add(localTime);
|
||||
}
|
||||
|
||||
private static void Evaluate(double initialS, double initialU, double initialA, double jerk,
|
||||
double localTime, out double progress, out double speed, out double acceleration)
|
||||
{
|
||||
acceleration = initialA + jerk * localTime;
|
||||
speed = initialU + initialA * localTime + 0.5d * jerk * localTime * localTime;
|
||||
progress = initialS + initialU * localTime + 0.5d * initialA * localTime * localTime +
|
||||
jerk * localTime * localTime * localTime / 6d;
|
||||
}
|
||||
|
||||
private static string FormatFailure(string kind, int interval, double localTime, double progress,
|
||||
double speed, double acceleration, double jerk, double regression, string detail)
|
||||
{
|
||||
return "Continuous ST profile rejected: kind=" + kind +
|
||||
";interval=" + interval.ToString(CultureInfo.InvariantCulture) +
|
||||
";localTime=" + Invariant(localTime) +
|
||||
";S=" + Invariant(progress) +
|
||||
";U=" + Invariant(speed) +
|
||||
";A=" + Invariant(acceleration) +
|
||||
";J=" + Invariant(jerk) +
|
||||
";regression=" + Invariant(regression) + detail + ".";
|
||||
}
|
||||
|
||||
private static string Invariant(double value)
|
||||
{
|
||||
return value.ToString("R", CultureInfo.InvariantCulture);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
+274
@@ -0,0 +1,274 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal sealed class LongitudinalEnvelopeTrustRegion
|
||||
{
|
||||
internal LongitudinalEnvelopeTrustRegion(IReadOnlyList<double> minimumPathS, IReadOnlyList<double> maximumPathS,
|
||||
IReadOnlyList<double> speedSlope, IReadOnlyList<double> speedIntercept,
|
||||
IReadOnlyList<int> envelopeSegmentIndex, double scale)
|
||||
{
|
||||
MinimumPathS = Copy(minimumPathS, nameof(minimumPathS));
|
||||
MaximumPathS = Copy(maximumPathS, nameof(maximumPathS));
|
||||
SpeedSlope = Copy(speedSlope, nameof(speedSlope));
|
||||
SpeedIntercept = Copy(speedIntercept, nameof(speedIntercept));
|
||||
EnvelopeSegmentIndex = Copy(envelopeSegmentIndex, nameof(envelopeSegmentIndex));
|
||||
Scale = scale;
|
||||
}
|
||||
|
||||
internal IReadOnlyList<double> MinimumPathS { get; }
|
||||
|
||||
internal IReadOnlyList<double> MaximumPathS { get; }
|
||||
|
||||
internal IReadOnlyList<double> SpeedSlope { get; }
|
||||
|
||||
internal IReadOnlyList<double> SpeedIntercept { get; }
|
||||
|
||||
internal IReadOnlyList<int> EnvelopeSegmentIndex { get; }
|
||||
|
||||
internal double Scale { get; }
|
||||
|
||||
internal bool CanShrinkTo(double nextScale, double minimumActiveWidthMeters, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
if (!IsFinite(nextScale) || nextScale <= 0d || nextScale >= Scale)
|
||||
{
|
||||
failureReason = "Next trust-region scale must be finite, positive, and smaller than the current scale.";
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
|
||||
{
|
||||
failureReason = "Minimum active width must be finite and positive.";
|
||||
return false;
|
||||
}
|
||||
double ratio = nextScale / Scale;
|
||||
for (int index = 0; index < MinimumPathS.Count; index++)
|
||||
{
|
||||
double currentWidth = MaximumPathS[index] - MinimumPathS[index];
|
||||
if (currentWidth > 0d && currentWidth * ratio < minimumActiveWidthMeters)
|
||||
{
|
||||
failureReason = "The next trust-region scale would fall below the minimum width at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> Copy(IReadOnlyList<double> source, string parameterName)
|
||||
{
|
||||
if (source == null)
|
||||
throw new ArgumentNullException(parameterName);
|
||||
var copy = new List<double>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<int> Copy(IReadOnlyList<int> source, string parameterName)
|
||||
{
|
||||
if (source == null)
|
||||
throw new ArgumentNullException(parameterName);
|
||||
var copy = new List<int>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<int>(copy);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class LongitudinalEnvelopeTrustRegionBuilder
|
||||
{
|
||||
private const double ScheduleProgressTolerance = 1e-12d;
|
||||
private const double StationSelectionTolerance = 1e-12d;
|
||||
|
||||
internal bool TryBuild(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
|
||||
IReadOnlyList<double> referencePathS, int terminalHoldStartIndex, double scale,
|
||||
double minimumActiveWidthMeters, out LongitudinalEnvelopeTrustRegion region,
|
||||
out string failureReason)
|
||||
{
|
||||
region = null;
|
||||
failureReason = string.Empty;
|
||||
if (speedLimit == null || anchor == null || referencePathS == null)
|
||||
{
|
||||
failureReason = "Trust region inputs must be present.";
|
||||
return false;
|
||||
}
|
||||
if (!IsSupportedScale(scale))
|
||||
{
|
||||
failureReason = "Trust-region scale must be one of 1, 0.5, 0.25, or 0.125.";
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
|
||||
{
|
||||
failureReason = "Minimum active width must be finite and positive.";
|
||||
return false;
|
||||
}
|
||||
if (referencePathS.Count != anchor.S.Count)
|
||||
{
|
||||
failureReason = "Reference PathS count must match the anchor knot count.";
|
||||
return false;
|
||||
}
|
||||
if (terminalHoldStartIndex == -1)
|
||||
terminalHoldStartIndex = anchor.S.Count;
|
||||
if (terminalHoldStartIndex < 1 || terminalHoldStartIndex > anchor.S.Count)
|
||||
{
|
||||
failureReason = "Terminal-hold start index is outside the anchor knot range.";
|
||||
return false;
|
||||
}
|
||||
if (!TryValidateAnchorAndReference(speedLimit, anchor, referencePathS, out failureReason))
|
||||
return false;
|
||||
|
||||
int knotCount = anchor.S.Count;
|
||||
var minimumPathS = new double[knotCount];
|
||||
var maximumPathS = new double[knotCount];
|
||||
var speedSlope = new double[knotCount];
|
||||
var speedIntercept = new double[knotCount];
|
||||
var segmentIndex = new int[knotCount];
|
||||
for (int index = 0; index < knotCount; index++)
|
||||
{
|
||||
double anchorS = anchor.S[index];
|
||||
bool fixedKnot = index == 0 || index >= terminalHoldStartIndex;
|
||||
int segment = SelectSegment(speedLimit, anchorS, referencePathS, index, fixedKnot);
|
||||
FindMaximalExactAffineRun(speedLimit, segment, out int firstSegment, out int lastSegment,
|
||||
out double slope, out double intercept);
|
||||
double lower = speedLimit.PathS[firstSegment];
|
||||
double upper = speedLimit.PathS[lastSegment + 1];
|
||||
if (anchorS < lower && lower - anchorS <= StationSelectionTolerance)
|
||||
lower = anchorS;
|
||||
if (anchorS > upper && anchorS - upper <= StationSelectionTolerance)
|
||||
upper = anchorS;
|
||||
double trustedLower = fixedKnot ? anchorS : anchorS - scale * (anchorS - lower);
|
||||
double trustedUpper = fixedKnot ? anchorS : anchorS + scale * (upper - anchorS);
|
||||
if (!fixedKnot && scale < 1d && trustedUpper - trustedLower < minimumActiveWidthMeters)
|
||||
{
|
||||
failureReason = "Active trust-region interval is narrower than the minimum width at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
|
||||
minimumPathS[index] = trustedLower;
|
||||
maximumPathS[index] = trustedUpper;
|
||||
speedSlope[index] = slope;
|
||||
speedIntercept[index] = intercept;
|
||||
segmentIndex[index] = segment;
|
||||
}
|
||||
|
||||
region = new LongitudinalEnvelopeTrustRegion(minimumPathS, maximumPathS, speedSlope, speedIntercept,
|
||||
segmentIndex, scale);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryValidateAnchorAndReference(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
|
||||
IReadOnlyList<double> referencePathS, out string failureReason)
|
||||
{
|
||||
if (anchor.S[0] != 0d)
|
||||
{
|
||||
failureReason = "The anchor must begin at exact PathS zero.";
|
||||
return false;
|
||||
}
|
||||
|
||||
double minimumPathS = speedLimit.PathS[0];
|
||||
double maximumPathS = speedLimit.PathS[speedLimit.PathS.Count - 1];
|
||||
double previousAnchorS = double.NegativeInfinity;
|
||||
for (int index = 0; index < anchor.S.Count; index++)
|
||||
{
|
||||
double anchorS = anchor.S[index];
|
||||
double referenceS = referencePathS[index];
|
||||
if (!IsFinite(anchorS) || anchorS < minimumPathS || anchorS > maximumPathS)
|
||||
{
|
||||
failureReason = "Anchor PathS is outside the speed-limit range at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
if (anchorS < previousAnchorS)
|
||||
{
|
||||
failureReason = "Anchor PathS must be nondecreasing.";
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(referenceS))
|
||||
{
|
||||
failureReason = "Reference PathS must be finite.";
|
||||
return false;
|
||||
}
|
||||
previousAnchorS = anchorS;
|
||||
}
|
||||
|
||||
failureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static int SelectSegment(PathSpeedLimit speedLimit, double anchorS, IReadOnlyList<double> referencePathS,
|
||||
int knotIndex, bool terminalHold)
|
||||
{
|
||||
int lastSegment = speedLimit.PathS.Count - 2;
|
||||
if (Math.Abs(anchorS - speedLimit.PathS[0]) <= StationSelectionTolerance)
|
||||
return 0;
|
||||
if (Math.Abs(anchorS - speedLimit.PathS[speedLimit.PathS.Count - 1]) <= StationSelectionTolerance)
|
||||
return lastSegment;
|
||||
for (int index = 1; index < speedLimit.PathS.Count - 1; index++)
|
||||
{
|
||||
if (Math.Abs(anchorS - speedLimit.PathS[index]) <= StationSelectionTolerance)
|
||||
{
|
||||
if (terminalHold)
|
||||
return index;
|
||||
double scheduleDelta = referencePathS[knotIndex] - referencePathS[knotIndex - 1];
|
||||
if (scheduleDelta > ScheduleProgressTolerance)
|
||||
return index;
|
||||
if (scheduleDelta < -ScheduleProgressTolerance)
|
||||
return index - 1;
|
||||
double leftWidth = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
|
||||
double rightWidth = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
|
||||
return rightWidth >= leftWidth ? index : index - 1;
|
||||
}
|
||||
if (anchorS < speedLimit.PathS[index])
|
||||
return index - 1;
|
||||
}
|
||||
return lastSegment;
|
||||
}
|
||||
|
||||
private static void GetAffineLine(PathSpeedLimit limit, int segment,
|
||||
out double slope, out double intercept)
|
||||
{
|
||||
double lower = limit.PathS[segment];
|
||||
double upper = limit.PathS[segment + 1];
|
||||
slope = (limit.MaximumSpeedMetersPerSecond[segment + 1] -
|
||||
limit.MaximumSpeedMetersPerSecond[segment]) / (upper - lower);
|
||||
intercept = limit.MaximumSpeedMetersPerSecond[segment] - slope * lower;
|
||||
}
|
||||
|
||||
private static void FindMaximalExactAffineRun(PathSpeedLimit limit, int selectedSegment,
|
||||
out int firstSegment, out int lastSegment, out double slope, out double intercept)
|
||||
{
|
||||
GetAffineLine(limit, selectedSegment, out slope, out intercept);
|
||||
firstSegment = selectedSegment;
|
||||
while (firstSegment > 0)
|
||||
{
|
||||
GetAffineLine(limit, firstSegment - 1, out double candidateSlope, out double candidateIntercept);
|
||||
if (candidateSlope != slope || candidateIntercept != intercept)
|
||||
break;
|
||||
firstSegment--;
|
||||
}
|
||||
lastSegment = selectedSegment;
|
||||
while (lastSegment < limit.PathS.Count - 2)
|
||||
{
|
||||
GetAffineLine(limit, lastSegment + 1, out double candidateSlope, out double candidateIntercept);
|
||||
if (candidateSlope != slope || candidateIntercept != intercept)
|
||||
break;
|
||||
lastSegment++;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool IsSupportedScale(double scale)
|
||||
{
|
||||
return scale == 1d || scale == 0.5d || scale == 0.25d || scale == 0.125d;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
+217
@@ -0,0 +1,217 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal sealed class LongitudinalQpAuditResult
|
||||
{
|
||||
internal LongitudinalQpAuditResult(bool isFeasible, double maximumResidual, int worstRow,
|
||||
string category, string unit)
|
||||
{
|
||||
IsFeasible = isFeasible;
|
||||
MaximumResidual = maximumResidual;
|
||||
WorstRow = worstRow;
|
||||
Category = category;
|
||||
Unit = unit;
|
||||
}
|
||||
|
||||
internal bool IsFeasible { get; }
|
||||
|
||||
internal double MaximumResidual { get; }
|
||||
|
||||
internal int WorstRow { get; }
|
||||
|
||||
internal string Category { get; }
|
||||
|
||||
internal string Unit { get; }
|
||||
}
|
||||
|
||||
internal static class LongitudinalQpFeasibilityAudit
|
||||
{
|
||||
internal static LongitudinalQpAuditResult Evaluate(QuadraticProgram problem,
|
||||
LongitudinalCandidate candidate, double tolerance, LongitudinalVariableLayout layout,
|
||||
int stabilizationStart)
|
||||
{
|
||||
if (problem == null)
|
||||
throw new ArgumentNullException(nameof(problem));
|
||||
if (candidate == null)
|
||||
throw new ArgumentNullException(nameof(candidate));
|
||||
if (layout == null)
|
||||
throw new ArgumentNullException(nameof(layout));
|
||||
if (problem.VariableCount != layout.VariableCount || candidate.S.Count != layout.KnotCount ||
|
||||
candidate.U.Count != layout.KnotCount || candidate.A.Count != layout.KnotCount ||
|
||||
candidate.J.Count != layout.KnotCount - 1)
|
||||
{
|
||||
throw new ArgumentException("The QP, candidate, and longitudinal layout must have matching dimensions.");
|
||||
}
|
||||
|
||||
double[] primal = ToPrimal(candidate, layout);
|
||||
var activity = new double[problem.ConstraintCount];
|
||||
bool allFinite = IsFinite(tolerance);
|
||||
SparseCscMatrix matrix = problem.ConstraintMatrix;
|
||||
for (int column = 0; column < matrix.ColumnCount; column++)
|
||||
{
|
||||
double value = primal[column];
|
||||
allFinite &= IsFinite(value);
|
||||
for (int entry = matrix.ColumnPointers[column]; entry < matrix.ColumnPointers[column + 1]; entry++)
|
||||
activity[matrix.RowIndices[entry]] += matrix.Values[entry] * value;
|
||||
}
|
||||
|
||||
double maximumResidual = 0d;
|
||||
int worstRow = problem.ConstraintCount == 0 ? -1 : 0;
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
double residual;
|
||||
if (!IsFinite(activity[row]))
|
||||
{
|
||||
allFinite = false;
|
||||
residual = double.PositiveInfinity;
|
||||
}
|
||||
else
|
||||
{
|
||||
residual = Math.Max(0d, Math.Max(
|
||||
problem.LowerBounds[row] - activity[row],
|
||||
activity[row] - problem.UpperBounds[row]));
|
||||
}
|
||||
if (row == 0 || residual > maximumResidual)
|
||||
{
|
||||
maximumResidual = residual;
|
||||
worstRow = row;
|
||||
}
|
||||
}
|
||||
|
||||
DescribeRow(worstRow, layout.KnotCount, stabilizationStart, out string category, out string unit);
|
||||
return new LongitudinalQpAuditResult(allFinite && maximumResidual <= tolerance,
|
||||
maximumResidual, worstRow, category, unit);
|
||||
}
|
||||
|
||||
private static double[] ToPrimal(LongitudinalCandidate candidate, LongitudinalVariableLayout layout)
|
||||
{
|
||||
var primal = new double[layout.VariableCount];
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
primal[layout.S(index)] = candidate.S[index];
|
||||
primal[layout.U(index)] = candidate.U[index];
|
||||
primal[layout.A(index)] = candidate.A[index];
|
||||
}
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
primal[layout.J(index)] = candidate.J[index];
|
||||
return primal;
|
||||
}
|
||||
|
||||
private static void DescribeRow(int targetRow, int knotCount, int stabilizationStart,
|
||||
out string category, out string unit)
|
||||
{
|
||||
int row = 0;
|
||||
for (int index = 0; index < knotCount; index++)
|
||||
{
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "PathS trust";
|
||||
unit = "m";
|
||||
return;
|
||||
}
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "speed";
|
||||
unit = "m/s";
|
||||
return;
|
||||
}
|
||||
if (index > 0 && targetRow == row++)
|
||||
{
|
||||
category = "speed envelope";
|
||||
unit = "m/s";
|
||||
return;
|
||||
}
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "acceleration";
|
||||
unit = "m/s^2";
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < knotCount - 1; index++)
|
||||
{
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "jerk";
|
||||
unit = "m/s^3";
|
||||
return;
|
||||
}
|
||||
}
|
||||
for (int index = 0; index < knotCount; index++)
|
||||
{
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "low-speed deceleration release";
|
||||
unit = "m/s";
|
||||
return;
|
||||
}
|
||||
}
|
||||
for (int index = 0; index < knotCount - 1; index++)
|
||||
{
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "monotonic progress";
|
||||
unit = "m";
|
||||
return;
|
||||
}
|
||||
}
|
||||
for (int index = 0; index < knotCount - 1; index++)
|
||||
{
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "exact dynamics";
|
||||
unit = "m/s^2";
|
||||
return;
|
||||
}
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "exact dynamics";
|
||||
unit = "m/s";
|
||||
return;
|
||||
}
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "exact dynamics";
|
||||
unit = "m";
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
string[] stateUnits = { "m", "m/s", "m/s^2" };
|
||||
for (int state = 0; state < stateUnits.Length; state++)
|
||||
{
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "exact start";
|
||||
unit = stateUnits[state];
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
int stopKnotCount = stabilizationStart >= 0 && stabilizationStart < knotCount
|
||||
? knotCount - stabilizationStart
|
||||
: 0;
|
||||
for (int index = 0; index < stopKnotCount; index++)
|
||||
{
|
||||
for (int state = 0; state < stateUnits.Length; state++)
|
||||
{
|
||||
if (targetRow == row++)
|
||||
{
|
||||
category = "exact stop";
|
||||
unit = stateUnits[state];
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
category = "row accounting";
|
||||
unit = string.Empty;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
+33
-13
@@ -1,5 +1,6 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
@@ -48,6 +49,8 @@ public sealed class LongitudinalSolutionValidator
|
||||
failureReason = "ST candidate violates exact constant-jerk dynamics.";
|
||||
return false;
|
||||
}
|
||||
if (!LongitudinalContinuousProfileValidator.TryValidate(candidate, tolerance, out failureReason))
|
||||
return false;
|
||||
if (!AreClose(candidate.S[0], 0d, tolerance) ||
|
||||
!AreClose(candidate.U[0], input.InitialProgressSpeedMetersPerSecond, tolerance) ||
|
||||
!AreClose(candidate.A[0], input.InitialAccelerationMetersPerSecondSquared, tolerance))
|
||||
@@ -56,6 +59,9 @@ public sealed class LongitudinalSolutionValidator
|
||||
return false;
|
||||
}
|
||||
|
||||
var canonicalS = new double[candidate.S.Count];
|
||||
var canonicalU = new double[candidate.U.Count];
|
||||
var canonicalA = new double[candidate.A.Count];
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
double progress = candidate.S[index];
|
||||
@@ -83,6 +89,11 @@ public sealed class LongitudinalSolutionValidator
|
||||
failureReason = "ST candidate PathS decreases at knot " + index + ".";
|
||||
return false;
|
||||
}
|
||||
canonicalS[index] = progress;
|
||||
canonicalU[index] = speed < 0d ? 0d : speed;
|
||||
canonicalA[index] = speed < 0d && acceleration < 0d && acceleration >= -tolerance
|
||||
? 0d
|
||||
: acceleration;
|
||||
}
|
||||
for (int index = 0; index < candidate.J.Count; index++)
|
||||
{
|
||||
@@ -122,25 +133,29 @@ public sealed class LongitudinalSolutionValidator
|
||||
{
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index],
|
||||
maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) ||
|
||||
candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance)
|
||||
bool hasStop = JerkLimitedStoppingMath.TryCalculate(canonicalU[index], canonicalA[index],
|
||||
maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop,
|
||||
out string stoppingFailure);
|
||||
double stopDistance = hasStop ? stop.DistanceMeters : double.NaN;
|
||||
double margin = hasStop
|
||||
? input.StopBoundaryPathS - canonicalS[index] - stopDistance
|
||||
: double.NaN;
|
||||
if (!hasStop || canonicalS[index] + stopDistance > input.StopBoundaryPathS + tolerance)
|
||||
{
|
||||
failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + ".";
|
||||
failureReason = "ST candidate leaves the jerk-limited stoppable set: knot=" +
|
||||
index.ToString(CultureInfo.InvariantCulture) +
|
||||
";S=" + Invariant(canonicalS[index]) +
|
||||
";U=" + Invariant(canonicalU[index]) +
|
||||
";A=" + Invariant(canonicalA[index]) +
|
||||
";stopDistance=" + Invariant(stopDistance) +
|
||||
";stopBoundary=" + Invariant(input.StopBoundaryPathS) +
|
||||
";margin=" + Invariant(margin) +
|
||||
(hasStop ? string.Empty : ";stoppingReason=" + stoppingFailure) + ".";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var canonicalS = new double[candidate.S.Count];
|
||||
var canonicalU = new double[candidate.U.Count];
|
||||
var canonicalA = new double[candidate.A.Count];
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
canonicalS[index] = candidate.S[index];
|
||||
canonicalU[index] = candidate.U[index];
|
||||
canonicalA[index] = candidate.A[index];
|
||||
}
|
||||
canonicalS[0] = 0d;
|
||||
canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
|
||||
canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
|
||||
@@ -202,6 +217,11 @@ public sealed class LongitudinalSolutionValidator
|
||||
return Math.Abs(actual - expected) <= tolerance;
|
||||
}
|
||||
|
||||
private static string Invariant(double value)
|
||||
{
|
||||
return value.ToString("R", CultureInfo.InvariantCulture);
|
||||
}
|
||||
|
||||
private static double RequireNonnegative(double value, string parameterName)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0d)
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal sealed class LongitudinalSolveTrace
|
||||
{
|
||||
private const int MaximumEntries = 12;
|
||||
private readonly List<string> _entries = new List<string>(MaximumEntries);
|
||||
|
||||
internal void Add(string phase, int callOrdinal, int anchorUpdateIndex, double trustScale,
|
||||
TimeSpan remainingBudget, TimeSpan remainingAfterReserve, TimeSpan elapsed, QpSolveResult result,
|
||||
double anchorObjective, double candidateObjective, string rejection)
|
||||
{
|
||||
if (_entries.Count >= MaximumEntries)
|
||||
throw new InvalidOperationException("Longitudinal solve trace exceeded the twelve-call cap.");
|
||||
string status = result == null ? "null" : result.Status.ToString();
|
||||
int iterations = result == null ? -1 : result.Iterations;
|
||||
double primal = result == null ? double.NaN : result.PrimalResidual;
|
||||
double dual = result == null ? double.NaN : result.DualResidual;
|
||||
_entries.Add("phase:" + phase +
|
||||
",call:" + callOrdinal.ToString(CultureInfo.InvariantCulture) +
|
||||
",anchor:" + anchorUpdateIndex.ToString(CultureInfo.InvariantCulture) +
|
||||
",scale:" + trustScale.ToString("R", CultureInfo.InvariantCulture) +
|
||||
",budgetMs:" + remainingBudget.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture) +
|
||||
",remainingAfterReserveMs:" + remainingAfterReserve.TotalMilliseconds.ToString(
|
||||
"F3", CultureInfo.InvariantCulture) +
|
||||
",elapsedMs:" + elapsed.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture) +
|
||||
",status:" + status +
|
||||
",iterations:" + iterations.ToString(CultureInfo.InvariantCulture) +
|
||||
",primal:" + primal.ToString("R", CultureInfo.InvariantCulture) +
|
||||
",dual:" + dual.ToString("R", CultureInfo.InvariantCulture) +
|
||||
",anchorObj:" + anchorObjective.ToString("R", CultureInfo.InvariantCulture) +
|
||||
",candidateObj:" + candidateObjective.ToString("R", CultureInfo.InvariantCulture) +
|
||||
",rejection:" + Sanitize(rejection));
|
||||
}
|
||||
|
||||
internal string Format() => string.Join("|", _entries);
|
||||
|
||||
private static string Sanitize(string text)
|
||||
{
|
||||
if (string.IsNullOrEmpty(text))
|
||||
return "none";
|
||||
return text.Replace(';', '/').Replace('|', '/').Replace('\r', '/').Replace('\n', '/');
|
||||
}
|
||||
}
|
||||
+486
-218
@@ -10,13 +10,21 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
/// <summary>Bounded ST envelope iteration retaining only independently validated physical candidates.</summary>
|
||||
public sealed class SequentialLongitudinalOptimizer
|
||||
{
|
||||
private const int MaximumEnvelopeIterations = 5;
|
||||
private const double OrdinaryEnvelopeProbeLookaheadSteps = 1d;
|
||||
private const double OrdinaryTerminalProbeFraction = 0.5d;
|
||||
private const int MaximumAcceptedAnchorUpdates = 5;
|
||||
private const int MaximumQpSolveCalls = 12;
|
||||
private const double MinimumTrustRegionWidthMeters = 0.001d;
|
||||
private const double ObjectiveAcceptanceRelativeTolerance = 1e-9d;
|
||||
private const double HighPrecisionRetryTolerance = 1e-7d;
|
||||
private const double StaticStartSeedBudgetFraction = 0.10d;
|
||||
private static readonly TimeSpan MaximumStaticStartSeedBudget = TimeSpan.FromMilliseconds(250d);
|
||||
private static readonly TimeSpan PublicationReserve = TimeSpan.FromMilliseconds(250d);
|
||||
private static readonly double[] TrustRegionScales = { 1d, 0.5d, 0.25d, 0.125d };
|
||||
private readonly IQpSolver _qpSolver;
|
||||
private readonly PathSpeedLimitBuilder _speedLimitBuilder;
|
||||
private readonly LongitudinalConstraintBuilder _constraintBuilder;
|
||||
private readonly LongitudinalSolutionValidator _solutionValidator;
|
||||
private readonly LongitudinalEnvelopeTrustRegionBuilder _trustRegionBuilder =
|
||||
new LongitudinalEnvelopeTrustRegionBuilder();
|
||||
|
||||
public SequentialLongitudinalOptimizer(IQpSolver qpSolver)
|
||||
: this(qpSolver, new PathSpeedLimitBuilder(), new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()),
|
||||
@@ -65,133 +73,418 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
return Failed(speedStatus, speedFailure);
|
||||
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
LongitudinalCandidate iterate;
|
||||
LongitudinalCandidate lastStrictCandidate = null;
|
||||
int remainingObjectiveIterations = iterationLimit;
|
||||
var solveTrace = new LongitudinalSolveTrace();
|
||||
LongitudinalCandidate initialCandidate;
|
||||
int projectionSolveCount = 0;
|
||||
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
|
||||
input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
|
||||
{
|
||||
if (!TryCreateInitialFeasibleCandidate(input, speedLimit, settings, totalBudget, convergenceTolerance,
|
||||
iterationLimit, stopwatch, cancellationToken, out iterate, out int projectionSolveCount,
|
||||
out EmPlanningStatus projectionStatus, out string projectionFailure))
|
||||
iterationLimit, stopwatch, solveTrace, cancellationToken, out initialCandidate,
|
||||
out int usedProjectionSolveCount, out EmPlanningStatus projectionStatus,
|
||||
out string projectionFailure))
|
||||
{
|
||||
return Failed(projectionStatus, projectionFailure);
|
||||
}
|
||||
lastStrictCandidate = CopyCandidate(iterate);
|
||||
remainingObjectiveIterations -= projectionSolveCount;
|
||||
if (remainingObjectiveIterations <= 0)
|
||||
{
|
||||
return new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, lastStrictCandidate,
|
||||
"The strict initial feasibility projection consumed the configured outer-iteration budget.");
|
||||
string projectionTrace = solveTrace.Format();
|
||||
return Failed(projectionStatus, projectionFailure +
|
||||
(string.IsNullOrEmpty(projectionTrace) ? string.Empty : ";solveTrace=" + projectionTrace));
|
||||
}
|
||||
projectionSolveCount = usedProjectionSolveCount;
|
||||
}
|
||||
else
|
||||
{
|
||||
iterate = CreateInitialIterate(input, speedLimit);
|
||||
if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
|
||||
lastStrictCandidate = null;
|
||||
LongitudinalCandidate seed = CreateInitialIterate(input, speedLimit);
|
||||
if (!_solutionValidator.TryValidate(input, speedLimit, seed, out initialCandidate,
|
||||
out EmPlanningStatus initializationStatus, out string initializationFailure))
|
||||
{
|
||||
return Failed(initializationStatus, "No strictly validated longitudinal candidate was found. " +
|
||||
initializationFailure);
|
||||
}
|
||||
}
|
||||
double[] warmStart = ToPrimal(iterate);
|
||||
bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
|
||||
string lastCandidateRejection = string.Empty;
|
||||
bool hasPreviousObjective = false;
|
||||
double previousObjective = 0d;
|
||||
|
||||
for (int iteration = 0; iteration < remainingObjectiveIterations; iteration++)
|
||||
LongitudinalCandidate anchor = CopyCandidate(initialCandidate);
|
||||
int stabilizationStart = GetStabilizationStart(input);
|
||||
int qpSolveCount = projectionSolveCount;
|
||||
int trustShrinkCount = 0;
|
||||
int acceptedAnchorCount = 0;
|
||||
int acceptedUpdateLimit = Math.Min(MaximumAcceptedAnchorUpdates, iterationLimit);
|
||||
double finalTrustScale = 1d;
|
||||
string lastRejection = string.Empty;
|
||||
|
||||
while (acceptedAnchorCount < acceptedUpdateLimit && qpSolveCount < MaximumQpSolveCalls)
|
||||
{
|
||||
bool promoted = false;
|
||||
for (int scaleIndex = 0; scaleIndex < TrustRegionScales.Length; scaleIndex++)
|
||||
{
|
||||
double scale = TrustRegionScales[scaleIndex];
|
||||
finalTrustScale = scale;
|
||||
if (cancellationToken.IsCancellationRequested || totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
|
||||
{
|
||||
return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
|
||||
finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
|
||||
}
|
||||
if (!_trustRegionBuilder.TryBuild(speedLimit, anchor, input.KnotSchedule.ReferencePathS,
|
||||
stabilizationStart, scale, MinimumTrustRegionWidthMeters,
|
||||
out LongitudinalEnvelopeTrustRegion region, out string regionFailure))
|
||||
{
|
||||
lastRejection = regionFailure;
|
||||
break;
|
||||
}
|
||||
if (!_constraintBuilder.TryBuildTrusted(input, speedLimit, anchor, region,
|
||||
convergenceTolerance, out QuadraticProgram problem, out string buildFailure))
|
||||
{
|
||||
lastRejection = buildFailure;
|
||||
break;
|
||||
}
|
||||
TrustedSolveAttempt attempt = SolveTrustedProblem(problem, input, speedLimit, anchor, settings,
|
||||
totalBudget, stopwatch, convergenceTolerance, stabilizationStart,
|
||||
MaximumQpSolveCalls - qpSolveCount, solveTrace, qpSolveCount, acceptedAnchorCount,
|
||||
scale, acceptedAnchorCount > 0, cancellationToken);
|
||||
qpSolveCount += attempt.SolveCount;
|
||||
lastRejection = attempt.FailureReason;
|
||||
if (attempt.Status == EmPlanningStatus.Cancelled)
|
||||
{
|
||||
return Failed(EmPlanningStatus.Cancelled, CreateRunDiagnostic(qpSolveCount,
|
||||
trustShrinkCount, acceptedAnchorCount, finalTrustScale, lastRejection, solveTrace));
|
||||
}
|
||||
if (attempt.Accepted)
|
||||
{
|
||||
anchor = CopyCandidate(attempt.Candidate);
|
||||
acceptedAnchorCount++;
|
||||
promoted = true;
|
||||
break;
|
||||
}
|
||||
if (attempt.Status != EmPlanningStatus.SuccessWithFallback)
|
||||
{
|
||||
return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
|
||||
finalTrustScale, false, lastRejection, solveTrace);
|
||||
}
|
||||
if (scaleIndex + 1 < TrustRegionScales.Length)
|
||||
{
|
||||
double nextScale = TrustRegionScales[scaleIndex + 1];
|
||||
if (!region.CanShrinkTo(nextScale, MinimumTrustRegionWidthMeters, out string shrinkFailure))
|
||||
{
|
||||
lastRejection = shrinkFailure;
|
||||
break;
|
||||
}
|
||||
trustShrinkCount++;
|
||||
}
|
||||
}
|
||||
if (!promoted)
|
||||
break;
|
||||
}
|
||||
|
||||
return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
|
||||
finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
|
||||
}
|
||||
|
||||
private TrustedSolveAttempt SolveTrustedProblem(QuadraticProgram problem,
|
||||
LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
|
||||
QpSolverSettings settings, TimeSpan totalBudget, Stopwatch stopwatch, double strictTolerance,
|
||||
int stabilizationStart, int remainingCallCount, LongitudinalSolveTrace solveTrace,
|
||||
int solveOrdinalOffset, int anchorUpdateIndex, double trustScale,
|
||||
bool optionalImprovement, CancellationToken cancellationToken)
|
||||
{
|
||||
int solveCount = 0;
|
||||
double anchorObjective = EvaluateObjective(problem, ToPrimal(anchor));
|
||||
if (!IsFinite(anchorObjective))
|
||||
{
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
|
||||
"The strict anchor objective is non-finite for the trusted QP.");
|
||||
}
|
||||
|
||||
IReadOnlyList<double> warmStart = ToPrimal(anchor);
|
||||
string lastFailure = string.Empty;
|
||||
for (int attemptIndex = 0; attemptIndex < 2; attemptIndex++)
|
||||
{
|
||||
if (solveCount >= remainingCallCount)
|
||||
{
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
||||
string.IsNullOrWhiteSpace(lastFailure) ? "The longitudinal QP solve-call cap was reached." : lastFailure);
|
||||
}
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
|
||||
{
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
|
||||
"Longitudinal optimization was cancelled before the trusted QP solve.");
|
||||
}
|
||||
TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
|
||||
if (remainingBudget <= TimeSpan.Zero)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
|
||||
"Longitudinal optimization exhausted its solve budget.");
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
||||
"Longitudinal optimization exhausted its shared solve budget.");
|
||||
}
|
||||
if (!_constraintBuilder.TryBuild(input, speedLimit, iterate, out QuadraticProgram problem, out string buildFailure))
|
||||
bool hasOptionalSolveBudget = TryGetOptionalSolveBudget(remainingBudget,
|
||||
out TimeSpan remainingAfterReserve);
|
||||
if (optionalImprovement && !hasOptionalSolveBudget)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
|
||||
"Longitudinal constraints are infeasible: " + buildFailure);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
||||
CreatePublicationReserveSkipDiagnostic(remainingAfterReserve));
|
||||
}
|
||||
TimeSpan solveBudget = optionalImprovement ? remainingAfterReserve : remainingBudget;
|
||||
|
||||
bool highPrecision = attemptIndex == 1;
|
||||
double absoluteTolerance = highPrecision
|
||||
? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
|
||||
: settings.AbsoluteTolerance;
|
||||
double relativeTolerance = highPrecision
|
||||
? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
|
||||
: settings.RelativeTolerance;
|
||||
var solveStopwatch = Stopwatch.StartNew();
|
||||
QpSolveResult solved = _qpSolver.Solve(problem,
|
||||
new QpSolverSettings(settings.MaximumIterations, absoluteTolerance, relativeTolerance,
|
||||
solveBudget, settings.EnableWarmStart, settings.EnablePolishing,
|
||||
settings.EnableNativeVerboseOutput), warmStart, cancellationToken);
|
||||
solveStopwatch.Stop();
|
||||
solveCount++;
|
||||
double candidateObjective = double.NaN;
|
||||
void AddSolveTrace(string rejection)
|
||||
{
|
||||
solveTrace.Add(attemptIndex == 0 ? "normal" : "retry",
|
||||
solveOrdinalOffset + solveCount, anchorUpdateIndex, trustScale,
|
||||
solveBudget, remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
|
||||
candidateObjective, rejection);
|
||||
}
|
||||
|
||||
QpSolveResult solved = _qpSolver.Solve(problem,
|
||||
new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
|
||||
remainingBudget, settings.EnableWarmStart && (iteration > 0 || hasDynamicsConsistentInitialWarmStart),
|
||||
settings.EnablePolishing,
|
||||
settings.EnableNativeVerboseOutput),
|
||||
warmStart, cancellationToken);
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled after the QP solve.");
|
||||
if (solved == null)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed, "The longitudinal QP solver returned no result.");
|
||||
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
|
||||
"The longitudinal QP solver timed out (status=" + solved.NativeStatus +
|
||||
", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
|
||||
", dual=" + solved.DualResidual + "): " + solved.Diagnostic);
|
||||
const string rejection = "Longitudinal optimization was cancelled after the trusted QP solve.";
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
if (solved == null)
|
||||
{
|
||||
const string rejection = "The longitudinal QP solver returned no result.";
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.Cancelled)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled,
|
||||
"The longitudinal QP solver was cancelled: " + solved.Diagnostic);
|
||||
{
|
||||
string rejection = "The longitudinal QP solver was cancelled: " + solved.Diagnostic;
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
||||
{
|
||||
string rejection = "The longitudinal QP solver timed out (status=" + solved.NativeStatus +
|
||||
", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
|
||||
", dual=" + solved.DualResidual + "): " + solved.Diagnostic;
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
|
||||
"The longitudinal QP solver reported infeasibility: " + solved.Diagnostic);
|
||||
string rejection = "The preflight-feasible longitudinal QP solver reported infeasibility;" +
|
||||
"solverNumericalAnomaly=true;status=" + solved.NativeStatus + ": " + solved.Diagnostic;
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolverUnavailable)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverUnavailable,
|
||||
"The longitudinal QP solver is unavailable: " + solved.Diagnostic);
|
||||
{
|
||||
string rejection = "The longitudinal QP solver is unavailable: " + solved.Diagnostic;
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.SolverUnavailable, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
|
||||
{
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed,
|
||||
"The longitudinal QP solver failed: " + solved.Diagnostic);
|
||||
string rejection = "The longitudinal QP solver failed (status=" + solved.NativeStatus + "): " +
|
||||
solved.Diagnostic;
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, convergenceTolerance))
|
||||
|
||||
if (!TryCreateCandidate(anchor.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
|
||||
{
|
||||
lastCandidateRejection = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
|
||||
const string rejection =
|
||||
"The solver primal does not match the ST variable layout or contains non-finite values.";
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
|
||||
bool accepted = true;
|
||||
if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, strictTolerance))
|
||||
{
|
||||
accepted = false;
|
||||
lastFailure = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
|
||||
" (primal=" + solved.PrimalResidual + ", dual=" + solved.DualResidual + ").";
|
||||
if (TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate inaccurateCandidate))
|
||||
warmStart = ToPrimal(inaccurateCandidate);
|
||||
continue;
|
||||
}
|
||||
if (!TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
|
||||
if (accepted && !TryFastValidateCandidate(problem, candidate, strictTolerance,
|
||||
stabilizationStart, out lastFailure))
|
||||
{
|
||||
lastCandidateRejection = "The solver primal does not match the ST variable layout.";
|
||||
continue;
|
||||
accepted = false;
|
||||
}
|
||||
if (!_solutionValidator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate validated,
|
||||
LongitudinalCandidate validated = null;
|
||||
if (accepted && !_solutionValidator.TryValidate(input, speedLimit, candidate, out validated,
|
||||
out string validationFailure))
|
||||
{
|
||||
string rejection = validationFailure + CreateEnvelopeDiagnostic(speedLimit, iterate, candidate,
|
||||
iteration + 1);
|
||||
lastCandidateRejection = string.IsNullOrEmpty(lastCandidateRejection)
|
||||
? rejection
|
||||
: lastCandidateRejection + " | " + rejection;
|
||||
if (TryCreateEnvelopeIterate(input, iterate, candidate, out LongitudinalCandidate nextIterate))
|
||||
accepted = false;
|
||||
lastFailure = validationFailure;
|
||||
}
|
||||
if (accepted)
|
||||
{
|
||||
candidateObjective = EvaluateObjective(problem, ToPrimal(validated));
|
||||
if (!IsObjectiveAccepted(anchorObjective, candidateObjective))
|
||||
{
|
||||
iterate = nextIterate;
|
||||
warmStart = ToPrimal(candidate);
|
||||
accepted = false;
|
||||
lastFailure = "The strictly valid candidate worsens the current trusted-QP objective" +
|
||||
" (anchor=" + anchorObjective.ToString("R", CultureInfo.InvariantCulture) +
|
||||
", candidate=" + candidateObjective.ToString("R", CultureInfo.InvariantCulture) + ").";
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if (accepted && cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
const string rejection = "Longitudinal optimization was cancelled before strict-anchor promotion.";
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
if (accepted && totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
|
||||
{
|
||||
const string rejection =
|
||||
"Longitudinal optimization exhausted its shared solve budget before strict-anchor promotion.";
|
||||
AddSolveTrace(rejection);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
|
||||
rejection);
|
||||
}
|
||||
if (accepted)
|
||||
{
|
||||
AddSolveTrace(string.Empty);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.Success, validated, solveCount, true, string.Empty);
|
||||
}
|
||||
if (highPrecision)
|
||||
{
|
||||
AddSolveTrace(lastFailure);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
|
||||
lastFailure);
|
||||
}
|
||||
|
||||
double maximumChange = MaximumProgressOrSpeedChange(iterate, validated);
|
||||
double relativeObjectiveImprovement = hasPreviousObjective
|
||||
? RelativeObjectiveImprovement(previousObjective, solved.Objective)
|
||||
: double.PositiveInfinity;
|
||||
lastStrictCandidate = CopyCandidate(validated);
|
||||
iterate = validated;
|
||||
warmStart = ToPrimal(validated);
|
||||
previousObjective = solved.Objective;
|
||||
hasPreviousObjective = true;
|
||||
if (maximumChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
|
||||
return new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
|
||||
AddSolveTrace(lastFailure);
|
||||
warmStart = ToPrimal(candidate);
|
||||
}
|
||||
|
||||
return lastStrictCandidate == null
|
||||
? Failed(EmPlanningStatus.LongitudinalInfeasible, "No strictly validated longitudinal candidate was found. " +
|
||||
lastCandidateRejection)
|
||||
: new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
|
||||
return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
|
||||
lastFailure);
|
||||
}
|
||||
|
||||
private static double EvaluateObjective(QuadraticProgram problem, IReadOnlyList<double> primal)
|
||||
{
|
||||
if (problem == null || primal == null || primal.Count != problem.VariableCount)
|
||||
return double.NaN;
|
||||
double objective = 0d;
|
||||
SparseCscMatrix hessian = problem.UpperTriangularP;
|
||||
for (int column = 0; column < hessian.ColumnCount; column++)
|
||||
{
|
||||
double columnValue = primal[column];
|
||||
if (!IsFinite(columnValue))
|
||||
return double.NaN;
|
||||
for (int entry = hessian.ColumnPointers[column]; entry < hessian.ColumnPointers[column + 1]; entry++)
|
||||
{
|
||||
int row = hessian.RowIndices[entry];
|
||||
double term = hessian.Values[entry] * primal[row] * columnValue;
|
||||
objective += row == column ? 0.5d * term : term;
|
||||
if (!IsFinite(objective))
|
||||
return double.NaN;
|
||||
}
|
||||
objective += problem.LinearCost[column] * columnValue;
|
||||
if (!IsFinite(objective))
|
||||
return double.NaN;
|
||||
}
|
||||
return objective;
|
||||
}
|
||||
|
||||
private static bool IsObjectiveAccepted(double anchorObjective, double candidateObjective)
|
||||
{
|
||||
if (!IsFinite(anchorObjective) || !IsFinite(candidateObjective))
|
||||
return false;
|
||||
double tolerance = ObjectiveAcceptanceRelativeTolerance * Math.Max(1d, Math.Abs(anchorObjective));
|
||||
return candidateObjective <= anchorObjective + tolerance;
|
||||
}
|
||||
|
||||
private static int GetStabilizationStart(LongitudinalPlanningInput input)
|
||||
{
|
||||
if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
|
||||
return input.KnotSchedule.KnotTimes.Count;
|
||||
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
||||
return input.KnotSchedule.TerminalHoldStartIndex;
|
||||
return LongitudinalTerminalSchedule.GetStabilizationStartIndex(input.KnotSchedule.KnotTimes,
|
||||
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
}
|
||||
|
||||
private static bool TryFastValidateCandidate(QuadraticProgram problem, LongitudinalCandidate candidate,
|
||||
double strictTolerance, int stabilizationStart, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
|
||||
LongitudinalQpAuditResult audit;
|
||||
try
|
||||
{
|
||||
audit = LongitudinalQpFeasibilityAudit.Evaluate(problem, candidate, strictTolerance,
|
||||
layout, stabilizationStart);
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = "The candidate cannot be audited against the current trusted QP: " + exception.Message;
|
||||
return false;
|
||||
}
|
||||
if (audit.IsFeasible)
|
||||
return true;
|
||||
failureReason = "The candidate violates the current trusted QP" +
|
||||
";row=" + audit.WorstRow + ";category=" + audit.Category +
|
||||
";residual=" + audit.MaximumResidual.ToString("R", CultureInfo.InvariantCulture) + audit.Unit +
|
||||
";tolerance=" + strictTolerance.ToString("R", CultureInfo.InvariantCulture);
|
||||
return false;
|
||||
}
|
||||
|
||||
private static string CreateRunDiagnostic(int qpSolveCount, int trustShrinkCount,
|
||||
int acceptedAnchorCount, double finalTrustScale, string lastRejection,
|
||||
LongitudinalSolveTrace solveTrace)
|
||||
{
|
||||
return "qpSolves=" + qpSolveCount +
|
||||
",trustShrinks=" + trustShrinkCount +
|
||||
",acceptedAnchors=" + acceptedAnchorCount +
|
||||
",trustScale=" + finalTrustScale.ToString("R", CultureInfo.InvariantCulture) +
|
||||
(string.IsNullOrWhiteSpace(lastRejection) ? string.Empty : ";lastRejection=" + lastRejection) +
|
||||
(string.IsNullOrEmpty(solveTrace.Format()) ? string.Empty : ";solveTrace=" + solveTrace.Format());
|
||||
}
|
||||
|
||||
internal static bool TryGetOptionalSolveBudget(TimeSpan remaining, out TimeSpan solveBudget)
|
||||
{
|
||||
solveBudget = remaining - PublicationReserve;
|
||||
if (solveBudget <= TimeSpan.Zero)
|
||||
{
|
||||
solveBudget = TimeSpan.Zero;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static string CreatePublicationReserveSkipDiagnostic(TimeSpan remainingAfterReserve)
|
||||
{
|
||||
return "remainingAfterReserveMs=" + remainingAfterReserve.TotalMilliseconds.ToString(
|
||||
"F3", CultureInfo.InvariantCulture) +
|
||||
";publicationReserveMs=" + PublicationReserve.TotalMilliseconds.ToString(
|
||||
"F0", CultureInfo.InvariantCulture) +
|
||||
";optionalImprovement=skipped";
|
||||
}
|
||||
|
||||
private static LongitudinalPlanningResult FinishFromAnchor(LongitudinalCandidate anchor,
|
||||
int acceptedAnchorCount, int qpSolveCount, int trustShrinkCount, double finalTrustScale,
|
||||
bool cancelled, string lastRejection, LongitudinalSolveTrace solveTrace)
|
||||
{
|
||||
string diagnostic = CreateRunDiagnostic(qpSolveCount, trustShrinkCount, acceptedAnchorCount,
|
||||
finalTrustScale, lastRejection, solveTrace);
|
||||
if (cancelled)
|
||||
return Failed(EmPlanningStatus.Cancelled, diagnostic);
|
||||
EmPlanningStatus status = acceptedAnchorCount > 0
|
||||
? EmPlanningStatus.Success
|
||||
: EmPlanningStatus.SuccessWithFallback;
|
||||
return new LongitudinalPlanningResult(status, CopyCandidate(anchor), diagnostic);
|
||||
}
|
||||
|
||||
private static bool TryCreateSettings(LongitudinalPlanningInput input, out QpSolverSettings settings,
|
||||
@@ -222,7 +515,7 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
settings = new QpSolverSettings(solver.MaximumOsqpIterations, solver.AbsoluteTolerance, solver.RelativeTolerance,
|
||||
totalBudget, solver.WarmStart, solver.Polish, solver.NativeVerbose);
|
||||
convergenceTolerance = solver.StrictResidualTolerance;
|
||||
iterationLimit = Math.Min(MaximumEnvelopeIterations, solver.MaximumOuterIterations);
|
||||
iterationLimit = Math.Min(MaximumAcceptedAnchorUpdates, solver.MaximumOuterIterations);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
@@ -234,8 +527,9 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
|
||||
private bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
||||
QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit,
|
||||
Stopwatch stopwatch, CancellationToken cancellationToken, out LongitudinalCandidate candidate,
|
||||
out int projectionSolveCount, out EmPlanningStatus failureStatus, out string failureReason)
|
||||
Stopwatch stopwatch, LongitudinalSolveTrace solveTrace, CancellationToken cancellationToken,
|
||||
out LongitudinalCandidate candidate, out int projectionSolveCount,
|
||||
out EmPlanningStatus failureStatus, out string failureReason)
|
||||
{
|
||||
candidate = null;
|
||||
projectionSolveCount = 0;
|
||||
@@ -248,8 +542,9 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
input.Configuration.Validation.KinematicTolerance;
|
||||
bool staticStartSeedUsed = false;
|
||||
string staticStartSeedFailure = string.Empty;
|
||||
if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed,
|
||||
out staticStartSeedFailure))
|
||||
TimeSpan staticStartSeedDeadline = stopwatch.Elapsed + GetStaticStartSeedBudget(totalBudget);
|
||||
if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, stopwatch, staticStartSeedDeadline,
|
||||
out LongitudinalCandidate staticStartSeed, out staticStartSeedFailure))
|
||||
{
|
||||
staticStartSeedUsed = true;
|
||||
candidate = staticStartSeed;
|
||||
@@ -289,60 +584,93 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
|
||||
double projectionTolerance = Math.Min(settings.AbsoluteTolerance,
|
||||
input.Configuration.Validation.KinematicTolerance * 0.1d);
|
||||
double anchorObjective = double.NaN;
|
||||
var solveStopwatch = Stopwatch.StartNew();
|
||||
QpSolveResult solved = _qpSolver.Solve(problem,
|
||||
new QpSolverSettings(settings.MaximumIterations, projectionTolerance, projectionTolerance,
|
||||
remainingBudget, settings.EnableWarmStart && linearizationIterate.SatisfiesExactDiscreteDynamics(1e-12d),
|
||||
settings.EnablePolishing, settings.EnableNativeVerboseOutput),
|
||||
ToPrimal(linearizationIterate), cancellationToken);
|
||||
solveStopwatch.Stop();
|
||||
projectionSolveCount++;
|
||||
int projectionCallOrdinal = projectionSolveCount;
|
||||
double candidateObjective = double.NaN;
|
||||
void AddProjectionTrace(string rejection)
|
||||
{
|
||||
TryGetOptionalSolveBudget(remainingBudget, out TimeSpan remainingAfterReserve);
|
||||
solveTrace.Add("projection", projectionCallOrdinal, 0, 1d, remainingBudget,
|
||||
remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
|
||||
candidateObjective, rejection);
|
||||
}
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
const string rejection =
|
||||
"Initial full-direction feasibility projection was cancelled after the QP solve.";
|
||||
AddProjectionTrace(rejection);
|
||||
failureStatus = EmPlanningStatus.Cancelled;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled after the QP solve.");
|
||||
failureReason = WithStaticSeedDiagnostic(rejection);
|
||||
return false;
|
||||
}
|
||||
if (solved == null)
|
||||
{
|
||||
const string rejection = "The initial full-direction feasibility solver returned no result.";
|
||||
AddProjectionTrace(rejection);
|
||||
failureStatus = EmPlanningStatus.Failed;
|
||||
failureReason = WithStaticSeedDiagnostic("The initial full-direction feasibility solver returned no result.");
|
||||
failureReason = WithStaticSeedDiagnostic(rejection);
|
||||
return false;
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
||||
{
|
||||
failureStatus = EmPlanningStatus.SolverTimedOut;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
|
||||
string rejection = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
|
||||
", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
|
||||
solved.DualResidual + "): " + solved.Diagnostic);
|
||||
solved.DualResidual + "): " + solved.Diagnostic;
|
||||
AddProjectionTrace(rejection);
|
||||
failureStatus = EmPlanningStatus.SolverTimedOut;
|
||||
failureReason = WithStaticSeedDiagnostic(rejection);
|
||||
return false;
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.Cancelled)
|
||||
{
|
||||
string rejection =
|
||||
"Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
|
||||
AddProjectionTrace(rejection);
|
||||
failureStatus = EmPlanningStatus.Cancelled;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic);
|
||||
failureReason = WithStaticSeedDiagnostic(rejection);
|
||||
return false;
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
|
||||
{
|
||||
string rejection =
|
||||
"Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
|
||||
AddProjectionTrace(rejection);
|
||||
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic);
|
||||
failureReason = WithStaticSeedDiagnostic(rejection);
|
||||
return false;
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolverUnavailable)
|
||||
{
|
||||
string rejection =
|
||||
"Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
|
||||
AddProjectionTrace(rejection);
|
||||
failureStatus = EmPlanningStatus.SolverUnavailable;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic);
|
||||
failureReason = WithStaticSeedDiagnostic(rejection);
|
||||
return false;
|
||||
}
|
||||
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
|
||||
{
|
||||
string rejection = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
|
||||
AddProjectionTrace(rejection);
|
||||
failureStatus = EmPlanningStatus.Failed;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver failed: " + solved.Diagnostic);
|
||||
failureReason = WithStaticSeedDiagnostic(rejection);
|
||||
return false;
|
||||
}
|
||||
if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
|
||||
{
|
||||
const string rejection =
|
||||
"Initial full-direction feasibility solver primal does not match the ST layout.";
|
||||
AddProjectionTrace(rejection);
|
||||
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver primal does not match the ST layout.");
|
||||
failureReason = WithStaticSeedDiagnostic(rejection);
|
||||
return false;
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
|
||||
@@ -350,11 +678,13 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
|
||||
out EmPlanningStatus validationStatus, out string validationFailure))
|
||||
{
|
||||
AddProjectionTrace(string.Empty);
|
||||
candidate = strict;
|
||||
return true;
|
||||
}
|
||||
if (validationStatus == EmPlanningStatus.NoProgress)
|
||||
{
|
||||
AddProjectionTrace(validationFailure);
|
||||
failureStatus = validationStatus;
|
||||
failureReason = WithStaticSeedDiagnostic(validationFailure);
|
||||
return false;
|
||||
@@ -365,8 +695,11 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
if (!TryCreateFeasibilityEnvelopeIterate(input, projected,
|
||||
out LongitudinalCandidate nextLinearization))
|
||||
{
|
||||
const string rejection =
|
||||
"Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
|
||||
AddProjectionTrace(rejection);
|
||||
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.");
|
||||
failureReason = WithStaticSeedDiagnostic(rejection);
|
||||
return false;
|
||||
}
|
||||
linearizationIterate = nextLinearization;
|
||||
@@ -374,6 +707,7 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
lastRejection = "Initial feasibility projection residuals exceed the strict acceptance tolerance.";
|
||||
else if (string.IsNullOrEmpty(lastRejection))
|
||||
lastRejection = "Initial feasibility projection violated the strict physical validator.";
|
||||
AddProjectionTrace(lastRejection);
|
||||
}
|
||||
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " +
|
||||
@@ -503,7 +837,7 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
}
|
||||
|
||||
private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
|
||||
out LongitudinalCandidate candidate, out string failureReason)
|
||||
Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate, out string failureReason)
|
||||
{
|
||||
candidate = null;
|
||||
failureReason = "unknown";
|
||||
@@ -515,9 +849,11 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
}
|
||||
|
||||
IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
|
||||
if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
|
||||
if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
|
||||
return true;
|
||||
failureReason = "exactJerkSeed=failed";
|
||||
if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
|
||||
return true;
|
||||
failureReason = "scurveSeed=failed; exactJerkSeed=failed";
|
||||
double firstDuration = times[1] - times[0];
|
||||
double secondDuration = times[2] - times[1];
|
||||
LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
|
||||
@@ -526,6 +862,8 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
|
||||
for (int sample = -256; sample <= 256; sample++)
|
||||
{
|
||||
if (HasReachedDeadline(stopwatch, deadline))
|
||||
return false;
|
||||
if (sample == 0)
|
||||
continue;
|
||||
double firstJerk = maximumFirstJerk * sample / 256d;
|
||||
@@ -545,18 +883,13 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
return true;
|
||||
}
|
||||
}
|
||||
if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, out candidate))
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
failureReason = "exactJerkSeed=failed; sampledSeeds=failed; scurveSeeds=failed";
|
||||
failureReason = "scurveSeed=failed; exactJerkSeed=failed; sampledSeeds=failed";
|
||||
return false;
|
||||
}
|
||||
|
||||
private bool TryCreateStaticStartScurveSeed(LongitudinalPlanningInput input,
|
||||
IReadOnlyList<double> times, int stabilizationStart, PathSpeedLimit speedLimit,
|
||||
out LongitudinalCandidate candidate)
|
||||
Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate)
|
||||
{
|
||||
candidate = null;
|
||||
int intervalCount = stabilizationStart;
|
||||
@@ -569,6 +902,8 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
{
|
||||
for (int plateau = 0; 4 * ramp + 2 * plateau <= intervalCount; plateau++)
|
||||
{
|
||||
if (HasReachedDeadline(stopwatch, deadline))
|
||||
return false;
|
||||
int cruise = intervalCount - 4 * ramp - 2 * plateau;
|
||||
var basisAccel = new double[intervalCount];
|
||||
var basisBrake = new double[intervalCount];
|
||||
@@ -688,6 +1023,14 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
|
||||
private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
||||
int stabilizationStart, PathSpeedLimit speedLimit, out LongitudinalCandidate candidate)
|
||||
{
|
||||
return TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, Stopwatch.StartNew(),
|
||||
TimeSpan.MaxValue, out candidate);
|
||||
}
|
||||
|
||||
private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
||||
int stabilizationStart, PathSpeedLimit speedLimit, Stopwatch stopwatch, TimeSpan deadline,
|
||||
out LongitudinalCandidate candidate)
|
||||
{
|
||||
candidate = null;
|
||||
int intervalCount = stabilizationStart;
|
||||
@@ -747,6 +1090,8 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
{
|
||||
for (int basisIndex = 0; basisIndex < intervalCount; basisIndex++)
|
||||
{
|
||||
if (HasReachedDeadline(stopwatch, deadline))
|
||||
return false;
|
||||
double[] direction = CreateEndpointNullspaceDirection(influence, gram, basisIndex);
|
||||
if (direction == null)
|
||||
continue;
|
||||
@@ -755,6 +1100,8 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
double bestViolation = currentViolation;
|
||||
for (int sample = -256; sample <= 256; sample++)
|
||||
{
|
||||
if (HasReachedDeadline(stopwatch, deadline))
|
||||
return false;
|
||||
double scale = maximumJerk * sample / 256d;
|
||||
var probeJerk = new double[intervalCount];
|
||||
for (int interval = 0; interval < intervalCount; interval++)
|
||||
@@ -776,6 +1123,18 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
return false;
|
||||
}
|
||||
|
||||
private static TimeSpan GetStaticStartSeedBudget(TimeSpan totalBudget)
|
||||
{
|
||||
double milliseconds = Math.Min(MaximumStaticStartSeedBudget.TotalMilliseconds,
|
||||
Math.Max(1d, totalBudget.TotalMilliseconds * StaticStartSeedBudgetFraction));
|
||||
return TimeSpan.FromMilliseconds(milliseconds);
|
||||
}
|
||||
|
||||
private static bool HasReachedDeadline(Stopwatch stopwatch, TimeSpan deadline)
|
||||
{
|
||||
return stopwatch.Elapsed >= deadline;
|
||||
}
|
||||
|
||||
private bool TryValidateExactSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
|
||||
int stabilizationStart, PathSpeedLimit speedLimit, IReadOnlyList<double> jerk,
|
||||
out LongitudinalCandidate candidate)
|
||||
@@ -1065,72 +1424,6 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
return primal;
|
||||
}
|
||||
|
||||
private static bool TryCreateEnvelopeIterate(LongitudinalPlanningInput input, LongitudinalCandidate previous,
|
||||
LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
|
||||
{
|
||||
nextIterate = null;
|
||||
if (candidate.S.Count != previous.S.Count)
|
||||
return false;
|
||||
int stabilizationStart = input.Mode != EmLongitudinalMode.ExactStopAtBoundary
|
||||
? candidate.S.Count
|
||||
: input.PlanningScope == EmPlanningScope.FullDirectionSegment
|
||||
? input.KnotSchedule.TerminalHoldStartIndex
|
||||
: LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
|
||||
input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
var candidateProgressSamples = new double[candidate.S.Count];
|
||||
double priorProgress = double.NegativeInfinity;
|
||||
double priorPreviousProgress = double.NegativeInfinity;
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
{
|
||||
double candidateProgress = candidate.S[index];
|
||||
double previousProgress = previous.S[index];
|
||||
if (!IsFinite(previousProgress) || previousProgress < 0d || previousProgress > input.PathUpperBoundS ||
|
||||
previousProgress < priorPreviousProgress)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(candidateProgress))
|
||||
{
|
||||
candidateProgress = previousProgress;
|
||||
}
|
||||
candidateProgress = Math.Max(0d, Math.Min(input.PathUpperBoundS, candidateProgress));
|
||||
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary && index >= stabilizationStart)
|
||||
candidateProgress = input.StopBoundaryPathS;
|
||||
candidateProgress = Math.Max(priorProgress, candidateProgress);
|
||||
candidateProgressSamples[index] = candidateProgress;
|
||||
priorProgress = candidateProgress;
|
||||
priorPreviousProgress = previousProgress;
|
||||
}
|
||||
var progress = new double[candidate.S.Count];
|
||||
double previousNextProgress = 0d;
|
||||
for (int index = 0; index < progress.Length; index++)
|
||||
{
|
||||
double candidateProgress = candidateProgressSamples[index];
|
||||
if (index == 0 || index == progress.Length - 1 || (input.Mode == EmLongitudinalMode.ExactStopAtBoundary &&
|
||||
index >= stabilizationStart) || candidateProgress >= input.PathUpperBoundS)
|
||||
{
|
||||
progress[index] = candidateProgress;
|
||||
}
|
||||
else
|
||||
{
|
||||
double timeStep = candidate.KnotTimes[index + 1] - candidate.KnotTimes[index];
|
||||
double iterationAdvance = Math.Max(0d, candidateProgress - previous.S[index]);
|
||||
double candidateSpeed = IsFinite(candidate.U[index]) ? Math.Max(0d, candidate.U[index]) : 0d;
|
||||
double lookaheadAdvance = IsFinite(candidate.U[index])
|
||||
? OrdinaryEnvelopeProbeLookaheadSteps * candidateSpeed * timeStep
|
||||
: 0d;
|
||||
double terminalLimitedAdvance = OrdinaryTerminalProbeFraction *
|
||||
(input.PathUpperBoundS - candidateProgress);
|
||||
double advance = Math.Min(Math.Max(iterationAdvance, lookaheadAdvance), terminalLimitedAdvance);
|
||||
progress[index] = candidateProgress + advance;
|
||||
}
|
||||
progress[index] = Math.Max(previousNextProgress, progress[index]);
|
||||
previousNextProgress = progress[index];
|
||||
}
|
||||
nextIterate = new LongitudinalCandidate(candidate.KnotTimes, progress, previous.U, previous.A, previous.J);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateFeasibilityEnvelopeIterate(LongitudinalPlanningInput input,
|
||||
LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
|
||||
{
|
||||
@@ -1161,52 +1454,27 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
result.PrimalResidual <= tolerance && result.DualResidual <= tolerance;
|
||||
}
|
||||
|
||||
private static string CreateEnvelopeDiagnostic(PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
|
||||
LongitudinalCandidate candidate, int iteration)
|
||||
private readonly struct TrustedSolveAttempt
|
||||
{
|
||||
int worstIndex = -1;
|
||||
double worstExcess = double.NegativeInfinity;
|
||||
for (int index = 0; index < candidate.S.Count; index++)
|
||||
internal TrustedSolveAttempt(EmPlanningStatus status, LongitudinalCandidate candidate,
|
||||
int solveCount, bool accepted, string failureReason)
|
||||
{
|
||||
if (!IsFinite(candidate.S[index]) || !IsFinite(candidate.U[index]))
|
||||
continue;
|
||||
double candidateProgress = Math.Max(0d, Math.Min(speedLimit.PathUpperBoundS, candidate.S[index]));
|
||||
double limit = speedLimit.MaximumSpeedAt(candidateProgress);
|
||||
double excess = candidate.U[index] - limit;
|
||||
if (excess > worstExcess)
|
||||
{
|
||||
worstExcess = excess;
|
||||
worstIndex = index;
|
||||
}
|
||||
Status = status;
|
||||
Candidate = candidate;
|
||||
SolveCount = solveCount;
|
||||
Accepted = accepted;
|
||||
FailureReason = failureReason ?? string.Empty;
|
||||
}
|
||||
if (worstIndex < 0)
|
||||
return "";
|
||||
return " Envelope iteration " + iteration + " used PathS=" + iterate.S[worstIndex] +
|
||||
" and produced PathS=" + candidate.S[worstIndex] + " at its largest speed-envelope excess.";
|
||||
}
|
||||
|
||||
private static double MaximumProgressOrSpeedChange(LongitudinalCandidate previous, LongitudinalCandidate current)
|
||||
{
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < previous.S.Count; index++)
|
||||
{
|
||||
maximum = Math.Max(maximum, Math.Abs(current.S[index] - previous.S[index]));
|
||||
maximum = Math.Max(maximum, Math.Abs(current.U[index] - previous.U[index]));
|
||||
}
|
||||
return maximum;
|
||||
}
|
||||
internal EmPlanningStatus Status { get; }
|
||||
|
||||
private static double RelativeObjectiveImprovement(double previous, double current)
|
||||
{
|
||||
return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
|
||||
}
|
||||
internal LongitudinalCandidate Candidate { get; }
|
||||
|
||||
private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
|
||||
EmPlanningStatus failureStatus, string failureReason)
|
||||
{
|
||||
return failureStatus == EmPlanningStatus.Cancelled || candidate == null
|
||||
? Failed(failureStatus, failureReason)
|
||||
: new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
|
||||
internal int SolveCount { get; }
|
||||
|
||||
internal bool Accepted { get; }
|
||||
|
||||
internal string FailureReason { get; }
|
||||
}
|
||||
|
||||
private static LongitudinalPlanningResult Failed(EmPlanningStatus status, string reason)
|
||||
|
||||
@@ -3,6 +3,9 @@ using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 求解器中立的二次规划边界。实现必须将数值状态转换为 <see cref="QpSolveResult"/>，而不能把原生求解器对象泄漏给规划层。
|
||||
/// </summary>
|
||||
public interface IQpSolver
|
||||
{
|
||||
QpSolveResult Solve(
|
||||
|
||||
@@ -5,6 +5,9 @@ using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// QP 求解后的数值和诊断快照。调用方必须结合状态判断解向量是否可用，不能将失败输出当作候选轨迹。
|
||||
/// </summary>
|
||||
public sealed class QpSolveResult
|
||||
{
|
||||
public QpSolveResult(
|
||||
|
||||
@@ -5,6 +5,9 @@ using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 供横向或纵向规划器提交给 <see cref="IQpSolver"/> 的二次规划快照；矩阵维度与变量布局必须一致。
|
||||
/// </summary>
|
||||
public sealed class QuadraticProgram
|
||||
{
|
||||
public const double MaximumFiniteBound = 1e30d;
|
||||
|
||||
@@ -6,8 +6,16 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 平滑参考路径中单一前进或倒车方向段的只读视图；EM 规划和投影不得跨越该段的精确换向边界。
|
||||
/// </summary>
|
||||
public sealed class DirectionSegmentView
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个已重基到局部 S=0 的方向段只读视图。
|
||||
/// 参数：points 的 ArcLength 为 m 且方向必须一致，边界与 sourceStartArcLength 为 m；首点与起边界必须位于局部 S=0，终边界须在 1e-12 m 内匹配末点。
|
||||
/// 返回：防御性复制的不可变段；索引、方向、边界或弧长不一致时引发异常。
|
||||
/// </summary>
|
||||
public DirectionSegmentView(
|
||||
int segmentIndex,
|
||||
TravelDirection direction,
|
||||
@@ -48,17 +56,38 @@ public sealed class DirectionSegmentView
|
||||
SourceStartArcLength = sourceStartArcLength;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在原始平滑路径方向段序列中的零基索引。
|
||||
/// </summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 全段唯一的行驶方向；投影和优化不得在本视图内切换方向。
|
||||
/// </summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 局部弧长已重基、按 S（m）非递减排列的平滑参考点只读副本。
|
||||
/// </summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Points { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 局部 S=0 的起始边界，含换向离开语义（若有）。
|
||||
/// </summary>
|
||||
public ReferenceBoundary StartBoundary { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 局部段末端的边界，含目标或换向接近语义（若有）。
|
||||
/// </summary>
|
||||
public ReferenceBoundary EndBoundary { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 本段局部 S=0 在原始平滑路径中的累计弧长，单位 m。
|
||||
/// </summary>
|
||||
public double SourceStartArcLength { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 本段从局部 S=0 到终边界的长度，单位 m。
|
||||
/// </summary>
|
||||
public double LengthMeters { get { return EndBoundary.SegmentLocalS; } }
|
||||
}
|
||||
|
||||
@@ -4,9 +4,16 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Reference-distance terminal chosen before LS without crossing the current direction segment.</summary>
|
||||
/// <summary>
|
||||
/// 在纵横向优化前选定的参考距离终端，保证不会跨越当前方向段。
|
||||
/// 所有参考站量以当前段局部 S（m）表示；边界类型和纵向模式明确滚动延续、接近停车或精确停车语义。
|
||||
/// </summary>
|
||||
public sealed class PlanningHorizonSelection
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建仅供本程序集发布的窗口与停车边界选择。
|
||||
/// 参数：两个参考站均为局部 S（m），boundary/terminal/mode 为已判定枚举；hasStopBoundary 表示终端是否受真实停车边界约束。
|
||||
/// </summary>
|
||||
internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType,
|
||||
EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
|
||||
double stopBoundaryReferenceS, bool hasStopBoundary)
|
||||
@@ -19,25 +26,58 @@ public sealed class PlanningHorizonSelection
|
||||
HasStopBoundary = hasStopBoundary;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 本次优化窗口末端的局部参考弧长 S，单位 m。
|
||||
/// </summary>
|
||||
public double WindowEndReferenceS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 规划输出的终端参考弧长 S，当前与窗口末端相同，单位 m。
|
||||
/// </summary>
|
||||
public double TerminalReferenceS => WindowEndReferenceS;
|
||||
|
||||
/// <summary>
|
||||
/// 窗口末端的边界语义；中途截断时为滚动安全停车边界。
|
||||
/// </summary>
|
||||
public EmBoundaryType WindowEndBoundaryType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 供终端约束使用的目标、换向或滚动终端类型。
|
||||
/// </summary>
|
||||
public EmTerminalType TerminalType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 纵向候选应滚动延续、接近停车边界还是在边界精确停车的模式。
|
||||
/// </summary>
|
||||
public EmLongitudinalMode LongitudinalMode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 实际目标或换向停车边界的局部参考弧长 S，单位 m；无此边界时仍保存段末。
|
||||
/// </summary>
|
||||
public double StopBoundaryReferenceS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 是否存在必须在当前方向段内处理的目标或换向停车边界。
|
||||
/// </summary>
|
||||
public bool HasStopBoundary { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据段边界、初始纵向状态和调度窗口选择 EM 优化地平线。
|
||||
/// 输入速度为 m/s、加速度为 m/s²、距离为 m；无效输入、越界初态或不足以停车的段会以明确状态拒绝。
|
||||
/// </summary>
|
||||
public sealed class PlanningHorizonSelector
|
||||
{
|
||||
/// <summary>
|
||||
/// 段终点、初态限值和停车距离比较使用的局部 S/距离容差，单位 m。
|
||||
/// </summary>
|
||||
private const double BoundaryTolerance = 1e-8d;
|
||||
|
||||
/// <summary>
|
||||
/// 为当前方向段选择不跨界的参考距离窗口和纵向终端模式。
|
||||
/// 参数：currentSegmentReferenceS 为 m，初速为 m/s，初加速度为 m/s²，configuration 提供 m、s 制限值；planningScope 必须为定义的范围枚举。
|
||||
/// 返回：成功时给出 selection；配置/初态非法返回 InvalidInput，停车距离越段返回 StoppingDistanceInsufficient，非法全段终边界返回 InvalidReferencePath。
|
||||
/// </summary>
|
||||
public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
|
||||
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
|
||||
EmPlanningScope planningScope, EmPlannerConfiguration configuration, out PlanningHorizonSelection selection,
|
||||
@@ -138,11 +178,19 @@ public sealed class PlanningHorizonSelector
|
||||
return EmPlanningStatus.Success;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断边界是否要求车辆在当前段内停止。
|
||||
/// 参数：boundaryType 为边界枚举；返回：Goal 或 GearSwitchApproach 时为 true，其余边界允许滚动延续。
|
||||
/// </summary>
|
||||
private static bool IsStopBoundary(EmBoundaryType boundaryType)
|
||||
{
|
||||
return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将参考边界语义映射为纵向终端类型。
|
||||
/// 参数：boundaryType 为边界枚举；返回：目标、任一换向边界分别映射到 Goal、GearSwitch，其余映射为 RollingSafetyStop。
|
||||
/// </summary>
|
||||
private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
|
||||
{
|
||||
return boundaryType == EmBoundaryType.Goal
|
||||
@@ -152,11 +200,19 @@ public sealed class PlanningHorizonSelector
|
||||
: EmTerminalType.RollingSafetyStop;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断配置中的正量是否可用于时间、距离或动力学计算。
|
||||
/// 参数：value 为对应单位的标量；返回：仅有限且严格大于零时为 true。
|
||||
/// </summary>
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0d;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断规划输入是否为有限实数。
|
||||
/// 参数：value 为任意标量；返回：NaN 与无穷均返回 false。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
@@ -2,8 +2,16 @@ using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 方向段的不可变参考边界标识，携带局部与源路径弧长以及边界语义。
|
||||
/// 两个弧长均以 m 计；相等性仅代表同一段、同一局部位置和同一类型，不以源弧长参与判等。
|
||||
/// </summary>
|
||||
public sealed class ReferenceBoundary : IEquatable<ReferenceBoundary>
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建已验证的参考边界。
|
||||
/// 参数：segmentIndex 必须非负，segmentLocalS 与 sourceArcLength 为非负有限 m 制弧长，boundaryType 必须是已定义枚举；非法输入会引发异常。
|
||||
/// </summary>
|
||||
public ReferenceBoundary(int segmentIndex, double segmentLocalS, EmBoundaryType boundaryType, double sourceArcLength)
|
||||
{
|
||||
if (segmentIndex < 0)
|
||||
@@ -21,22 +29,46 @@ public sealed class ReferenceBoundary : IEquatable<ReferenceBoundary>
|
||||
SourceArcLength = sourceArcLength;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 边界所属方向段在路径段序列中的零基索引。
|
||||
/// </summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 边界相对该方向段起点的局部参考弧长 S，单位 m。
|
||||
/// </summary>
|
||||
public double SegmentLocalS { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 边界的目标、换向或普通边界语义。
|
||||
/// </summary>
|
||||
public EmBoundaryType BoundaryType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 边界在未重基原始平滑路径中的累计弧长，单位 m。
|
||||
/// </summary>
|
||||
public double SourceArcLength { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 比较两个边界在优化语义上是否相同。
|
||||
/// 参数：other 可为 null；返回：仅段索引、局部 S（精确 double 比较）和边界类型都相同时为 true，源弧长不参与比较。
|
||||
/// </summary>
|
||||
public bool Equals(ReferenceBoundary other)
|
||||
{
|
||||
return other != null && SegmentIndex == other.SegmentIndex && SegmentLocalS.Equals(other.SegmentLocalS) &&
|
||||
BoundaryType == other.BoundaryType;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 比较任意对象是否为同一参考边界。
|
||||
/// 参数：obj 可为空或非边界对象；返回：仅可转换为 <see cref="ReferenceBoundary"/> 且满足强类型相等性时为 true。
|
||||
/// </summary>
|
||||
public override bool Equals(object obj) { return Equals(obj as ReferenceBoundary); }
|
||||
|
||||
/// <summary>
|
||||
/// 计算与边界相等性一致的哈希值。
|
||||
/// 返回：由段索引、局部 S 与边界类型组成的哈希；源弧长不参与以保持与 <see cref="Equals(ReferenceBoundary)"/> 一致。
|
||||
/// </summary>
|
||||
public override int GetHashCode()
|
||||
{
|
||||
unchecked
|
||||
|
||||
@@ -6,8 +6,15 @@ using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 当前方向段内供一次 EM 优化使用的参考路径切片，保留全局参考站与切片局部弧长的对应关系。
|
||||
/// </summary>
|
||||
public sealed class ReferenceHorizonSlice
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个以当前段起点为基准的参考地平线切片。
|
||||
/// 参数：points 为局部 S（m）有序的参考点，terminalBoundary 必须属于 segment；返回：points 的防御性只读副本，空输入或边界不匹配会被拒绝。
|
||||
/// </summary>
|
||||
public ReferenceHorizonSlice(DirectionSegmentView segment, IReadOnlyList<SmoothedPathPoint> points,
|
||||
ReferenceBoundary terminalBoundary)
|
||||
{
|
||||
@@ -25,17 +32,38 @@ public sealed class ReferenceHorizonSlice
|
||||
TerminalBoundary = terminalBoundary;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 切片所属且不会跨越的单一方向段。
|
||||
/// </summary>
|
||||
public DirectionSegmentView Segment { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 从段起点至精确终端点的参考点只读副本，局部 ArcLength 单位为 m。
|
||||
/// </summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Points { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 切片末端的精确参考边界；中途截断时具有滚动安全停车语义。
|
||||
/// </summary>
|
||||
public ReferenceBoundary TerminalBoundary { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按局部参考距离截取单一方向段，并在请求终端精确插值参考点。
|
||||
/// 切片只包含本段坐标；S、原始弧长与 X/Y 使用 m，航向使用 rad，终端附近以 1e-12 m 判定相等。
|
||||
/// </summary>
|
||||
public static class ReferenceHorizonSlicer
|
||||
{
|
||||
/// <summary>
|
||||
/// 端点复制、精确节点匹配和原段终点判定使用的局部 S 容差，单位 m。
|
||||
/// </summary>
|
||||
private const double Epsilon = 1e-12d;
|
||||
|
||||
/// <summary>
|
||||
/// 将方向段截取到请求的局部终端 S，并追加该终端的精确点。
|
||||
/// 参数：requestedEndSegmentLocalS 为非负有限 m 制局部 S；超过段长的请求夹紧到段末。
|
||||
/// 返回：段末保留原边界，中途截断创建 RollingSafetyStop 边界；空段或非法 S 会引发异常。
|
||||
/// </summary>
|
||||
public static ReferenceHorizonSlice Slice(DirectionSegmentView segment, double requestedEndSegmentLocalS)
|
||||
{
|
||||
if (segment == null)
|
||||
@@ -60,6 +88,10 @@ public static class ReferenceHorizonSlicer
|
||||
return new ReferenceHorizonSlice(segment, points, terminal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 取得给定局部 S 的精确已有点或其相邻点之间的插值点。
|
||||
/// 参数：terminalS 为已夹紧的 m 制局部 S；返回：与节点相差不超过 1e-12 m 时复用节点，否则在正跨度内线性插值。
|
||||
/// </summary>
|
||||
private static SmoothedPathPoint GetExactTerminalPoint(DirectionSegmentView segment, double terminalS)
|
||||
{
|
||||
for (int index = 0; index < segment.Points.Count; index++)
|
||||
@@ -76,6 +108,10 @@ public static class ReferenceHorizonSlicer
|
||||
return segment.Points[segment.Points.Count - 1];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在线性弧长区间内插值一个终端平滑路径点。
|
||||
/// 参数：lower/upper 的 ArcLength 为 m，localS 为其间的 m 制位置；返回：位置、展开航向、曲率和净空线性插值，标记为 Interpolated。
|
||||
/// </summary>
|
||||
private static SmoothedPathPoint Interpolate(SmoothedPathPoint lower, SmoothedPathPoint upper, double localS)
|
||||
{
|
||||
double interval = upper.ArcLength - lower.ArcLength;
|
||||
@@ -98,6 +134,10 @@ public static class ReferenceHorizonSlicer
|
||||
SmoothedPathPointSource.Interpolated);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算同单位标量的线性插值。
|
||||
/// 参数：lower、upper 为端点，fraction 为无单位比例；返回：未夹紧的线性结果。
|
||||
/// </summary>
|
||||
private static double Interpolate(double lower, double upper, double fraction)
|
||||
{
|
||||
return lower + (upper - lower) * fraction;
|
||||
|
||||
@@ -5,8 +5,16 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 将已发布的平滑参考路径拆分为不会跨越换向点的局部方向段视图。
|
||||
/// 每个输出段以局部 S=0 重基（m），同时保留原始累计弧长（m）及目标/换向边界语义；索引覆盖或弧长不连续会被拒绝。
|
||||
/// </summary>
|
||||
public static class ReferencePathSegmenter
|
||||
{
|
||||
/// <summary>
|
||||
/// 从已验证的平滑路径及其方向分段创建局部方向段视图。
|
||||
/// 参数：referencePath 必须含完整 Path 和 Segments，路径弧长为有限 m 制量；返回：按源段索引排列的只读视图，任何漏覆盖、方向或严格递增弧长错误均引发异常。
|
||||
/// </summary>
|
||||
public static IReadOnlyList<DirectionSegmentView> Create(PathSmoothingResult referencePath)
|
||||
{
|
||||
if (referencePath == null || referencePath.Path == null || referencePath.Segments == null ||
|
||||
@@ -65,6 +73,10 @@ public static class ReferencePathSegmenter
|
||||
return new ReadOnlyCollection<DirectionSegmentView>(result);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复制一个源参考点并将其弧长重基到当前方向段局部坐标。
|
||||
/// 参数：source 保存世界 X/Y（m）、航向（rad）和曲率量，localS 为非负段局部 m 制弧长；返回：除 ArcLength 外保持源点几何和来源不变的副本。
|
||||
/// </summary>
|
||||
internal static SmoothedPathPoint CloneAtLocalS(SmoothedPathPoint source, double localS)
|
||||
{
|
||||
return new SmoothedPathPoint(
|
||||
|
||||
@@ -1,726 +0,0 @@
|
||||
# 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
@@ -159,6 +159,11 @@ internal sealed class LocalG2WindowPlanner
|
||||
return;
|
||||
}
|
||||
|
||||
// The tolerance above deliberately accepts boundary-sized round-off. Clamp that
|
||||
// round-off before constructing the strict, non-negative window contract.
|
||||
left = Math.Min(left, availableLeft);
|
||||
right = Math.Min(right, availableRight);
|
||||
|
||||
double start = anchor - left;
|
||||
double end = anchor + right;
|
||||
if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target)
|
||||
|
||||
@@ -58,6 +58,12 @@ public sealed class EmPlanningCoordinator
|
||||
/// <param name="cancellationToken">调用方取消令牌；会与本周期内部令牌链接。</param>
|
||||
/// <returns>完成后给出不可变 <see cref="PlanningCycleResult"/>；只有版本和身份仍当前且结果成功完整时其 <c>Published</c> 为 <see langword="true"/>。</returns>
|
||||
public Task<PlanningCycleResult> PlanLatestAsync(PlanningCycleInput input, CancellationToken cancellationToken)
|
||||
{
|
||||
return PlanLatestAsync(input, cancellationToken, null);
|
||||
}
|
||||
|
||||
internal Task<PlanningCycleResult> PlanLatestAsync(PlanningCycleInput input,
|
||||
CancellationToken cancellationToken, Action<EmTrajectory> publicationCommit)
|
||||
{
|
||||
if (input == null)
|
||||
throw new ArgumentNullException(nameof(input));
|
||||
@@ -75,7 +81,8 @@ public sealed class EmPlanningCoordinator
|
||||
replanPeriodSeconds = input.ReplanPeriodSeconds;
|
||||
}
|
||||
|
||||
return Task.Run(() => CompleteCycle(version, input, cycleCancellation));
|
||||
return Task.Run(() => CompleteCycle(version, input, cycleCancellation, cancellationToken,
|
||||
publicationCommit));
|
||||
}
|
||||
|
||||
/// <summary>从协调器当前已发布的不可变轨迹构建下一周期的安全交接决策。</summary>
|
||||
@@ -100,7 +107,8 @@ public sealed class EmPlanningCoordinator
|
||||
/// <param name="cycleCancellation">本周期与调用方取消状态链接的令牌源。</param>
|
||||
/// <returns>包含原始或 Superseded 规划结果、发布标记和观察器诊断的周期记录。</returns>
|
||||
private PlanningCycleResult CompleteCycle(long version, PlanningCycleInput input,
|
||||
CancellationTokenSource cycleCancellation)
|
||||
CancellationTokenSource cycleCancellation, CancellationToken callerCancellationToken,
|
||||
Action<EmTrajectory> publicationCommit)
|
||||
{
|
||||
EmPlanningResult planned;
|
||||
try
|
||||
@@ -130,11 +138,57 @@ public sealed class EmPlanningCoordinator
|
||||
}
|
||||
else
|
||||
{
|
||||
bool publishable = (planned.Status == EmPlanningStatus.Success || planned.Status == EmPlanningStatus.SuccessWithFallback) &&
|
||||
planned.Trajectory != null;
|
||||
bool publishable = (planned.Status == EmPlanningStatus.Success ||
|
||||
planned.Status == EmPlanningStatus.SuccessWithFallback) && planned.Trajectory != null;
|
||||
if (publishable)
|
||||
publishedTrajectory = planned.Trajectory;
|
||||
result = new PlanningCycleResult(version, input.Identity, planned, publishable, planned.FailureReason);
|
||||
{
|
||||
EmPlanningPublicationDecision decision = input.Request.TryAuthorizePublication(
|
||||
callerCancellationToken, () =>
|
||||
{
|
||||
publicationCommit?.Invoke(planned.Trajectory);
|
||||
publishedTrajectory = planned.Trajectory;
|
||||
});
|
||||
if (decision == EmPlanningPublicationDecision.CallerCancelled)
|
||||
{
|
||||
var cancelled = new EmPlanningResult(EmPlanningStatus.Cancelled, null,
|
||||
"callerCancellation=true;phase=publication");
|
||||
result = new PlanningCycleResult(version, input.Identity, cancelled, false,
|
||||
cancelled.FailureReason);
|
||||
}
|
||||
else if (decision == EmPlanningPublicationDecision.DeadlineExpired)
|
||||
{
|
||||
var expired = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
|
||||
"cycleDeadlineExpired=true;phase=publication;remainingMs=0.000");
|
||||
result = new PlanningCycleResult(version, input.Identity, expired, false,
|
||||
expired.FailureReason);
|
||||
}
|
||||
else
|
||||
{
|
||||
result = new PlanningCycleResult(version, input.Identity, planned, true,
|
||||
planned.FailureReason);
|
||||
}
|
||||
}
|
||||
else if (input.Request.CallerCancellationToken.IsCancellationRequested ||
|
||||
callerCancellationToken.IsCancellationRequested)
|
||||
{
|
||||
var cancelled = new EmPlanningResult(EmPlanningStatus.Cancelled, null,
|
||||
"callerCancellation=true;phase=publication");
|
||||
result = new PlanningCycleResult(version, input.Identity, cancelled, false,
|
||||
cancelled.FailureReason);
|
||||
}
|
||||
else if (input.Request.CycleDeadlineToken.IsCancellationRequested ||
|
||||
input.Request.IsCycleDeadlineExpired())
|
||||
{
|
||||
var expired = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
|
||||
"cycleDeadlineExpired=true;phase=publication;remainingMs=0.000");
|
||||
result = new PlanningCycleResult(version, input.Identity, expired, false,
|
||||
expired.FailureReason);
|
||||
}
|
||||
else
|
||||
{
|
||||
result = new PlanningCycleResult(version, input.Identity, planned, false,
|
||||
planned.FailureReason);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1,68 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace TrajectoryOutputDemo;
|
||||
|
||||
/// <summary>供外部控制模块消费的单个只读轨迹点；全部字段直接来自已验证的 <see cref="EmTrajectoryPoint"/>。</summary>
|
||||
public sealed class ControlTrajectoryPoint
|
||||
{
|
||||
internal ControlTrajectoryPoint(EmTrajectoryPoint source)
|
||||
{
|
||||
TimeFromStartSeconds = source.TimeFromStart;
|
||||
XMeters = source.X;
|
||||
YMeters = source.Y;
|
||||
YawRadians = source.Yaw;
|
||||
SignedLongitudinalVelocityMetersPerSecond = source.SignedLongitudinalVelocity;
|
||||
YawRateRadiansPerSecond = source.YawRate;
|
||||
CurvaturePerMeter = source.VehicleCurvature;
|
||||
Direction = source.Direction;
|
||||
SegmentIndex = source.SegmentIndex;
|
||||
PathSMeters = source.PathS;
|
||||
BoundaryType = source.BoundaryType;
|
||||
}
|
||||
|
||||
public double TimeFromStartSeconds { get; }
|
||||
public double XMeters { get; }
|
||||
public double YMeters { get; }
|
||||
public double YawRadians { get; }
|
||||
public double SignedLongitudinalVelocityMetersPerSecond { get; }
|
||||
public double YawRateRadiansPerSecond { get; }
|
||||
public double CurvaturePerMeter { get; }
|
||||
public TravelDirection Direction { get; }
|
||||
public int SegmentIndex { get; }
|
||||
public double PathSMeters { get; }
|
||||
public EmBoundaryType BoundaryType { get; }
|
||||
}
|
||||
|
||||
/// <summary>轨迹元数据与控制点序列的不可变组合；它不包含也不发送任何硬件命令。</summary>
|
||||
public sealed class ControlTrajectorySequence
|
||||
{
|
||||
internal ControlTrajectorySequence(EmTrajectoryMetadata metadata, IReadOnlyList<ControlTrajectoryPoint> points)
|
||||
{
|
||||
Metadata = metadata ?? throw new ArgumentNullException(nameof(metadata));
|
||||
Points = points ?? throw new ArgumentNullException(nameof(points));
|
||||
}
|
||||
|
||||
public EmTrajectoryMetadata Metadata { get; }
|
||||
public IReadOnlyList<ControlTrajectoryPoint> Points { get; }
|
||||
}
|
||||
|
||||
/// <summary>将不可变 EM 轨迹投影为控制模块可引用的只读序列，不进行采样、插值或底盘协议转换。</summary>
|
||||
public sealed class ControlModuleTrajectoryAdapter
|
||||
{
|
||||
/// <summary>逐点复制公开控制字段；调用方只能在完整非空轨迹上调用此方法。</summary>
|
||||
public ControlTrajectorySequence Create(EmTrajectory trajectory)
|
||||
{
|
||||
if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
|
||||
throw new ArgumentException("必须提供完整非空的 EM 轨迹。", nameof(trajectory));
|
||||
|
||||
var points = new List<ControlTrajectoryPoint>(trajectory.Points.Count);
|
||||
for (int index = 0; index < trajectory.Points.Count; index++)
|
||||
points.Add(new ControlTrajectoryPoint(trajectory.Points[index]));
|
||||
return new ControlTrajectorySequence(trajectory.Metadata,
|
||||
new ReadOnlyCollection<ControlTrajectoryPoint>(points));
|
||||
}
|
||||
}
|
||||
@@ -1,45 +0,0 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
|
||||
namespace TrajectoryOutputDemo;
|
||||
|
||||
/// <summary>Trajplanner_output 控制台入口；只输出规划诊断、轨迹摘要和 CSV 路径。</summary>
|
||||
internal static class Program
|
||||
{
|
||||
private static int Main(string[] args)
|
||||
{
|
||||
if (args.Length != 0)
|
||||
{
|
||||
Console.Error.WriteLine("用法：dotnet run --project TrajectoryOutputDemo.csproj");
|
||||
return 2;
|
||||
}
|
||||
|
||||
string csvPath = Path.Combine(AppContext.BaseDirectory, "output", "trajectory.csv");
|
||||
try
|
||||
{
|
||||
TrajectoryOutputDemoResult result = new TrajectoryOutputDemoRunner().Run(
|
||||
TrajectoryOutputDemoConfiguration.CreateDefault(csvPath));
|
||||
if (!result.Succeeded || result.Trajectory == null || result.ControlTrajectory == null || result.CsvPath == null)
|
||||
{
|
||||
Console.Error.WriteLine(result.Diagnostic);
|
||||
return 1;
|
||||
}
|
||||
|
||||
ControlTrajectoryPoint first = result.ControlTrajectory.Points[0];
|
||||
ControlTrajectoryPoint last = result.ControlTrajectory.Points[result.ControlTrajectory.Points.Count - 1];
|
||||
Console.WriteLine("轨迹 ID：" + result.Trajectory.Metadata.TrajectoryId);
|
||||
Console.WriteLine("生效时间：" + result.Trajectory.Metadata.EffectiveAtUtc.ToString("O"));
|
||||
Console.WriteLine("方向段：" + result.Trajectory.Metadata.SegmentIndex + "，终端类型：" + result.Trajectory.Metadata.TerminalType);
|
||||
Console.WriteLine("轨迹点数：" + result.ControlTrajectory.Points.Count);
|
||||
Console.WriteLine("首点：t=" + first.TimeFromStartSeconds + "s, X=" + first.XMeters + "m, Y=" + first.YMeters + "m");
|
||||
Console.WriteLine("末点：t=" + last.TimeFromStartSeconds + "s, X=" + last.XMeters + "m, Y=" + last.YMeters + "m");
|
||||
Console.WriteLine("CSV：" + result.CsvPath);
|
||||
return 0;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
Console.Error.WriteLine("轨迹 Demo 运行异常：" + exception.Message);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,164 +0,0 @@
|
||||
# Trajplanner_output 真实轨迹序列 Demo
|
||||
|
||||
`Trajplanner_output` 是面向学习、调参和控制模块对接的独立控制台示例。它在一个明确允许的空地图演示场景中依次执行粗路径规划、Local G2 平滑和真实 OSQP EM 规划，最终得到不可变 `EmTrajectory`，导出 CSV，并投影为控制模块可读取的只读轨迹序列。
|
||||
|
||||
它不读取真实定位、传感器或底盘状态，不驱动、转向、制动或换向车辆。演示空地图只用于理解接口与算法链路，不能替代真实作业地图。
|
||||
|
||||
## 模块说明（Module Overview）
|
||||
|
||||
| 模块 | 负责内容 | 不负责内容 |
|
||||
| --- | --- | --- |
|
||||
| `TrajectoryOutputDemoConfiguration` | 集中保存地图、起终点、车辆、曲率、初速和输出路径 | 运行时读取 UI 或硬件参数 |
|
||||
| `TrajectoryOutputDemoRunner` | 串联 CoarsePath、PathSmoothing 与真实 OSQP EM 规划 | 发布硬件命令或伪造失败轨迹 |
|
||||
| `ControlModuleTrajectoryAdapter` | 将 `EmTrajectory` 转为只读控制序列 | 插值、采样或底盘协议转换 |
|
||||
| `TrajectorySequenceExporter` | 原子导出稳定字段顺序的 UTF-8 CSV | 将 CSV 当作车辆命令发送 |
|
||||
| `Program` | 打印轨迹摘要、诊断和 CSV 路径 | 逐行打印轨迹或修改规划结果 |
|
||||
|
||||
唯一的轨迹生成入口是：
|
||||
|
||||
```csharp
|
||||
TrajectoryOutputDemoResult result =
|
||||
new TrajectoryOutputDemoRunner().Run(configuration);
|
||||
```
|
||||
|
||||
## 文件结构（File Structure）
|
||||
|
||||
```text
|
||||
Trajplanner_output/
|
||||
├── TrajectoryOutputDemo.csproj # 独立 net10.0-windows 控制台项目
|
||||
├── Program.cs # 运行入口和轨迹摘要
|
||||
├── TrajectoryOutputDemoConfiguration.cs # 唯一调参位置
|
||||
├── TrajectoryOutputDemoRunner.cs # 粗路径、平滑和真实 EM 规划编排
|
||||
├── ControlModuleTrajectoryAdapter.cs # EM 轨迹到控制只读序列的映射
|
||||
├── TrajectorySequenceExporter.cs # 原子 CSV 导出
|
||||
├── README.md # 本说明
|
||||
└── Tests/
|
||||
├── TrajectoryOutputDemo.Tests.csproj # 真实 OSQP 自检项目
|
||||
└── Program.cs # CSV 和逐点映射契约验证
|
||||
```
|
||||
|
||||
## 轨迹数据流（Trajectory Data Flow）
|
||||
|
||||
```text
|
||||
TrajectoryOutputDemoConfiguration
|
||||
▼
|
||||
PlanningMapRequest（显式允许的空地图）
|
||||
▼
|
||||
CoarsePathPlanningService
|
||||
▼
|
||||
PathSmoothingService（Local G2）
|
||||
▼
|
||||
EmPlanningService(new OsqpNativeSolver())
|
||||
▼
|
||||
EmPlanningResult
|
||||
│ 仅 Success / SuccessWithFallback 且 Trajectory 非空
|
||||
▼
|
||||
EmTrajectory.Points + Metadata
|
||||
├── ControlModuleTrajectoryAdapter
|
||||
│ └── ControlTrajectorySequence
|
||||
└── TrajectorySequenceExporter
|
||||
└── output/trajectory.csv
|
||||
```
|
||||
|
||||
中间任一阶段失败都会立刻停止，打印状态与诊断，且不会导出部分或伪造的 CSV。
|
||||
|
||||
## 全链路可视化（Offline Flow Demo）
|
||||
|
||||
双击打开 [trajectory-planning-flow-demo.html](trajectory-planning-flow-demo.html)，可在同一米制地图中按阶段查看地图、Hybrid A* 粗路径、Local G2 平滑路径、`EmTrajectory` 和控制层采样命令。该页面使用内置说明数据，帮助理解版本身份与数据边界；它不调用 OSQP、定位、硬件或真实控制器。
|
||||
|
||||
控制器只消费已验证完整轨迹的采样结果或执行层导出的 `TrajectoryControlCommand`。粗路径、平滑路径、地图和规划诊断属于观测、回放和问题定位证据链，不是逐周期的底盘命令。
|
||||
|
||||
## 运行（Run）
|
||||
|
||||
在仓库根目录执行：
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj
|
||||
```
|
||||
|
||||
成功时控制台会输出轨迹 ID、生效时间、方向段、终端类型、轨迹点数量、首末点和 CSV 绝对路径。默认 CSV 位于 Demo 程序输出目录下的 `output/trajectory.csv`。
|
||||
|
||||
运行自检：
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/Tests/TrajectoryOutputDemo.Tests.csproj
|
||||
```
|
||||
|
||||
自检使用真实 OSQP，不使用假求解器；它验证成功轨迹存在、CSV 存在、CSV 表头稳定，且控制序列点数与 `EmTrajectory.Points` 相同。
|
||||
|
||||
## 调参（Configuration）
|
||||
|
||||
所有 Demo 参数集中在 `TrajectoryOutputDemoConfiguration.CreateDefault(csvOutputPath)`：
|
||||
|
||||
| 参数 | 单位 | 默认值 | 作用 |
|
||||
| --- | --- | ---: | --- |
|
||||
| `MapBounds` | mm | `0..6000 × 0..4000` | 演示地图范围 |
|
||||
| `MapResolutionMillimeters` | mm | `50` | 占据栅格分辨率 |
|
||||
| `Start` / `Goal` | m, rad | `(1,1,0)` / `(3,1,0)` | 车辆几何中心位姿 |
|
||||
| `VehicleLengthMeters` / `VehicleWidthMeters` | m | `0.80 / 0.60` | 车辆矩形尺寸 |
|
||||
| `SafetyMarginMeters` | m | `0.05` | 车辆外扩安全余量 |
|
||||
| `MaximumCurvaturePerMeter` | 1/m | `1 / 1.20` | 最大允许曲率 |
|
||||
| `InitialSignedSpeedMetersPerSecond` | m/s | `0` | 初始带符号纵向速度 |
|
||||
| `CsvOutputPath` | 文件路径 | 运行时指定 | 完整 CSV 输出位置 |
|
||||
|
||||
调整参数后应重新运行自检。若更换为真实作业场景，必须将 `ObstacleSources` 替换为有效地图来源，并取消演示空地图策略。
|
||||
|
||||
## CSV 契约（CSV Contract）
|
||||
|
||||
首行固定为：
|
||||
|
||||
```text
|
||||
time_s,x_m,y_m,yaw_rad,signed_velocity_mps,yaw_rate_radps,curvature_per_m,direction,segment_index,path_s_m,boundary_type
|
||||
```
|
||||
|
||||
每一行与一个不可变 `EmTrajectoryPoint` 一一对应。CSV 使用 UTF-8 无 BOM 和不受系统区域设置影响的小数点格式；导出过程先写临时文件，再替换最终文件，避免读取方获得半写入内容。
|
||||
|
||||
| 字段 | 单位 / 语义 |
|
||||
| --- | --- |
|
||||
| `time_s` | 自轨迹生效时刻起的秒数，严格递增 |
|
||||
| `x_m`, `y_m`, `yaw_rad` | 世界位置和航向 |
|
||||
| `signed_velocity_mps` | 带符号纵向速度；前进为正、倒车为负 |
|
||||
| `yaw_rate_radps` | 世界航向角速度，不是转向角 |
|
||||
| `curvature_per_m` | 车辆曲率 |
|
||||
| `direction`, `segment_index`, `boundary_type` | 方向段与终端边界语义 |
|
||||
|
||||
## 控制模块对接（Control Module Integration）
|
||||
|
||||
控制模块优先以项目引用依赖规划库：
|
||||
|
||||
```xml
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\ClumsyPilot\ClumsyPilot.csproj"
|
||||
AdditionalProperties="ExcludeLegacyAutoAvoidance=true" />
|
||||
</ItemGroup>
|
||||
```
|
||||
|
||||
对接方应接收上层规划服务提供的完整 `EmTrajectory`，再使用适配器读取不可变序列：
|
||||
|
||||
```csharp
|
||||
var sequence = new ControlModuleTrajectoryAdapter().Create(trajectory);
|
||||
|
||||
foreach (ControlTrajectoryPoint point in sequence.Points)
|
||||
{
|
||||
SendReference(
|
||||
point.TimeFromStartSeconds,
|
||||
point.XMeters,
|
||||
point.YMeters,
|
||||
point.YawRadians,
|
||||
point.SignedLongitudinalVelocityMetersPerSecond,
|
||||
point.YawRateRadiansPerSecond);
|
||||
}
|
||||
```
|
||||
|
||||
`SendReference` 是控制模块自己的协议适配函数，不是本项目 API。控制模块必须保留 `sequence.Metadata`，并依据方向、换向边界和生效时间实施自己的安全策略。不要把 `VelocityX`、`VelocityY` 当作底盘命令；本 Demo 也不实现换向确认、制动和硬件通信。
|
||||
|
||||
## 失败与限制（Failures and Limits）
|
||||
|
||||
| 现象 | 原因 | 处理 |
|
||||
| --- | --- | --- |
|
||||
| 没有 CSV 输出 | 粗路径、平滑、OSQP 或 EM 验证未成功 | 阅读控制台诊断；不能将失败当作部分轨迹 |
|
||||
| OSQP 加载失败 | `osqp.dll` 未随运行输出部署，或 Windows x64 运行时不匹配 | 使用项目引用构建，检查输出目录的 OSQP 文件 |
|
||||
| 控制模块轨迹跳变 | 忽略轨迹 ID、生效时间或方向段 | 持有 `EmTrajectoryMetadata` 并按控制周期安全接管 |
|
||||
| 想用于真实车辆 | Demo 仍使用显式空地图 | 先接入真实障碍物来源、状态快照和硬件安全审查 |
|
||||
|
||||
本项目是“如何得到并交给他人轨迹序列”的学习/接口示例，不是经过现场认证的车辆控制器。
|
||||
@@ -1,45 +0,0 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
using TrajectoryOutputDemo;
|
||||
|
||||
namespace TrajectoryOutputDemoTests;
|
||||
|
||||
internal static class Program
|
||||
{
|
||||
private static int Main()
|
||||
{
|
||||
string temporaryDirectory = Path.Combine(Path.GetTempPath(), "trajectory-output-demo-tests", Guid.NewGuid().ToString("N"));
|
||||
try
|
||||
{
|
||||
Directory.CreateDirectory(temporaryDirectory);
|
||||
string csvPath = Path.Combine(temporaryDirectory, "trajectory.csv");
|
||||
var configuration = TrajectoryOutputDemoConfiguration.CreateDefault(csvPath);
|
||||
TrajectoryOutputDemoResult result = new TrajectoryOutputDemoRunner().Run(configuration);
|
||||
|
||||
if (!result.Succeeded)
|
||||
throw new InvalidOperationException("真实 OSQP 规划未成功：" + result.Diagnostic);
|
||||
if (!File.Exists(csvPath))
|
||||
throw new InvalidOperationException("成功规划必须导出 CSV。");
|
||||
ControlTrajectorySequence sequence = result.ControlTrajectory ??
|
||||
throw new InvalidOperationException("成功规划必须提供控制模块序列。");
|
||||
var trajectory = result.Trajectory ??
|
||||
throw new InvalidOperationException("成功规划必须提供 EM 轨迹。");
|
||||
if (sequence.Points.Count != trajectory.Points.Count)
|
||||
throw new InvalidOperationException("控制序列必须逐点对应 EM 轨迹。");
|
||||
|
||||
string[] lines = File.ReadAllLines(csvPath);
|
||||
if (lines.Length < 2)
|
||||
throw new InvalidOperationException("CSV 必须包含表头和至少一个轨迹点。");
|
||||
if (lines[0] != "time_s,x_m,y_m,yaw_rad,signed_velocity_mps,yaw_rate_radps,curvature_per_m,direction,segment_index,path_s_m,boundary_type")
|
||||
throw new InvalidOperationException("CSV 表头不符合控制模块契约。");
|
||||
|
||||
Console.WriteLine("PASS trajectory-output-demo");
|
||||
return 0;
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (Directory.Exists(temporaryDirectory))
|
||||
Directory.Delete(temporaryDirectory, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
-11
@@ -1,11 +0,0 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
<PropertyGroup>
|
||||
<OutputType>Exe</OutputType>
|
||||
<TargetFramework>net10.0-windows</TargetFramework>
|
||||
<ImplicitUsings>disable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\TrajectoryOutputDemo.csproj" />
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
@@ -1,15 +0,0 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
<PropertyGroup>
|
||||
<OutputType>Exe</OutputType>
|
||||
<TargetFramework>net10.0-windows</TargetFramework>
|
||||
<ImplicitUsings>disable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\..\ClumsyPilot.csproj"
|
||||
AdditionalProperties="ExcludeLegacyAutoAvoidance=true" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Compile Remove="Tests\**\*.cs" />
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
-63
@@ -1,63 +0,0 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace TrajectoryOutputDemo;
|
||||
|
||||
/// <summary>
|
||||
/// Trajplanner_output 的唯一演示调参入口。
|
||||
/// 地图边界与分辨率使用 mm；车辆、起终点位置和安全余量使用 m；航向使用 rad；输出路径为 CSV 文件位置。
|
||||
/// </summary>
|
||||
public sealed class TrajectoryOutputDemoConfiguration
|
||||
{
|
||||
/// <summary>演示地图的世界边界；空地图只允许用于算法学习，不能替代真实障碍物来源。</summary>
|
||||
public MapBoundsMm MapBounds { get; init; } = new MapBoundsMm(0f, 6000f, 0f, 4000f);
|
||||
|
||||
/// <summary>演示占据栅格分辨率，单位 mm。</summary>
|
||||
public float MapResolutionMillimeters { get; init; }
|
||||
|
||||
/// <summary>车辆几何中心起点，位置单位 m、航向单位 rad。</summary>
|
||||
public Pose2D Start { get; init; } = new Pose2D(1d, 1d, 0d);
|
||||
|
||||
/// <summary>车辆几何中心目标点，位置单位 m、航向单位 rad。</summary>
|
||||
public Pose2D Goal { get; init; } = new Pose2D(3d, 1d, 0d);
|
||||
|
||||
/// <summary>车辆长度，单位 m。</summary>
|
||||
public double VehicleLengthMeters { get; init; }
|
||||
|
||||
/// <summary>车辆宽度，单位 m。</summary>
|
||||
public double VehicleWidthMeters { get; init; }
|
||||
|
||||
/// <summary>车辆矩形外的附加安全余量，单位 m。</summary>
|
||||
public double SafetyMarginMeters { get; init; }
|
||||
|
||||
/// <summary>车辆最大曲率，单位 1/m。</summary>
|
||||
public double MaximumCurvaturePerMeter { get; init; }
|
||||
|
||||
/// <summary>轨迹开始时的带符号纵向速度，单位 m/s；前进为正、倒车为负。</summary>
|
||||
public double InitialSignedSpeedMetersPerSecond { get; init; }
|
||||
|
||||
/// <summary>输出 CSV 的绝对或相对路径。</summary>
|
||||
public string CsvOutputPath { get; init; } = string.Empty;
|
||||
|
||||
/// <summary>创建可直线通行的最小真实 EM 演示配置。</summary>
|
||||
public static TrajectoryOutputDemoConfiguration CreateDefault(string csvOutputPath)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(csvOutputPath))
|
||||
throw new ArgumentException("CSV 输出路径不能为空。", nameof(csvOutputPath));
|
||||
|
||||
return new TrajectoryOutputDemoConfiguration
|
||||
{
|
||||
MapBounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
MapResolutionMillimeters = 50f,
|
||||
Start = new Pose2D(1d, 1d, 0d),
|
||||
Goal = new Pose2D(3d, 1d, 0d),
|
||||
VehicleLengthMeters = 0.80d,
|
||||
VehicleWidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
InitialSignedSpeedMetersPerSecond = 0d,
|
||||
CsvOutputPath = csvOutputPath,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -1,99 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
namespace TrajectoryOutputDemo;
|
||||
|
||||
/// <summary>一次 Demo 运行的结果；成功时同时提供原始 EM 轨迹、控制只读序列和已完成的 CSV 路径。</summary>
|
||||
public sealed class TrajectoryOutputDemoResult
|
||||
{
|
||||
private TrajectoryOutputDemoResult(bool succeeded, string diagnostic, EmTrajectory? trajectory,
|
||||
ControlTrajectorySequence? controlTrajectory, string? csvPath)
|
||||
{
|
||||
Succeeded = succeeded;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
Trajectory = trajectory;
|
||||
ControlTrajectory = controlTrajectory;
|
||||
CsvPath = csvPath;
|
||||
}
|
||||
|
||||
public bool Succeeded { get; }
|
||||
public string Diagnostic { get; }
|
||||
public EmTrajectory? Trajectory { get; }
|
||||
public ControlTrajectorySequence? ControlTrajectory { get; }
|
||||
public string? CsvPath { get; }
|
||||
|
||||
internal static TrajectoryOutputDemoResult Failure(string diagnostic) => new(false, diagnostic, null, null, null);
|
||||
internal static TrajectoryOutputDemoResult Success(EmTrajectory trajectory, ControlTrajectorySequence sequence, string csvPath) =>
|
||||
new(true, string.Empty, trajectory, sequence, csvPath);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 编排一次真实轨迹输出：粗路径、Local G2 平滑、真实 OSQP EM 规划、控制序列投影和 CSV 导出。
|
||||
/// 它只处理冻结的演示输入，绝不读取硬件状态或发送控制命令。
|
||||
/// </summary>
|
||||
public sealed class TrajectoryOutputDemoRunner
|
||||
{
|
||||
/// <summary>在配置定义的演示场景运行一次完整规划；失败时不导出任何轨迹文件。</summary>
|
||||
public TrajectoryOutputDemoResult Run(TrajectoryOutputDemoConfiguration configuration)
|
||||
{
|
||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||
|
||||
CoarsePathPlanningJob job = CreateCoarseJob(configuration);
|
||||
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper().Bootstrap(job, CancellationToken.None);
|
||||
if (!bootstrap.Succeeded)
|
||||
return TrajectoryOutputDemoResult.Failure("粗路径或平滑阶段失败：" + bootstrap.FailureReason);
|
||||
if (bootstrap.Segments.Count == 0)
|
||||
return TrajectoryOutputDemoResult.Failure("平滑路径没有可供 EM 规划的方向段。");
|
||||
|
||||
DateTimeOffset now = DateTimeOffset.UtcNow;
|
||||
DirectionSegmentView segment = bootstrap.Segments[0];
|
||||
var state = new VehicleMotionState(configuration.Start, configuration.InitialSignedSpeedMetersPerSecond,
|
||||
0d, now, 1L);
|
||||
var request = new EmPlanningRequest(bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state,
|
||||
EmPlannerConfiguration.CreateDefault(), segment.SegmentIndex, null, now, now,
|
||||
"trajectory-output-demo-" + now.ToUnixTimeMilliseconds(), "trajectory-output-demo-reference", string.Empty,
|
||||
EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.FullDirectionSegment);
|
||||
EmPlanningResult result = new EmPlanningService(new OsqpNativeSolver()).Plan(request, CancellationToken.None);
|
||||
bool accepted = (result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback) &&
|
||||
result.Trajectory != null && result.Trajectory.Points.Count > 0;
|
||||
if (!accepted)
|
||||
return TrajectoryOutputDemoResult.Failure("EM 规划失败：" + result.Status + "；" + result.FailureReason);
|
||||
|
||||
EmTrajectory trajectory = result.Trajectory!;
|
||||
ControlTrajectorySequence sequence = new ControlModuleTrajectoryAdapter().Create(trajectory);
|
||||
string csvPath = new TrajectorySequenceExporter().Export(sequence, configuration.CsvOutputPath);
|
||||
return TrajectoryOutputDemoResult.Success(trajectory, sequence, csvPath);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateCoarseJob(TrajectoryOutputDemoConfiguration configuration)
|
||||
{
|
||||
return new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = configuration.MapBounds,
|
||||
ResolutionMm = configuration.MapResolutionMillimeters,
|
||||
ObstacleSources = Array.Empty<IMapObstacleSource>(),
|
||||
AllowExplicitEmptyMap = true,
|
||||
},
|
||||
Start = configuration.Start,
|
||||
Goal = configuration.Goal,
|
||||
Vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = configuration.VehicleLengthMeters,
|
||||
WidthMeters = configuration.VehicleWidthMeters,
|
||||
SafetyMarginMeters = configuration.SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = configuration.MaximumCurvaturePerMeter,
|
||||
},
|
||||
Configuration = new HybridAStarConfiguration(),
|
||||
StartDirection = TravelDirection.Forward,
|
||||
GoalDirection = GoalDirectionConstraint.Forward,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -1,46 +0,0 @@
|
||||
using System;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
|
||||
namespace TrajectoryOutputDemo;
|
||||
|
||||
/// <summary>将完整控制轨迹以稳定字段顺序导出为 UTF-8 CSV；只在完整内容写完后替换目标文件。</summary>
|
||||
public sealed class TrajectorySequenceExporter
|
||||
{
|
||||
/// <summary>控制模块 CSV 的固定表头；数值字段均按不受区域设置影响的点号格式写出。</summary>
|
||||
public const string Header = "time_s,x_m,y_m,yaw_rad,signed_velocity_mps,yaw_rate_radps,curvature_per_m,direction,segment_index,path_s_m,boundary_type";
|
||||
|
||||
/// <summary>将完整序列原子写入目标路径，防止读方看到半写入的 CSV。</summary>
|
||||
public string Export(ControlTrajectorySequence sequence, string outputPath)
|
||||
{
|
||||
if (sequence == null) throw new ArgumentNullException(nameof(sequence));
|
||||
if (string.IsNullOrWhiteSpace(outputPath)) throw new ArgumentException("输出路径不能为空。", nameof(outputPath));
|
||||
|
||||
string fullPath = Path.GetFullPath(outputPath);
|
||||
string? directory = Path.GetDirectoryName(fullPath);
|
||||
if (!string.IsNullOrEmpty(directory)) Directory.CreateDirectory(directory);
|
||||
string temporaryPath = fullPath + ".tmp";
|
||||
|
||||
using (var writer = new StreamWriter(temporaryPath, false, new UTF8Encoding(false)))
|
||||
{
|
||||
writer.WriteLine(Header);
|
||||
for (int index = 0; index < sequence.Points.Count; index++)
|
||||
{
|
||||
ControlTrajectoryPoint point = sequence.Points[index];
|
||||
writer.WriteLine(string.Join(",", new[]
|
||||
{
|
||||
Number(point.TimeFromStartSeconds), Number(point.XMeters), Number(point.YMeters), Number(point.YawRadians),
|
||||
Number(point.SignedLongitudinalVelocityMetersPerSecond), Number(point.YawRateRadiansPerSecond),
|
||||
Number(point.CurvaturePerMeter), point.Direction.ToString(), point.SegmentIndex.ToString(CultureInfo.InvariantCulture),
|
||||
Number(point.PathSMeters), point.BoundaryType.ToString(),
|
||||
}));
|
||||
}
|
||||
}
|
||||
|
||||
File.Move(temporaryPath, fullPath, true);
|
||||
return fullPath;
|
||||
}
|
||||
|
||||
private static string Number(double value) => value.ToString("G17", CultureInfo.InvariantCulture);
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>将冻结的单方向段 EM 轨迹转换为几何控制器使用的 SI 单位有符号速度轨迹。</summary>
|
||||
public sealed class EmControlTrajectoryAdapter
|
||||
{
|
||||
private const double MinimumSegmentLengthMeters = 1e-6d;
|
||||
|
||||
/// <summary>按世界位置重建从零开始的弧长，同时保留航向、曲率以及前进为正倒车为负的参考速度。</summary>
|
||||
public Trajectory2D Create(EmTrajectory trajectory)
|
||||
{
|
||||
if (trajectory == null)
|
||||
throw new ArgumentNullException(nameof(trajectory));
|
||||
|
||||
var points = new List<TrajectoryPoint>(trajectory.Points.Count);
|
||||
double arcLengthMeters = 0d;
|
||||
for (int index = 0; index < trajectory.Points.Count; index++)
|
||||
{
|
||||
EmTrajectoryPoint source = trajectory.Points[index];
|
||||
var pose = new Pose2D(source.X, source.Y, source.Yaw);
|
||||
var converted = new TrajectoryPoint(arcLengthMeters, pose,
|
||||
source.VehicleCurvature, source.SignedLongitudinalVelocity);
|
||||
|
||||
if (points.Count == 0)
|
||||
{
|
||||
points.Add(converted);
|
||||
continue;
|
||||
}
|
||||
|
||||
TrajectoryPoint previous = points[points.Count - 1];
|
||||
double deltaX = pose.XMeters - previous.PoseInWorld.XMeters;
|
||||
double deltaY = pose.YMeters - previous.PoseInWorld.YMeters;
|
||||
double distanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (distanceMeters < MinimumSegmentLengthMeters)
|
||||
{
|
||||
points[points.Count - 1] = new TrajectoryPoint(
|
||||
previous.ArcLengthMeters, pose, source.VehicleCurvature,
|
||||
source.SignedLongitudinalVelocity);
|
||||
continue;
|
||||
}
|
||||
|
||||
arcLengthMeters += distanceMeters;
|
||||
points.Add(new TrajectoryPoint(arcLengthMeters, pose,
|
||||
source.VehicleCurvature, source.SignedLongitudinalVelocity));
|
||||
}
|
||||
|
||||
if (points.Count < 2)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"EM轨迹至少需要包含两个不同位置的有效控制点。", nameof(trajectory));
|
||||
}
|
||||
|
||||
return new Trajectory2D(points);
|
||||
}
|
||||
}
|
||||
+697
@@ -0,0 +1,697 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Globalization;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
using MyParking.Shared;
|
||||
using TrajectoryPlanningVisualization;
|
||||
using PlanningPose2D = MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>规划一次并冻结首个 EM 方向段，再使用现有几何控制器完成实车状态反馈闭环。</summary>
|
||||
[MovementTest(name = "EM闭环测试")]
|
||||
public sealed class EmClosedLoopMovementTest : MovementTest
|
||||
{
|
||||
private const int MaximumManualObstacleCount = 20;
|
||||
private static long nextObstacleSnapshotVersion;
|
||||
|
||||
public double GoalXmm = double.NaN;
|
||||
public double GoalYmm = double.NaN;
|
||||
public double GoalYawDeg;
|
||||
public double MapPaddingMeters = 2d;
|
||||
public float MapResolutionMm = 50f;
|
||||
public double SolverTimeoutSeconds = 5d;
|
||||
public int MaximumOsqpIterations = 100000;
|
||||
public double OutputTimeStepSeconds = 0.10d;
|
||||
public double VehicleLengthMeters = 0.80d;
|
||||
public double VehicleWidthMeters = 0.60d;
|
||||
public double SafetyMarginMeters = 0.05d;
|
||||
public double MaximumCurvaturePerMeter = 1d / 1.20d;
|
||||
public bool EnableWebVisualization = true;
|
||||
public bool AutoOpenWebVisualization = true;
|
||||
public int WebVisualizationPort;
|
||||
public double WebRefreshRateHz = 10d;
|
||||
public int VisualizationHistoryCycleLimit = 60;
|
||||
public bool EnableNativePainterVisualization = true;
|
||||
public double MaximumCommandSpeedMetersPerSecond = 1.00d;
|
||||
public double MaximumDistanceToTrajectoryMeters = 0.30d;
|
||||
public double ExecutionTimeoutSeconds = 120d;
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
|
||||
/// <summary>读取冻结输入并启动一次规划、一次控制器接管的后台会话。</summary>
|
||||
public override void Test()
|
||||
{
|
||||
try
|
||||
{
|
||||
double goalXMillimeters = GoalXmm;
|
||||
double goalYMillimeters = GoalYmm;
|
||||
double goalYawDegrees = GoalYawDeg;
|
||||
if (!IsFinite(goalXMillimeters) || !IsFinite(goalYMillimeters))
|
||||
{
|
||||
goalXMillimeters = ReadFiniteInput("EM闭环终点 X（世界 mm）");
|
||||
goalYMillimeters = ReadFiniteInput("EM闭环终点 Y（世界 mm）");
|
||||
goalYawDegrees = ReadFiniteInput("EM闭环终点航向（世界 deg）");
|
||||
}
|
||||
|
||||
EnsureFinite(goalXMillimeters, nameof(GoalXmm));
|
||||
EnsureFinite(goalYMillimeters, nameof(GoalYmm));
|
||||
EnsureFinite(goalYawDegrees, nameof(GoalYawDeg));
|
||||
EnsurePositive(MaximumCommandSpeedMetersPerSecond,
|
||||
nameof(MaximumCommandSpeedMetersPerSecond));
|
||||
EnsurePositive(MaximumDistanceToTrajectoryMeters,
|
||||
nameof(MaximumDistanceToTrajectoryMeters));
|
||||
EnsurePositive(ExecutionTimeoutSeconds, nameof(ExecutionTimeoutSeconds));
|
||||
EnsurePositive(WheelAlignmentToleranceDegrees,
|
||||
nameof(WheelAlignmentToleranceDegrees));
|
||||
|
||||
var settings = new TrajectoryObservationSettings
|
||||
{
|
||||
PlanningScope = EmPlanningScope.FullDirectionSegment,
|
||||
MapPaddingMeters = MapPaddingMeters,
|
||||
MapResolutionMillimeters = MapResolutionMm,
|
||||
SolverTimeoutSeconds = SolverTimeoutSeconds,
|
||||
MaximumOsqpIterations = MaximumOsqpIterations,
|
||||
OutputTimeStepSeconds = OutputTimeStepSeconds,
|
||||
VehicleLengthMeters = VehicleLengthMeters,
|
||||
VehicleWidthMeters = VehicleWidthMeters,
|
||||
SafetyMarginMeters = SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
|
||||
EnableWebVisualization = EnableWebVisualization,
|
||||
AutoOpenWebVisualization = AutoOpenWebVisualization,
|
||||
WebVisualizationPort = WebVisualizationPort,
|
||||
WebRefreshRateHz = WebRefreshRateHz,
|
||||
VisualizationHistoryCycleLimit = VisualizationHistoryCycleLimit,
|
||||
EnableNativePainterVisualization = EnableNativePainterVisualization,
|
||||
}.CreateValidatedSnapshot();
|
||||
|
||||
IReadOnlyList<TrajectoryObservationObstacle> obstacles = ReadManualObstacles();
|
||||
long obstacleSnapshotVersion = obstacles.Count == 0
|
||||
? 0L
|
||||
: Interlocked.Increment(ref nextObstacleSnapshotVersion);
|
||||
var goal = new PlanningPose2D(
|
||||
goalXMillimeters / 1000d,
|
||||
goalYMillimeters / 1000d,
|
||||
goalYawDegrees * Math.PI / 180d);
|
||||
|
||||
EmClosedLoopMovementTestRunner.Start(goal, settings, obstacles,
|
||||
obstacleSnapshotVersion, MaximumCommandSpeedMetersPerSecond,
|
||||
MaximumDistanceToTrajectoryMeters, ExecutionTimeoutSeconds,
|
||||
WheelAlignmentToleranceDegrees);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
EmClosedLoopMovementTestRunner.ShowFailure("测试未启动", exception);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>取消尚未完成的规划并停止正在运行的控制任务。</summary>
|
||||
public override void TestStop()
|
||||
{
|
||||
EmClosedLoopMovementTestRunner.Stop();
|
||||
}
|
||||
|
||||
private static IReadOnlyList<TrajectoryObservationObstacle> ReadManualObstacles()
|
||||
{
|
||||
int count = ReadBoundedIntegerInput("EM闭环手动障碍物数量（0-20）",
|
||||
0, MaximumManualObstacleCount);
|
||||
var obstacles = new List<TrajectoryObservationObstacle>(count);
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
string label = "障碍物 " + (index + 1).ToString(CultureInfo.InvariantCulture);
|
||||
int kind = ReadBoundedIntegerInput(label + " 类型（1=圆形，2=轴对齐矩形）", 1, 2);
|
||||
double centerXMillimeters = ReadFiniteInput(label + " 中心 X（世界 mm）");
|
||||
double centerYMillimeters = ReadFiniteInput(label + " 中心 Y（世界 mm）");
|
||||
if (kind == 1)
|
||||
{
|
||||
double radiusMillimeters = ReadPositiveFiniteInput(label + " 半径（mm）");
|
||||
obstacles.Add(TrajectoryObservationObstacle.Circle(
|
||||
centerXMillimeters, centerYMillimeters, radiusMillimeters));
|
||||
}
|
||||
else
|
||||
{
|
||||
double lengthXMillimeters = ReadPositiveFiniteInput(label + " X 方向长度（mm）");
|
||||
double widthYMillimeters = ReadPositiveFiniteInput(label + " Y 方向宽度（mm）");
|
||||
obstacles.Add(TrajectoryObservationObstacle.Rectangle(
|
||||
centerXMillimeters - lengthXMillimeters / 2d,
|
||||
centerXMillimeters + lengthXMillimeters / 2d,
|
||||
centerYMillimeters - widthYMillimeters / 2d,
|
||||
centerYMillimeters + widthYMillimeters / 2d));
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
EnsurePositive(value, prompt);
|
||||
return value;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
EnsureFinite(value, prompt);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static void EnsurePositive(double value, string name)
|
||||
{
|
||||
EnsureFinite(value, name);
|
||||
if (value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(name, "输入必须是正数：" + name);
|
||||
}
|
||||
|
||||
private static void EnsureFinite(double value, string name)
|
||||
{
|
||||
if (!IsFinite(value))
|
||||
throw new ArgumentException("输入必须是有限数字：" + name);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>拥有单次规划和单个控制任务的会话生命周期，并保证停止操作幂等。</summary>
|
||||
internal static class EmClosedLoopMovementTestRunner
|
||||
{
|
||||
private static readonly object SessionSync = new object();
|
||||
private static CancellationTokenSource activeCancellation;
|
||||
private static Task activeTask;
|
||||
private static DriveTask activeDriveTask;
|
||||
private static TrajectoryObservationPresentation activePresentation;
|
||||
private static TrajectoryObservationVisualizationPublisher activeWebPublisher;
|
||||
private static long nextSessionId;
|
||||
private static long activeSessionId;
|
||||
private static long stateSequence;
|
||||
|
||||
internal static void Start(PlanningPose2D goal, TrajectoryObservationSettings settings,
|
||||
IReadOnlyList<TrajectoryObservationObstacle> obstacles, long obstacleSnapshotVersion,
|
||||
double maximumCommandSpeedMetersPerSecond,
|
||||
double maximumDistanceToTrajectoryMeters,
|
||||
double executionTimeoutSeconds,
|
||||
float wheelAlignmentToleranceDegrees)
|
||||
{
|
||||
if (goal == null) throw new ArgumentNullException(nameof(goal));
|
||||
if (settings == null) throw new ArgumentNullException(nameof(settings));
|
||||
if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
|
||||
|
||||
var cancellation = new CancellationTokenSource();
|
||||
CancellationTokenSource previousCancellation;
|
||||
DriveTask previousDriveTask;
|
||||
TrajectoryObservationPresentation previousPresentation;
|
||||
TrajectoryObservationVisualizationPublisher previousWebPublisher;
|
||||
long sessionId;
|
||||
lock (SessionSync)
|
||||
{
|
||||
previousCancellation = activeCancellation;
|
||||
previousDriveTask = activeDriveTask;
|
||||
previousPresentation = activePresentation;
|
||||
previousWebPublisher = activeWebPublisher;
|
||||
activeCancellation = cancellation;
|
||||
activeDriveTask = null;
|
||||
activePresentation = null;
|
||||
activeWebPublisher = null;
|
||||
activeTask = null;
|
||||
activeSessionId = sessionId = ++nextSessionId;
|
||||
}
|
||||
|
||||
Cancel(previousCancellation);
|
||||
StopDriveTask(previousDriveTask);
|
||||
ClearPresentation(previousPresentation);
|
||||
StopWebPublisher(previousWebPublisher);
|
||||
PrintStatus("会话 " + sessionId.ToString(CultureInfo.InvariantCulture) +
|
||||
" 已启动；车辆运动前将只规划一次。");
|
||||
|
||||
Task task = Task.Run(() => RunSessionAsync(sessionId, goal, settings,
|
||||
obstacles, obstacleSnapshotVersion, maximumCommandSpeedMetersPerSecond,
|
||||
maximumDistanceToTrajectoryMeters, executionTimeoutSeconds,
|
||||
wheelAlignmentToleranceDegrees, cancellation.Token), cancellation.Token);
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId == sessionId)
|
||||
activeTask = task;
|
||||
}
|
||||
|
||||
_ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed),
|
||||
TaskScheduler.Default);
|
||||
}
|
||||
|
||||
internal static void Stop()
|
||||
{
|
||||
CancellationTokenSource cancellation;
|
||||
DriveTask driveTask;
|
||||
TrajectoryObservationPresentation presentation;
|
||||
TrajectoryObservationVisualizationPublisher webPublisher;
|
||||
lock (SessionSync)
|
||||
{
|
||||
cancellation = activeCancellation;
|
||||
driveTask = activeDriveTask;
|
||||
presentation = activePresentation;
|
||||
webPublisher = activeWebPublisher;
|
||||
activeCancellation = null;
|
||||
activeDriveTask = null;
|
||||
activePresentation = null;
|
||||
activeWebPublisher = null;
|
||||
activeTask = null;
|
||||
activeSessionId = 0L;
|
||||
}
|
||||
|
||||
Cancel(cancellation);
|
||||
StopDriveTask(driveTask);
|
||||
ClearPresentation(presentation);
|
||||
StopWebPublisher(webPublisher);
|
||||
PrintStatus("已请求取消规划并停止控制任务。");
|
||||
}
|
||||
|
||||
internal static void ShowFailure(string context, Exception exception)
|
||||
{
|
||||
PrintStatus(context + "：" + (exception == null ? "未知错误" : exception.Message));
|
||||
}
|
||||
|
||||
private static async Task RunSessionAsync(long sessionId, PlanningPose2D goal,
|
||||
TrajectoryObservationSettings settings,
|
||||
IReadOnlyList<TrajectoryObservationObstacle> obstacles,
|
||||
long obstacleSnapshotVersion,
|
||||
double maximumCommandSpeedMetersPerSecond,
|
||||
double maximumDistanceToTrajectoryMeters,
|
||||
double executionTimeoutSeconds,
|
||||
float wheelAlignmentToleranceDegrees,
|
||||
CancellationToken token)
|
||||
{
|
||||
token.ThrowIfCancellationRequested();
|
||||
VehicleMotionState initialState = ReadPlanningState();
|
||||
using var planningDeadline = new TrajectoryObservationPlanningDeadline(
|
||||
TimeSpan.FromSeconds(settings.SolverTimeoutSeconds), token);
|
||||
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
||||
initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);
|
||||
TrajectoryObservationBootstrapResult bootstrap =
|
||||
new TrajectoryObservationBootstrapper().Bootstrap(job, planningDeadline, token);
|
||||
if (!bootstrap.Succeeded)
|
||||
throw new InvalidOperationException(bootstrap.FailureReason);
|
||||
|
||||
var controller = new TrajectoryObservationController(
|
||||
bootstrap, settings, new EmPlanningService(new OsqpNativeSolver()),
|
||||
"em-closed-loop-" + sessionId.ToString(CultureInfo.InvariantCulture));
|
||||
PlanningCycleResult cycle = await controller.StartCycle(
|
||||
initialState.CapturedAtUtc, initialState, planningDeadline, token).ConfigureAwait(false);
|
||||
if (!cycle.Published || controller.PublishedTrajectory == null)
|
||||
{
|
||||
string reason = cycle.Result == null ? string.Empty : cycle.Result.FailureReason;
|
||||
throw new InvalidOperationException(string.IsNullOrWhiteSpace(reason)
|
||||
? cycle.Diagnostic
|
||||
: reason);
|
||||
}
|
||||
|
||||
EmTrajectory emTrajectory = controller.PublishedTrajectory;
|
||||
Trajectory2D controlTrajectory = new EmControlTrajectoryAdapter().Create(emTrajectory);
|
||||
TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualization(
|
||||
sessionId, bootstrap, controller, settings, obstacleSnapshotVersion, token);
|
||||
TrajectoryObservationPresentation presentation = settings.EnableNativePainterVisualization
|
||||
? new TrajectoryObservationPresentation()
|
||||
: null;
|
||||
if (!TryRegisterPresentation(sessionId, presentation, token))
|
||||
{
|
||||
ClearPresentation(presentation);
|
||||
token.ThrowIfCancellationRequested();
|
||||
return;
|
||||
}
|
||||
UpdateVisualization(sessionId, presentation, bootstrap, controller, emTrajectory,
|
||||
initialState, cycle, "规划已冻结，尚未向底盘下发控制指令。");
|
||||
PrintStatus("规划完成：EM点数=" + emTrajectory.Points.Count.ToString(CultureInfo.InvariantCulture) +
|
||||
"，控制点数=" + controlTrajectory.Count.ToString(CultureInfo.InvariantCulture) +
|
||||
"，长度=" + controlTrajectory.TotalLengthMeters.ToString("F3", CultureInfo.InvariantCulture) +
|
||||
" m，方向=" + emTrajectory.Metadata.Direction +
|
||||
"，终端=" + emTrajectory.Metadata.TerminalType + "。");
|
||||
|
||||
token.ThrowIfCancellationRequested();
|
||||
EmClosedLoopWheelSafety.RequireWheelsForward(wheelAlignmentToleranceDegrees);
|
||||
var movement = new TrajectoryTrackingMovement
|
||||
{
|
||||
Trajectory = controlTrajectory,
|
||||
MaximumCommandSpeedMetersPerSecond = maximumCommandSpeedMetersPerSecond,
|
||||
MaximumDistanceToTrajectoryMeters = maximumDistanceToTrajectoryMeters,
|
||||
ExecutionTimeoutSeconds = executionTimeoutSeconds,
|
||||
CycleObserver = control => UpdatePresentationFromControlCycle(
|
||||
sessionId, presentation, bootstrap, controller, emTrajectory, cycle, control),
|
||||
};
|
||||
var driveTask = new DriveTask(movement.Get());
|
||||
if (!TryRegisterDriveTask(sessionId, driveTask, token))
|
||||
{
|
||||
StopDriveTask(driveTask);
|
||||
token.ThrowIfCancellationRequested();
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
PrintStatus("冻结轨迹已交给现有控制器，开始单方向段闭环跟踪。");
|
||||
driveTask.Wait();
|
||||
if (emTrajectory.Metadata.TerminalType == EmTerminalType.GearSwitch)
|
||||
PrintStatus("已在换向边界停车；本测试不启动下一方向段。");
|
||||
else
|
||||
PrintStatus("首个方向段已完成并停车。");
|
||||
}
|
||||
finally
|
||||
{
|
||||
StopDriveTask(driveTask);
|
||||
ClearDriveTask(sessionId, driveTask);
|
||||
}
|
||||
}
|
||||
|
||||
private static void UpdatePresentationFromControlCycle(long sessionId,
|
||||
TrajectoryObservationPresentation presentation,
|
||||
TrajectoryObservationBootstrapResult bootstrap,
|
||||
TrajectoryObservationController planningController,
|
||||
EmTrajectory trajectory,
|
||||
PlanningCycleResult cycle,
|
||||
MultiWheelC.Control.Execution.ParkingGeometricController control)
|
||||
{
|
||||
if (control == null || !control.LastVehicleState.HasValue)
|
||||
return;
|
||||
|
||||
var state = control.LastVehicleState.Value;
|
||||
var planningState = new VehicleMotionState(
|
||||
new PlanningPose2D(state.PoseInWorld.XMeters, state.PoseInWorld.YMeters,
|
||||
state.PoseInWorld.YawRadians),
|
||||
state.TwistInBody.VxMetersPerSecond, null, DateTimeOffset.UtcNow,
|
||||
Interlocked.Increment(ref stateSequence));
|
||||
string diagnostic = "现有控制器正在执行冻结轨迹";
|
||||
if (control.LastCommand.HasValue)
|
||||
{
|
||||
var command = control.LastCommand.Value;
|
||||
diagnostic += "：底盘速度=" +
|
||||
command.SpeedMetersPerSecond.ToString("F3", CultureInfo.InvariantCulture) +
|
||||
" m/s，前GCP=" +
|
||||
(command.FrontAngleRadians * 180d / Math.PI).ToString("F2", CultureInfo.InvariantCulture) +
|
||||
" deg，后GCP=" +
|
||||
(command.RearAngleRadians * 180d / Math.PI).ToString("F2", CultureInfo.InvariantCulture) + " deg。";
|
||||
}
|
||||
|
||||
UpdateVisualization(sessionId, presentation, bootstrap, planningController,
|
||||
trajectory, planningState, cycle, diagnostic);
|
||||
}
|
||||
|
||||
private static void UpdateVisualization(long sessionId,
|
||||
TrajectoryObservationPresentation presentation,
|
||||
TrajectoryObservationBootstrapResult bootstrap,
|
||||
TrajectoryObservationController controller,
|
||||
EmTrajectory trajectory,
|
||||
VehicleMotionState state,
|
||||
PlanningCycleResult cycle,
|
||||
string diagnostic)
|
||||
{
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId != sessionId)
|
||||
return;
|
||||
var observation = new TrajectoryObservationObservation(
|
||||
DateTimeOffset.UtcNow, state, trajectory, null, null, null);
|
||||
DirectionSegmentView segment = controller.ActiveSegment;
|
||||
TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
|
||||
trajectory, segment,
|
||||
controller.CreateEffectiveConfigurationSnapshot().Frenet.MaximumProjectionDistanceMeters);
|
||||
if (presentation != null && ReferenceEquals(activePresentation, presentation))
|
||||
{
|
||||
presentation.DrawWorld(bootstrap, observation,
|
||||
TrajectoryObservationRuntimeState.Create(DateTimeOffset.UtcNow, trajectory), diagnostic);
|
||||
presentation.DrawLs(charts, diagnostic);
|
||||
presentation.DrawSt(charts, diagnostic);
|
||||
}
|
||||
|
||||
TrajectoryObservationVisualizationPublisher publisher = activeWebPublisher;
|
||||
if (publisher != null)
|
||||
{
|
||||
var tick = new TrajectoryObservationLoopTick(observation, cycle, TimeSpan.Zero,
|
||||
false, false, true, false, controller.SegmentState);
|
||||
publisher.TryPublish(observation.ObservedAtUtc, () =>
|
||||
{
|
||||
PlanningVisualizationDynamicSnapshot source =
|
||||
new TrajectoryObservationDynamicSnapshotBuilder().Build(
|
||||
Interlocked.Increment(ref stateSequence), tick, segment,
|
||||
controller.PreviousTrajectoryForVisualization, bootstrap.Vehicle,
|
||||
controller.CreateEffectiveConfigurationSnapshot());
|
||||
return new PlanningVisualizationDynamicSnapshot(source.Sequence,
|
||||
source.ObservedAtUtc, "EM闭环控制中", source.ActiveSegmentIndex,
|
||||
source.ActiveDirection, source.VehiclePose, source.DynamicPolylines,
|
||||
source.DynamicMarkers, source.Charts, source.StatusValues, source.CycleSummary);
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static TrajectoryObservationVisualizationPublisher StartWebVisualization(
|
||||
long sessionId, TrajectoryObservationBootstrapResult bootstrap,
|
||||
TrajectoryObservationController controller, TrajectoryObservationSettings settings,
|
||||
long obstacleSnapshotVersion, CancellationToken token)
|
||||
{
|
||||
if (!settings.EnableWebVisualization)
|
||||
return null;
|
||||
var publisher = new TrajectoryObservationVisualizationPublisher(settings,
|
||||
new PlanningVisualizationSessionSink(), PrintStatus);
|
||||
try
|
||||
{
|
||||
PlanningVisualizationStaticSnapshot source =
|
||||
new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap,
|
||||
controller.CreateEffectiveConfigurationSnapshot(), settings, obstacleSnapshotVersion);
|
||||
var closedLoopSnapshot = new PlanningVisualizationStaticSnapshot("EM闭环测试",
|
||||
source.WorldBounds, source.OccupancyGrid, source.StaticPolylines,
|
||||
source.StaticMarkers, source.DirectionSegments, source.ConfigurationGroups);
|
||||
PlanningVisualizationSessionInfo info = publisher.Start(closedLoopSnapshot);
|
||||
if (info == null)
|
||||
return null;
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId != sessionId || token.IsCancellationRequested)
|
||||
{
|
||||
publisher.Stop();
|
||||
return null;
|
||||
}
|
||||
activeWebPublisher = publisher;
|
||||
}
|
||||
PrintStatus("网页可视化地址（含会话令牌）：" + info.Uri.AbsoluteUri);
|
||||
if (settings.AutoOpenWebVisualization)
|
||||
{
|
||||
Process.Start(new ProcessStartInfo
|
||||
{
|
||||
FileName = info.Uri.AbsoluteUri,
|
||||
UseShellExecute = true,
|
||||
});
|
||||
}
|
||||
return publisher;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
publisher.Disable(exception);
|
||||
PrintStatus("网页可视化未启动：" + exception.Message);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool TryRegisterPresentation(long sessionId,
|
||||
TrajectoryObservationPresentation presentation, CancellationToken token)
|
||||
{
|
||||
if (presentation == null)
|
||||
return true;
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId != sessionId || activeCancellation == null ||
|
||||
activeCancellation.IsCancellationRequested || token.IsCancellationRequested)
|
||||
return false;
|
||||
activePresentation = presentation;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
private static VehicleMotionState ReadPlanningState()
|
||||
{
|
||||
var location = DetourInterface.getCartLocation();
|
||||
if (location == null)
|
||||
throw new InvalidOperationException("实时定位不可用。");
|
||||
if (BasicPilotBase.Chassis == null)
|
||||
throw new InvalidOperationException("实时底盘读接口不可用。");
|
||||
|
||||
var speed = BasicPilotBase.Chassis.GetCarSpeed(true);
|
||||
return new VehicleMotionState(
|
||||
new PlanningPose2D(location.x / 1000d, location.y / 1000d,
|
||||
location.th * Math.PI / 180d),
|
||||
speed.Vx, null, DateTimeOffset.UtcNow,
|
||||
Interlocked.Increment(ref stateSequence));
|
||||
}
|
||||
|
||||
private static bool TryRegisterDriveTask(long sessionId, DriveTask driveTask,
|
||||
CancellationToken token)
|
||||
{
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId != sessionId || activeCancellation == null ||
|
||||
activeCancellation.IsCancellationRequested || token.IsCancellationRequested)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
activeDriveTask = driveTask;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
private static void ClearDriveTask(long sessionId, DriveTask driveTask)
|
||||
{
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId == sessionId && ReferenceEquals(activeDriveTask, driveTask))
|
||||
activeDriveTask = null;
|
||||
ClearPresentation(activePresentation);
|
||||
activePresentation = null;
|
||||
StopWebPublisher(activeWebPublisher);
|
||||
activeWebPublisher = null;
|
||||
}
|
||||
}
|
||||
|
||||
private static void Finish(long sessionId, CancellationTokenSource cancellation,
|
||||
Task completed)
|
||||
{
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (activeSessionId == sessionId)
|
||||
{
|
||||
activeCancellation = null;
|
||||
activeDriveTask = null;
|
||||
activeTask = null;
|
||||
activeSessionId = 0L;
|
||||
}
|
||||
}
|
||||
|
||||
if (completed.IsFaulted)
|
||||
{
|
||||
Exception failure = completed.Exception == null
|
||||
? null
|
||||
: completed.Exception.GetBaseException();
|
||||
ShowFailure("闭环会话失败并已停车", failure);
|
||||
}
|
||||
else if (completed.IsCanceled)
|
||||
{
|
||||
PrintStatus("闭环会话已取消。");
|
||||
}
|
||||
|
||||
cancellation.Dispose();
|
||||
}
|
||||
|
||||
private static void Cancel(CancellationTokenSource cancellation)
|
||||
{
|
||||
if (cancellation == null)
|
||||
return;
|
||||
try
|
||||
{
|
||||
cancellation.Cancel();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
private static void StopDriveTask(DriveTask driveTask)
|
||||
{
|
||||
if (driveTask == null)
|
||||
return;
|
||||
try
|
||||
{
|
||||
driveTask.Stop();
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
ShowFailure("停止控制任务失败", exception);
|
||||
}
|
||||
}
|
||||
|
||||
private static void ClearPresentation(TrajectoryObservationPresentation presentation)
|
||||
{
|
||||
if (presentation == null)
|
||||
return;
|
||||
try
|
||||
{
|
||||
presentation.ClearAll();
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
ShowFailure("清理闭环可视化失败", exception);
|
||||
}
|
||||
}
|
||||
|
||||
private static void StopWebPublisher(TrajectoryObservationVisualizationPublisher publisher)
|
||||
{
|
||||
if (publisher == null)
|
||||
return;
|
||||
try
|
||||
{
|
||||
publisher.Stop();
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
ShowFailure("停止闭环网页可视化失败", exception);
|
||||
}
|
||||
}
|
||||
|
||||
private static void PrintStatus(string message)
|
||||
{
|
||||
Console.WriteLine("[EM闭环测试] " + message);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>在控制器接管底盘前只读检查四个舵轮是否已经与车体前向对齐。</summary>
|
||||
internal static class EmClosedLoopWheelSafety
|
||||
{
|
||||
internal static void RequireWheelsForward(float toleranceDegrees)
|
||||
{
|
||||
var chassis = PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
throw new InvalidOperationException("当前底盘不是 MultiWheelChassis。");
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(chassis, PilotDefinition.Self.CarNum);
|
||||
double toleranceRadians = AngleMath.DegreesToRadians(toleranceDegrees);
|
||||
if (!adapter.AreParallelWheelsAligned(0d, toleranceRadians))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"四个舵轮尚未与车头方向一致，控制器未接管底盘。");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,157 +1,201 @@
|
||||
# EM trajectory observation MovementTest
|
||||
# EM 轨迹规划 MovementTest（观察与闭环）
|
||||
|
||||
`TrajectoryObservationMovementTest` is an observe-only host for the real MDCS localization and chassis-speed read
|
||||
interfaces. It bootstraps the coarse path and Local G2 reference once, requests one frozen full-direction-segment EM
|
||||
plan per active direction segment by default, samples the latest published trajectory, and draws the world, LS, and ST
|
||||
layers. It does not drive, steer, brake, change gear, or invoke a geometric vehicle controller.
|
||||
`tarjplanner_movementtest` 将粗路径、Local G2 连续参考和 EM 轨迹规划接入两个 MovementTest：
|
||||
|
||||
Every runtime status contains `OBSERVE_ONLY: no chassis command is sent.` Treat the displayed control command as a
|
||||
diagnostic prediction only. Goal and rolling-safety-stop commands are logged, never applied to hardware. At the end of a
|
||||
gear-switch trajectory, the observer remains on the current direction segment and waits for real direction confirmation;
|
||||
it does not create or dispatch a direction-change action.
|
||||
- `EM轨迹规划观察闭环测试`：只读地采样真实定位和底盘速度，规划并观察轨迹，**绝不向底盘发送命令**。
|
||||
- `EM闭环测试`：冻结首个 EM 方向段后，把轨迹交给现有几何控制器执行；这是会实际控制车辆的测试，必须在受监督的安全场地使用。
|
||||
|
||||
Every frozen observation session writes a text report to
|
||||
`TrajectoryObservationReports/trajectory-observation-session-<id>.txt` in the process working directory when the
|
||||
session stops; the report path is printed to the console.
|
||||
两种测试都会在启动时冻结目标、手动障碍物、地图和规划配置。它们不读取或修改传感器地图，也不做持续重建图。观察测试的所有状态都包含 `OBSERVE_ONLY: no chassis command is sent.`；其中显示的控制命令只是诊断预测，不能转发到硬件。
|
||||
|
||||
## FullDirectionSegment observation (default)
|
||||
## 模块说明（Module Overview）
|
||||
|
||||
`UseFullDirectionSegmentPlanning=true` maps to `EmPlanningScope.FullDirectionSegment`. MovementTest performs one frozen
|
||||
optimization for the complete remaining active direction segment, then republishes that trajectory with fresh observation
|
||||
state until the real direction boundary is confirmed and the next segment becomes active. `s_end` is the actual Local G2
|
||||
`PathS` at the active segment boundary; `T_end` is derived from the feasible acceleration, cruise, jerk-limited stopping,
|
||||
and zero-speed hold behavior instead of being forced by a legacy time horizon.
|
||||
| 模块 | 负责内容 | 不负责内容 |
|
||||
| --- | --- | --- |
|
||||
| `Map` | 根据冻结的起点、终点和手动障碍物创建不可变规划地图 | 实时环境更新、车辆控制 |
|
||||
| `CoarsePath` | Hybrid A* 粗路径、碰撞复核与方向分段 | 速度规划、底盘命令 |
|
||||
| `PathSmoothing` | 将粗路径转换为连续的 Local G2 参考 | 实时状态读取、控制 |
|
||||
| `EMPlanner` | 针对活动方向段生成不可变 `EmTrajectory` | 定位读取、UI、硬件协议 |
|
||||
| `TrajectoryExecution` | 轨迹采样、换向状态和诊断性控制意图 | 底盘接口、命令下发 |
|
||||
| 本目录 | 会话编排、状态采样、可视化、报告，以及闭环测试的轨迹适配 | 改写上游规划结果或地图快照 |
|
||||
|
||||
`DistanceHorizonMeters` and `TimeHorizonSeconds` are rolling-compatible fields and do not truncate full mode. Forward
|
||||
desired/hard maximum speed is `1.0 m/s`; reverse desired/hard maximum speed is `0.5 m/s`. A successful real boundary
|
||||
requires a terminal stop plus exact world-position and normalized yaw matching (`0 m` / `0 rad`).
|
||||
`EM轨迹规划观察闭环测试` 在换向边界只等待真实方向确认；不创建、不派发换向动作。`EM闭环测试` 只执行冻结的首个方向段；若终端是换向边界，它停车后不会自动启动下一段。
|
||||
|
||||
## Rolling trajectory observation
|
||||
## 文件结构（File Structure）
|
||||
|
||||
Rolling trajectory observation remains `OBSERVE_ONLY` and never sends a chassis command. Rolling and approach trajectories
|
||||
may end with nonzero speed because `DistanceHorizonMeters` is an L-S reference window and `TimeHorizonSeconds` is the
|
||||
single S-T output duration. Only a real Goal or gear-switch boundary may publish an exact zero-speed terminal.
|
||||
```text
|
||||
tarjplanner_movementtest/
|
||||
├── README.md # 模块边界、配置、操作和限制
|
||||
├── MovementTest.TrajectoryObservationTest.cs # 只读观察 UI 入口与会话生命周期
|
||||
├── MovementTest.EmClosedLoopTest.cs # 实际闭环 UI 入口、舵轮前向校验与停止
|
||||
├── EmControlTrajectoryAdapter.cs # EM 单方向段轨迹转几何控制器 Trajectory2D
|
||||
├── TrajectoryObservationContracts.cs # 设置快照、障碍物、启动输入和结果契约
|
||||
├── TrajectoryObservationPipeline.cs # Bootstrap、EM 周期、采样和观察循环
|
||||
├── TrajectoryObservationPlanningDeadline.cs # 单周期截止时间与发布授权
|
||||
├── TrajectoryObservationSegmentTracker.cs # 终停、真实换向确认和活动方向段推进
|
||||
├── TrajectoryObservationStaticSnapshotBuilder.cs # 地图、路径、配置的静态网页快照
|
||||
├── TrajectoryObservationDynamicSnapshotBuilder.cs # 每个 tick 的动态 World/LS/ST 快照
|
||||
├── TrajectoryObservationKinematicChartBuilder.cs # 速度、加速度、jerk、曲率等图表数据
|
||||
├── TrajectoryObservationHandoffAnalyzer.cs # 轨迹交接与连续性诊断
|
||||
├── TrajectoryObservationDiagnostics.cs # 配置、规划和运行诊断模型
|
||||
├── TrajectoryObservationPresentation.cs # Painter 图层与中文状态文本
|
||||
├── TrajectoryObservationVisualizationPublisher.cs # 本机网页会话与 Painter 发布隔离
|
||||
└── TrajectoryObservationReportWriter.cs # 只读观察会话文本报告
|
||||
```
|
||||
|
||||
## Configuration
|
||||
## 运行数据流（Runtime Data Flow）
|
||||
|
||||
| Field | Unit | MovementTest UI default | Meaning |
|
||||
```text
|
||||
目标、手动障碍物、实时起始位姿和冻结设置
|
||||
│
|
||||
▼
|
||||
Map -> CoarsePath -> PathSmoothing
|
||||
│
|
||||
▼
|
||||
当前方向段的 EMPlanner -> EmTrajectory
|
||||
│
|
||||
┌─────────┴──────────────────┐
|
||||
▼ ▼
|
||||
只读观察：每个 tick 读取状态 闭环测试：首段 Trajectory2D
|
||||
并采样/绘制/报告 -> 现有几何控制器 -> 底盘
|
||||
```
|
||||
|
||||
观察测试中，规划失败、报告写入失败、网页或 Painter 发布失败均只产生诊断，不能改变其只读属性。闭环测试在控制器接管前检查底盘类型和四个舵轮是否与车体前向平行；不满足时不会接管底盘。
|
||||
|
||||
## 运行状态与停止（Runtime State and Stop）
|
||||
|
||||
每个测试同时最多保留一个活动会话。重新启动会取消前一会话；`TestStop()` 取消规划和后台任务，并清理 Painter 与网页发布器。
|
||||
|
||||
观察测试在停止时把会话内容写入进程工作目录的：
|
||||
|
||||
```text
|
||||
TrajectoryObservationReports/trajectory-observation-session-<id>.txt
|
||||
```
|
||||
|
||||
报告路径会输出到控制台。关闭浏览器标签不会停止会话；必须使用 MovementTest 的正常停止操作。闭环测试停止时会请求停止当前 `DriveTask`，并清理其可视化资源。
|
||||
|
||||
## 坐标与单位（Coordinates and Units）
|
||||
|
||||
| 项目 | 坐标/单位 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| UI 目标与手动障碍物 | 世界坐标 `mm`、航向 `deg` | 入口处转换为规划用 SI 单位。 |
|
||||
| `Pose2D` / EM 路径 | `m`、`rad` | 车辆几何中心的世界位姿。 |
|
||||
| `ReferenceS` | `m` | 投影到完整活动方向段后的共享参考站。 |
|
||||
| `PathS` | `m` | ST 求解使用、从活动段局部起点累计的弧长；不可与 `ReferenceS` 跨段直接相减。 |
|
||||
| `l`、`v`、`a`、`j`、`κ`、`ω` | `m`、`m/s`、`m/s²`、`m/s³`、`m⁻¹`、`rad/s` | 网页/Painter 图表使用的横纵轴单位。 |
|
||||
|
||||
`EmTrajectoryPoint.VelocityX` 与 `VelocityY` 是世界坐标系分量。观察测试不会进行驱动坐标转换，也不会把它们发送到车辆接口。闭环测试使用 `EmControlTrajectoryAdapter`，以世界位置重建弧长并保留有符号纵向速度、航向和曲率，得到现有控制器使用的 `Trajectory2D`。
|
||||
|
||||
## 最小使用说明（Minimal Use）
|
||||
|
||||
### 只读观察
|
||||
|
||||
1. 在车辆 UI 选择 `EM轨迹规划观察闭环测试`。
|
||||
2. 设置有限的 `GoalXmm`、`GoalYmm`、`GoalYawDeg`；X 或 Y 为 `NaN` 时会依次弹出输入框。
|
||||
3. 输入 `0` 至 `20` 个手动障碍物，或输入 `0` 使用空障碍物快照。
|
||||
4. 启动测试，确认状态含有 `OBSERVE_ONLY: no chassis command is sent.`。
|
||||
5. 查看网页或 Painter 的 World、LS、ST 与运动学图层；停止时使用 UI 的正常停止操作。
|
||||
|
||||
### 实车闭环
|
||||
|
||||
1. 仅在已审查、受监督的车辆和场地中选择 `EM闭环测试`。
|
||||
2. 确认实时定位与底盘读接口可用；确认底盘为 `MultiWheelChassis`，且四个舵轮已对齐车头方向。
|
||||
3. 设置目标、障碍物以及命令速度、偏离阈值和执行超时。
|
||||
4. 启动后先等待“规划完成”状态；只有通过舵轮校验后，冻结的首个方向段才会交给现有控制器。
|
||||
5. 任意异常或人工停止时，使用 `TestStop()` 所对应的 UI 停止动作；确认控制任务已停止。
|
||||
|
||||
## 配置（Configuration）
|
||||
|
||||
### 两个入口共有的规划与可视化配置
|
||||
|
||||
| 字段 | 单位 | UI 默认值 | 含义 |
|
||||
| --- | --- | ---: | --- |
|
||||
| `GoalXmm` | world mm | `NaN` | Goal X. If X or Y is not finite, the host prompts for X, Y, and yaw. |
|
||||
| `GoalYmm` | world mm | `NaN` | Goal Y. |
|
||||
| `GoalYawDeg` | world deg | `0` | Goal heading. |
|
||||
| `MapPaddingMeters` | m | `2.0` | Padding added on all sides of the start/goal bounds. |
|
||||
| `MapResolutionMm` | mm | `50` | Local occupancy-grid resolution. |
|
||||
| `UseFullDirectionSegmentPlanning` | bool | `true` | Selects `FullDirectionSegment`; set `false` for legacy `RollingHorizon`. |
|
||||
| `ReplanPeriodSeconds` | s | `0.20` | Minimum interval between EM planning cycles in rolling scope. |
|
||||
| `ObserverPeriodSeconds` | s | `0.05` | Live-state sampling and redraw interval. |
|
||||
| `SolverTimeoutSeconds` | s | `5.0` | EM solver timeout frozen for the session. |
|
||||
| `MaximumOsqpIterations` | iterations | `100000` | OSQP iteration limit frozen for the session. |
|
||||
| `TimeHorizonSeconds` | s | `20.0` | Rolling-only ST trajectory horizon, not the observer period; full mode derives `T_end` instead. |
|
||||
| `OutputTimeStepSeconds` | s | `0.10` | Published trajectory timestamp spacing. |
|
||||
| `VehicleLengthMeters` | m | `0.80` | Vehicle envelope length supplied to coarse, smoothing, and EM planning. |
|
||||
| `VehicleWidthMeters` | m | `0.60` | Vehicle envelope width supplied to planning. |
|
||||
| `SafetyMarginMeters` | m | `0.05` | Additional planning clearance outside the vehicle envelope. |
|
||||
| `MaximumCurvaturePerMeter` | 1/m | `1 / 1.20` | Maximum allowed vehicle curvature (about `0.8333 1/m`). |
|
||||
| `EnableWebVisualization` | bool | `true` | Enables the primary local web dashboard session. |
|
||||
| `AutoOpenWebVisualization` | bool | `true` | Opens the tokenized local dashboard URI when web output is enabled. |
|
||||
| `WebVisualizationPort` | port | `0` | `0` selects an ephemeral port; otherwise use `1024` through `65535`. |
|
||||
| `WebRefreshRateHz` | Hz | `10` | Maximum dynamic web snapshot publication rate. |
|
||||
| `VisualizationHistoryCycleLimit` | cycles | `60` | Bounded web history length. |
|
||||
| `EnableNativePainterVisualization` | bool | `true` | Enables optional native Painter audit windows; disable unless explicitly reviewed. |
|
||||
| `DirectionConfirmationSpeedMetersPerSecond` | m/s | `0.02` | Minimum signed-speed magnitude used to confirm the next direction. |
|
||||
| `DirectionConfirmationSamples` | samples | `3` | Strictly increasing matching-direction samples required after the stop hold. |
|
||||
| `GearSwitchProjectionToleranceMeters` | m | `0.50` | Maximum switch-point projection distance for both adjacent segments. |
|
||||
| `GearSwitchStopHoldSeconds` | s | `0.20` | Continuous real-stop duration required before direction samples count. |
|
||||
| `GoalXmm` / `GoalYmm` | 世界 mm | `NaN` | 终点位置；非有限值时弹出输入。 |
|
||||
| `GoalYawDeg` | 世界 deg | `0` | 终点航向。 |
|
||||
| `MapPaddingMeters` | m | `2.0` | 起点/终点范围四周增加的地图边距。 |
|
||||
| `MapResolutionMm` | mm | `50` | 局部占据栅格分辨率。 |
|
||||
| `SolverTimeoutSeconds` | s | `5.0` | 冻结的单次规划截止时间。 |
|
||||
| `MaximumOsqpIterations` | 次 | `100000` | OSQP 最大迭代数。 |
|
||||
| `OutputTimeStepSeconds` | s | `0.10` | 发布轨迹相邻时间戳间隔。 |
|
||||
| `VehicleLengthMeters` / `VehicleWidthMeters` | m | `0.80` / `0.60` | 规划使用的车辆外形尺寸。 |
|
||||
| `SafetyMarginMeters` | m | `0.05` | 车辆外形之外的额外安全余量。 |
|
||||
| `MaximumCurvaturePerMeter` | `m⁻¹` | `1 / 1.20` | 允许的最大曲率。 |
|
||||
| `EnableWebVisualization` | bool | `true` | 启用本机网页看板。 |
|
||||
| `AutoOpenWebVisualization` | bool | `true` | 网页会话启动后尝试打开浏览器。 |
|
||||
| `WebVisualizationPort` | port | `0` | `0` 自动选择回环端口；否则必须为 `1024` 至 `65535`。 |
|
||||
| `WebRefreshRateHz` | Hz | `10` | 动态网页快照的最高发布频率。 |
|
||||
| `VisualizationHistoryCycleLimit` | cycles | `60` | 网页保存的有限历史周期数。 |
|
||||
| `EnableNativePainterVisualization` | bool | `true` | 启用可选 Painter 图层。 |
|
||||
|
||||
`TrajectoryObservationSettings` contract defaults are `PlanningScope=FullDirectionSegment`,
|
||||
`TimeHorizonSeconds=2.0`, `EnableWebVisualization=true`, and `EnableNativePainterVisualization=false`; the MovementTest
|
||||
UI currently exposes the values above. All public fields are validated and copied into a frozen input snapshot before
|
||||
the background session starts. Later edits cannot change an active session. The live pose and actual longitudinal speed
|
||||
are then read once per observer tick.
|
||||
`TrajectoryObservationSettings` 的契约默认值与 UI 不完全相同：`TimeHorizonSeconds=2.0`、`EnableNativePainterVisualization=false`，其余入口字段由各 MovementTest UI 显式赋值。启动前调用 `CreateValidatedSnapshot()`；之后 UI 改动不会影响活动会话。
|
||||
|
||||
In full scope, EM planning runs once per active segment with one attempt in flight. In rolling scope, EM planning
|
||||
respects `ReplanPeriodSeconds` with at most one planning cycle in flight. Every observer tick still captures fresh
|
||||
state, samples the currently published trajectory, redraws all layers, and emits a session-guarded status; a slow
|
||||
planner therefore does not reduce the configured observation cadence.
|
||||
### 只读观察专用配置
|
||||
|
||||
## Manual obstacles
|
||||
| 字段 | 单位 | UI 默认值 | 含义 |
|
||||
| --- | --- | ---: | --- |
|
||||
| `UseFullDirectionSegmentPlanning` | bool | `true` | `true` 使用 `FullDirectionSegment`，`false` 使用 `RollingHorizon`。 |
|
||||
| `ReplanPeriodSeconds` | s | `0.20` | 滚动模式两次规划之间的最小间隔。 |
|
||||
| `ObserverPeriodSeconds` | s | `0.05` | 读取状态、采样、绘图的观察周期。 |
|
||||
| `TimeHorizonSeconds` | s | `20.0` | 仅滚动兼容字段；完整方向段模式不用于截断。 |
|
||||
| `DirectionConfirmationSpeedMetersPerSecond` | m/s | `0.02` | 换向确认所需的下一方向速度阈值。 |
|
||||
| `DirectionConfirmationSamples` | samples | `3` | 停车后连续有效方向样本数。 |
|
||||
| `GearSwitchProjectionToleranceMeters` | m | `0.50` | 换向点在相邻两段上的最大投影距离。 |
|
||||
| `GearSwitchStopHoldSeconds` | s | `0.20` | 方向样本开始计数前要求的连续停车保持时间。 |
|
||||
|
||||
Enter a count from `0` through `20`, then select each obstacle type. Coordinates are global/world millimeters.
|
||||
完整方向段模式对每个活动方向段仅规划一次；滚动模式遵守 `ReplanPeriodSeconds`。无论规划是否较慢，观察循环仍按 `ObserverPeriodSeconds` 读取新鲜状态并发布当前轨迹快照。
|
||||
|
||||
- Circle example: center `(2500, 1200) mm`, radius `300 mm`.
|
||||
- Axis-aligned rectangle example: center `(4000, -500) mm`, X length `800 mm`, Y width `500 mm`.
|
||||
### 闭环专用配置
|
||||
|
||||
The complete obstacle envelope must fit inside the start/goal bounds plus `MapPaddingMeters`; otherwise bootstrap is
|
||||
rejected before EM planning.
|
||||
| 字段 | 单位 | UI 默认值 | 含义 |
|
||||
| --- | --- | ---: | --- |
|
||||
| `MaximumCommandSpeedMetersPerSecond` | m/s | `1.00` | 交给现有控制器的最大命令速度。 |
|
||||
| `MaximumDistanceToTrajectoryMeters` | m | `0.30` | 控制器允许的最大轨迹偏离距离。 |
|
||||
| `ExecutionTimeoutSeconds` | s | `120` | 首个方向段执行超时。 |
|
||||
| `WheelAlignmentToleranceDegrees` | deg | `2` | 四个舵轮与前向平行的最大角度误差。 |
|
||||
|
||||
## Reading the layers
|
||||
闭环入口固定使用 `FullDirectionSegment`，规划成功后只执行第一个活动方向段。
|
||||
|
||||
- `TrajectoryObserver.World` shows map bounds and occupied cells, the frozen start and goal, Hybrid A* coarse path,
|
||||
Local G2 smoothed path, current real pose, and latest published EM trajectory.
|
||||
- `TrajectoryObserver.LS` plots reference path-S horizontally and lateral offset vertically. Projection failures indicate
|
||||
trajectory points that could not be associated with the current direction segment.
|
||||
- `TrajectoryObserver.ST` overlays time-to-path-S and time-to-signed-speed. Use it to check monotonic time/progress,
|
||||
stop profiles, and the sign of forward/reverse velocity.
|
||||
## 手动障碍物与会话冻结（Obstacles and Frozen Session）
|
||||
|
||||
`EmTrajectoryPoint.VelocityX` and `VelocityY` are world-frame components. They are not chassis-frame velocity commands
|
||||
and must never be forwarded directly to a vehicle motion interface. This MovementTest performs no coordinate conversion
|
||||
for driving and has no driving capability; any future execution mode requires a separate safety-reviewed design.
|
||||
两个入口都要求输入 `0` 至 `20` 个障碍物，坐标均为世界 `mm`：
|
||||
|
||||
## 网页看板与可选 Painter
|
||||
- 圆形：中心 `(2500, 1200) mm`、半径 `300 mm`。
|
||||
- 轴对齐矩形：中心 `(4000, -500) mm`、X 向长度 `800 mm`、Y 向宽度 `500 mm`。
|
||||
|
||||
将 `EnableWebVisualization=true` 可启用本机网页看板；它只绑定 `127.0.0.1`，启动日志会给出完整的、带随机
|
||||
会话令牌的 URL。端口为 `0` 时由系统选择可用回环端口；浏览器必须使用该完整 URL，不能删掉 token。默认
|
||||
`AutoOpenWebVisualization=true` 会尝试打开默认浏览器；无桌面会话或打开失败只记录 URL，服务和观察循环继续运行。
|
||||
网页默认以 `10 Hz` 发布不可变动态快照，网页关闭、断开或慢客户端只会丢弃网页帧，绝不会阻塞、序列化等待或改变
|
||||
规划周期。
|
||||
障碍物完整包络必须位于起点/终点矩形加 `MapPaddingMeters` 后的地图范围内，否则启动阶段拒绝会话。启动成功后，目标、障碍物、地图、车辆参数和配置均为会话快照；不会跟随 UI 后续编辑改变。
|
||||
|
||||
`EnableNativePainterVisualization=true` 才会为本会话创建 `TrajectoryObserver.World`、`LS`、`ST` Painter。网页是主
|
||||
观察界面，Painter 仅作为可选的正确性/审计输出；受监督车辆清单执行前应显式关闭 Painter，除非另行审查。网页与
|
||||
Painter 可以同时开启，也可全部关闭：全部关闭时仍保留 `OBSERVE_ONLY` 规划、控制台和 UI 状态。网页的启动、快照或
|
||||
服务出现异常时会只记录一次中文诊断并熔断本次网页输出，不自动循环重启，也不会停止规划观察。
|
||||
## 网页看板与可选 Painter（Visualization）
|
||||
|
||||
网页采用中文说明配合科研绘图约定：白底、细灰网格和细曲线；当前轨迹为蓝色实线，上一轮为灰色虚线，换向与交接点
|
||||
为橙色，红色仅表示实际越界或失败。路径总览把已完成方向段画为灰色细实线、活动段画为蓝色细实线、未来段画为浅灰
|
||||
虚线，并以深蓝覆盖当前规划视界。页面的 `ReferenceS (m)` 是将轨迹投影到**完整活动方向段**后的共享参考站；
|
||||
`PathS (m)` 是 ST 求解使用、从该段局部起点累计的实际路径弧长，二者不可互相替代或跨段直接相减。
|
||||
启用 `EnableWebVisualization=true` 后，服务只绑定 `127.0.0.1`。启动日志输出含随机会话令牌的完整 URL；必须使用完整地址，不能去掉 token。端口为 `0` 时系统选择可用回环端口。浏览器打开失败、客户端断开或快照发布异常只记录诊断并隔离该网页输出，不应阻塞规划、观察或闭环停止。
|
||||
|
||||
所有图表轴标签与单位由实际快照提供：世界 `X/Y (m)`、`ReferenceS (m)`、`PathS (m)`、`t (s)`、`l (m)`、
|
||||
`v (m/s)`、`a (m/s²)`、`j (m/s³)`、`κ (m⁻¹)`、`ω (rad/s)`。`ls` 的横轴是 `ReferenceS (m)`、纵轴是 `l (m)`；
|
||||
`st` 的横轴是 `t (s)`、纵轴是 `PathS (m)`。每张图支持鼠标滚轮以指针为中心缩放、拖拽框选局部放大、`重置视图`
|
||||
与 `全屏`；这些操作只改变本地视口，不修改原始快照，也不向规划器发送参数。
|
||||
启用 `EnableNativePainterVisualization=true` 后，可查看 `TrajectoryObserver.World`、`TrajectoryObserver.LS` 与 `TrajectoryObserver.ST` 图层。网页是主观察界面；Painter 是可选审计输出。网页与 Painter 可同时启用，也可同时关闭。
|
||||
|
||||
运动学页中 `j[i]` 仅表示真实区间 `[tᵢ, tᵢ₊₁)`，所以 jerk 只有 `N-1` 个样本；末点之后没有虚构的 `j=0`。
|
||||
状态页将滚动续航、接近停车边界和精确停车边界分别标为 `RollingContinuation`、
|
||||
`ApproachStopBoundary`、`ExactStopAtBoundary`：前两者允许非零末端速度，只有真实边界才是精确停车锚点。换向高亮
|
||||
仅会在投影位置满足容差、已连续停车保持 `GearSwitchStopHoldSeconds`，并取得
|
||||
`DirectionConfirmationSamples` 个满足 `DirectionConfirmationSpeedMetersPerSecond` 阈值的下一方向带符号速度样本后，
|
||||
严格从 `N -> N+1` 前进。
|
||||
- World：地图边界、占据格、冻结起点/终点、粗路径、Local G2、实时位姿和 EM 轨迹。
|
||||
- LS：横轴 `ReferenceS (m)`，纵轴 `l (m)`；投影失败表示点无法关联到活动方向段。
|
||||
- ST：横轴 `t (s)`，纵轴 `PathS (m)`；用于检查时间、进度、停车与速度符号。
|
||||
- 运动学图：显示 `v`、`a`、`j`、`κ`、`ω` 等快照数据。`j[i]` 对应区间 `[tᵢ, tᵢ₊₁)`，因此只有 `N-1` 个样本。
|
||||
|
||||
## Operator launch and stop checklist
|
||||
## 报告与诊断（Reports and Diagnostics）
|
||||
|
||||
The vehicle UI entry is `EM轨迹规划观察闭环测试`. It is an observation session only; it has no chassis,
|
||||
motor, steering, brake, or gear output.
|
||||
只读观察会话把带时间戳的诊断写入文本报告，包含会话编号、运行状态和 `OBSERVE_ONLY` 安全标识。常见状态包括当前活动方向段、规划是否已发布、轨迹采样结果、换向确认进度和可视化地址。
|
||||
|
||||
Before starting:
|
||||
闭环测试会在控制器接管前输出冻结轨迹点数、控制点数、长度、方向和终端类型；执行期间输出底盘速度与前后 GCP 角度，供监督人员诊断。该测试的命令输出属于真实控制流程，不能按观察模式解释。
|
||||
|
||||
1. Ensure the vehicle is in a safe, supervised state; this test reads live localization and signed chassis speed but
|
||||
never controls the vehicle.
|
||||
2. Confirm localization and the chassis read interface are available. The session reports a startup failure if either
|
||||
cannot be read.
|
||||
3. Set a finite goal X/Y/yaw (or be ready to enter them when prompted), then enter zero to twenty manual obstacles.
|
||||
Each obstacle must remain inside the configured start/goal rectangle plus map padding.
|
||||
4. Keep `EnableWebVisualization=true` and set `EnableNativePainterVisualization=false` unless the Painter audit output is
|
||||
explicitly required and reviewed for this vehicle session.
|
||||
## 常见错误（Common Errors）
|
||||
|
||||
To start, select the entry in the vehicle UI and start the MovementTest. Confirm the status begins with
|
||||
`OBSERVE_ONLY: no chassis command is sent.`, then review the bootstrap status and the
|
||||
`TrajectoryObserver.World`, `TrajectoryObserver.LS`, and `TrajectoryObserver.ST` layers. A logged trajectory control
|
||||
command is diagnostic information only and must not be copied into a vehicle-control interface.
|
||||
| 现象 | 检查项 |
|
||||
| --- | --- |
|
||||
| 启动即提示定位或底盘接口不可用 | 确认 MDCS 定位和底盘只读接口已连接；观察与闭环入口都需要初始状态。 |
|
||||
| 目标或障碍物输入被拒绝 | 检查所有值为有限数；障碍物数量为 `0–20`；半径/边长为正；包络位于地图范围内。 |
|
||||
| 设置校验失败 | 正数配置不得为零或非有限；`OutputTimeStepSeconds` 不得大于 `TimeHorizonSeconds`；端口只能为 `0` 或 `1024–65535`。 |
|
||||
| 观察测试没有车辆动作 | 这是预期行为。该入口永远是 `OBSERVE_ONLY`。 |
|
||||
| 闭环测试未接管车辆 | 检查首段规划是否成功、控制轨迹是否至少有两个不同位置点、底盘类型以及舵轮前向对齐。 |
|
||||
| 闭环在换向边界停止 | 这是预期行为；该测试不自动执行下一方向段。 |
|
||||
| 网页没有打开或无法访问 | 从控制台复制完整令牌 URL；确认使用本机回环地址；打开失败不会停止会话。 |
|
||||
|
||||
To stop, use the vehicle UI's normal MovementTest stop action. Confirm the status says
|
||||
`Observation stop requested; all observer layers were cleared.` The cancellation request stops observer work and clears
|
||||
an existing Painter, stops the web publisher/HTTP/SSE service, and releases its port. A current-session runtime fault
|
||||
also clears an existing Painter and stops the web session so stale diagnostics are not left on screen; an intentional
|
||||
bootstrap failure still reports through UI/console even when both visualization modes are off. Closing a browser tab does
|
||||
not stop the session; `TestStop()` owns final web and Painter cleanup. Stopping, running, or starting this test must not
|
||||
issue chassis, motor, steering, brake, or gear output.
|
||||
## 当前限制（Current Limits）
|
||||
|
||||
- 地图和障碍物只在启动时冻结；本模块不实现实时感知更新或全局重规划。
|
||||
- 观察测试不具备任何底盘、转向、制动或换向命令能力。
|
||||
- 闭环测试只覆盖冻结的首个方向段，换向后续段不自动执行。
|
||||
- 闭环控制依赖现有 `TrajectoryTrackingMovement` 和 `MultiWheelChassis`；它不是对任意底盘的通用执行接口。
|
||||
- 网页服务仅限本机回环访问，且可视化故障只作为诊断处理。
|
||||
|
||||
+2
@@ -7,6 +7,7 @@ using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>观察模式下的不可变诊断消息；错误只报告给操作者，不能触发车辆动作。</summary>
|
||||
public sealed class TrajectoryObservationDiagnostic
|
||||
{
|
||||
public TrajectoryObservationDiagnostic(string text)
|
||||
@@ -17,6 +18,7 @@ public sealed class TrajectoryObservationDiagnostic
|
||||
public string Text { get; }
|
||||
}
|
||||
|
||||
/// <summary>构造带会话上下文的统一观察诊断文本。</summary>
|
||||
public static class TrajectoryObservationDiagnostics
|
||||
{
|
||||
public static TrajectoryObservationDiagnostic CreateCurveReport(TrajectoryObservationCharts charts,
|
||||
|
||||
+1
@@ -7,6 +7,7 @@ using TrajectoryPlanningVisualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>将每个 tick 的新鲜车辆状态与已发布轨迹转换为不可变动态可视化快照。</summary>
|
||||
public sealed class TrajectoryObservationDynamicSnapshotBuilder
|
||||
{
|
||||
private readonly TrajectoryObservationKinematicChartBuilder charts = new TrajectoryObservationKinematicChartBuilder();
|
||||
|
||||
+2
@@ -4,6 +4,7 @@ using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>描述相邻已发布轨迹在交接处的位置、航向和速度连续性，仅作诊断。</summary>
|
||||
public sealed class TrajectoryObservationHandoffMetrics
|
||||
{
|
||||
internal TrajectoryObservationHandoffMetrics(bool available, double? position, double? referenceS, double? velocity, double? acceleration)
|
||||
@@ -15,6 +16,7 @@ public sealed class TrajectoryObservationHandoffMetrics
|
||||
public double? DeltaAccelerationMetersPerSecondSquared { get; }
|
||||
}
|
||||
|
||||
/// <summary>计算观察会话中的轨迹交接指标，不参与轨迹发布或控制决策。</summary>
|
||||
public sealed class TrajectoryObservationHandoffAnalyzer
|
||||
{
|
||||
public TrajectoryObservationHandoffMetrics Analyze(EmTrajectory current, EmTrajectory previous, DirectionSegmentView segment)
|
||||
|
||||
+1
@@ -7,6 +7,7 @@ using TrajectoryPlanningVisualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>建立轨迹的时间、路径弧长、速度、加速度和 jerk 图表数据；仅用于观察与诊断。</summary>
|
||||
public sealed class TrajectoryObservationKinematicChartBuilder
|
||||
{
|
||||
public IReadOnlyList<VisualizationChart> Build(EmTrajectory trajectory, DirectionSegmentView segment,
|
||||
|
||||
+150
-9
@@ -1,6 +1,7 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Globalization;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
@@ -12,6 +13,7 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>一次观察会话启动的不可变结果，成功时同时携带冻结的粗路径、平滑路径、地图和方向段。</summary>
|
||||
public sealed class TrajectoryObservationBootstrapResult
|
||||
{
|
||||
private readonly VehicleParameters vehicle;
|
||||
@@ -87,6 +89,7 @@ public sealed class TrajectoryObservationBootstrapResult
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>只在会话开始阶段构造并冻结规划输入；后续观察 tick 不会重新读取或改写该快照。</summary>
|
||||
public sealed class TrajectoryObservationBootstrapper
|
||||
{
|
||||
private readonly CoarsePathPlanningService coarseService;
|
||||
@@ -109,22 +112,110 @@ public sealed class TrajectoryObservationBootstrapper
|
||||
{
|
||||
if (job == null) throw new ArgumentNullException(nameof(job));
|
||||
|
||||
CoarsePathPlanningJobResult coarse = coarseService.Plan(job, cancellationToken);
|
||||
TimeSpan configuredLimit = job.Configuration == null
|
||||
? TimeSpan.FromSeconds(5d)
|
||||
: job.Configuration.SearchTimeout;
|
||||
using var deadline = new TrajectoryObservationPlanningDeadline(configuredLimit, cancellationToken);
|
||||
return Bootstrap(job, deadline, cancellationToken);
|
||||
}
|
||||
|
||||
public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
|
||||
TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (job == null) throw new ArgumentNullException(nameof(job));
|
||||
if (deadline == null) throw new ArgumentNullException(nameof(deadline));
|
||||
if (DeadlineExpired(deadline))
|
||||
return DeadlineFailure(job, null, null, deadline, "coarse");
|
||||
|
||||
CoarsePathPlanningJob effectiveJob = CopyJobWithDeadline(job, deadline);
|
||||
bool coarseUsesCycleRemainder = job.Configuration != null &&
|
||||
effectiveJob.Configuration.SearchTimeout < job.Configuration.SearchTimeout;
|
||||
using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
||||
deadline.Token, cancellationToken);
|
||||
|
||||
CoarsePathPlanningJobResult coarse = coarseService.Plan(effectiveJob, linkedCancellation.Token);
|
||||
if (DeadlineExpired(deadline) ||
|
||||
(coarseUsesCycleRemainder && coarse.PlanningResult.Status == PlanningStatus.SearchTimeout))
|
||||
{
|
||||
return DeadlineFailure(job, coarse, null, deadline, "coarse");
|
||||
}
|
||||
if (coarse.PlanningResult.Status != PlanningStatus.Success)
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(
|
||||
job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);
|
||||
|
||||
if (DeadlineExpired(deadline))
|
||||
return DeadlineFailure(job, coarse, null, deadline, "smoothing");
|
||||
|
||||
var smoothingRequest = new PathSmoothingRequest(
|
||||
CopyFiniteClearance(coarse.PlanningResult.Path, coarse.MapResult.Map),
|
||||
coarse.PlanningResult.Segments, coarse.MapResult.Map, job.Vehicle,
|
||||
new PathSmoothingConfiguration());
|
||||
PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, cancellationToken);
|
||||
PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, linkedCancellation.Token);
|
||||
if (DeadlineExpired(deadline))
|
||||
return DeadlineFailure(job, coarse, smooth, deadline, "smoothing");
|
||||
if (!IsPublishedSmoothingStatus(smooth.Status))
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(
|
||||
job, coarse, smooth, smooth.Diagnostics.TerminationReason);
|
||||
return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
|
||||
}
|
||||
|
||||
private static bool DeadlineExpired(TrajectoryObservationPlanningDeadline deadline)
|
||||
{
|
||||
return deadline.IsExpired && !deadline.CallerCancellationRequested;
|
||||
}
|
||||
|
||||
private static TrajectoryObservationBootstrapResult DeadlineFailure(CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult coarse, PathSmoothingResult smooth,
|
||||
TrajectoryObservationPlanningDeadline deadline, string phase)
|
||||
{
|
||||
string diagnostic = "cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
|
||||
deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture);
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(job, coarse, smooth, diagnostic);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CopyJobWithDeadline(CoarsePathPlanningJob source,
|
||||
TrajectoryObservationPlanningDeadline deadline)
|
||||
{
|
||||
return new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = source.MapRequest,
|
||||
Start = source.Start,
|
||||
Goal = source.Goal,
|
||||
Vehicle = source.Vehicle,
|
||||
Configuration = CopyConfigurationWithDeadline(source.Configuration, deadline),
|
||||
StartVehicleCurvature = source.StartVehicleCurvature,
|
||||
StartDirection = source.StartDirection,
|
||||
GoalDirection = source.GoalDirection,
|
||||
DebugOptions = source.DebugOptions,
|
||||
};
|
||||
}
|
||||
|
||||
private static HybridAStarConfiguration CopyConfigurationWithDeadline(HybridAStarConfiguration source,
|
||||
TrajectoryObservationPlanningDeadline deadline)
|
||||
{
|
||||
if (source == null) return null;
|
||||
return new HybridAStarConfiguration
|
||||
{
|
||||
PrimitiveLengthMeters = source.PrimitiveLengthMeters,
|
||||
IntegrationStepMeters = source.IntegrationStepMeters,
|
||||
MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
|
||||
HeadingResolutionRadians = source.HeadingResolutionRadians,
|
||||
CurvatureLevelCount = source.CurvatureLevelCount,
|
||||
GoalPositionToleranceMeters = source.GoalPositionToleranceMeters,
|
||||
GoalHeadingToleranceRadians = source.GoalHeadingToleranceRadians,
|
||||
MaximumExpandedNodes = source.MaximumExpandedNodes,
|
||||
SearchTimeout = deadline.Clamp(source.SearchTimeout),
|
||||
HeuristicWeight = source.HeuristicWeight,
|
||||
ReverseCostMultiplier = source.ReverseCostMultiplier,
|
||||
GearSwitchPenaltyMeters = source.GearSwitchPenaltyMeters,
|
||||
CurvatureMagnitudeWeight = source.CurvatureMagnitudeWeight,
|
||||
CurvatureChangePenaltyMetersPerLevel = source.CurvatureChangePenaltyMetersPerLevel,
|
||||
ClearanceCostWeight = source.ClearanceCostWeight,
|
||||
ClearanceCostDistanceMeters = source.ClearanceCostDistanceMeters,
|
||||
AllowReverse = source.AllowReverse,
|
||||
};
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CoarsePathPoint> CopyFiniteClearance(IReadOnlyList<CoarsePathPoint> path,
|
||||
PlanningGridMap map)
|
||||
{
|
||||
@@ -158,6 +249,7 @@ public sealed class TrajectoryObservationBootstrapper
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>单个观察 tick 的只读结果，供状态文本、报告和可视化共同消费。</summary>
|
||||
public sealed class TrajectoryObservationObservation
|
||||
{
|
||||
internal TrajectoryObservationObservation(DateTimeOffset observedAtUtc, VehicleMotionState vehicleState,
|
||||
@@ -185,6 +277,7 @@ public sealed class TrajectoryObservationObservation
|
||||
public TrajectoryExecutionState ExecutorState { get; }
|
||||
}
|
||||
|
||||
/// <summary>协调一次冻结会话中的 EM 规划、轨迹采样和诊断；从不将预测命令发送给车辆。</summary>
|
||||
public sealed class TrajectoryObservationController
|
||||
{
|
||||
private readonly TrajectoryObservationBootstrapResult bootstrap;
|
||||
@@ -272,8 +365,19 @@ public sealed class TrajectoryObservationController
|
||||
|
||||
public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
using var deadline = new TrajectoryObservationPlanningDeadline(
|
||||
TimeSpan.FromSeconds(settings.SolverTimeoutSeconds), cancellationToken);
|
||||
return await StartCycle(now, state, deadline, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
|
||||
TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken)
|
||||
{
|
||||
if (state == null) throw new ArgumentNullException(nameof(state));
|
||||
if (deadline == null) throw new ArgumentNullException(nameof(deadline));
|
||||
using var callerCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
||||
deadline.CallerToken, cancellationToken);
|
||||
|
||||
int targetSegmentIndex = ActiveSegment.SegmentIndex;
|
||||
int pendingSegmentIndex = -1;
|
||||
@@ -303,18 +407,53 @@ public sealed class TrajectoryObservationController
|
||||
}
|
||||
|
||||
long currentCycleId = Interlocked.Increment(ref cycleId);
|
||||
EmPlannerConfiguration effectiveConfiguration = configuration.Copy();
|
||||
TimeSpan cycleRemaining = deadline.Remaining;
|
||||
TimeSpan configuredSolverLimit = TimeSpan.FromSeconds(
|
||||
configuration.Scheduling.SolverTimeoutSeconds);
|
||||
if (cycleRemaining > TimeSpan.Zero &&
|
||||
cycleRemaining < configuredSolverLimit - TimeSpan.FromMilliseconds(1d))
|
||||
{
|
||||
effectiveConfiguration.Scheduling.SolverTimeoutSeconds = cycleRemaining.TotalSeconds;
|
||||
}
|
||||
var request = new EmPlanningRequest(
|
||||
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration,
|
||||
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, effectiveConfiguration,
|
||||
targetSegmentIndex, previousTrajectory, now, now,
|
||||
sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
|
||||
previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
|
||||
EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope);
|
||||
PlanningCycleResult result = await targetCoordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken)
|
||||
.ConfigureAwait(false);
|
||||
if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment && result.Published &&
|
||||
result.Result.Trajectory != null && !planningPendingFullDirection)
|
||||
EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope, cycleRemaining,
|
||||
callerCancellation.Token, deadline.DeadlineToken,
|
||||
deadline.PublicationAuthorization);
|
||||
if (callerCancellation.IsCancellationRequested)
|
||||
{
|
||||
activeFullDirectionTrajectory = result.Result.Trajectory;
|
||||
var cancelledResult = new EmPlanningResult(EmPlanningStatus.Cancelled, null,
|
||||
"callerCancellation=true;phase=request");
|
||||
return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
|
||||
cancelledResult, false, cancelledResult.FailureReason);
|
||||
}
|
||||
if (deadline.DeadlineToken.IsCancellationRequested)
|
||||
{
|
||||
var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
|
||||
"cycleDeadlineExpired=true;phase=request;remainingMs=" +
|
||||
cycleRemaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
|
||||
return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
|
||||
expiredResult, false, expiredResult.FailureReason);
|
||||
}
|
||||
Action<EmTrajectory> fullDirectionCommit =
|
||||
settings.PlanningScope == EmPlanningScope.FullDirectionSegment && !planningPendingFullDirection
|
||||
? trajectory => activeFullDirectionTrajectory = trajectory
|
||||
: null;
|
||||
PlanningCycleResult result = await targetCoordinator.PlanLatestAsync(
|
||||
new PlanningCycleInput(request, now), callerCancellation.Token,
|
||||
fullDirectionCommit).ConfigureAwait(false);
|
||||
if (!result.Published && deadline.IsExpired && !callerCancellation.IsCancellationRequested &&
|
||||
result.Result.Status == EmPlanningStatus.Cancelled)
|
||||
{
|
||||
var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
|
||||
"cycleDeadlineExpired=true;phase=planning;remainingMs=" +
|
||||
deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
|
||||
result = new PlanningCycleResult(result.Version, result.Identity, expiredResult, false,
|
||||
expiredResult.FailureReason);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
@@ -454,6 +593,7 @@ public sealed class TrajectoryObservationLoopTick
|
||||
public bool ShouldLog => true;
|
||||
}
|
||||
|
||||
/// <summary>按观察周期捕获新鲜只读状态并发布快照的循环；规划耗时不能改变观察安全边界。</summary>
|
||||
public sealed class TrajectoryObservationLoop
|
||||
{
|
||||
private readonly TrajectoryObservationController controller;
|
||||
@@ -511,6 +651,7 @@ public enum TrajectoryObservationSessionEndReason
|
||||
Cancellation,
|
||||
}
|
||||
|
||||
/// <summary>集中定义观察会话的开始、停止和故障清理语义，避免留下旧会话可视化。</summary>
|
||||
public static class TrajectoryObservationSessionLifecycle
|
||||
{
|
||||
public static bool ShouldClearLayers(TrajectoryObservationSessionEndReason reason)
|
||||
|
||||
+226
@@ -0,0 +1,226 @@
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>Owns one monotonic time budget shared by every phase of an observation planning cycle.</summary>
|
||||
public sealed class TrajectoryObservationPlanningDeadline : IDisposable
|
||||
{
|
||||
private static readonly TimeSpan MaximumTimerDelay = TimeSpan.FromMilliseconds(uint.MaxValue - 1d);
|
||||
private readonly object authorizationGate = new object();
|
||||
private readonly TimeSpan limit;
|
||||
private readonly Stopwatch stopwatch;
|
||||
private readonly TimeSpan? injectedElapsed;
|
||||
private readonly CancellationToken callerToken;
|
||||
private readonly CancellationTokenSource expirationCancellation;
|
||||
private readonly CancellationTokenSource linkedCancellation;
|
||||
private readonly Timer expirationTimer;
|
||||
private readonly IEmPlanningPublicationAuthorization publicationAuthorization;
|
||||
private bool callerCancellationObserved;
|
||||
private bool deadlineExpired;
|
||||
private bool disposed;
|
||||
|
||||
public TrajectoryObservationPlanningDeadline(TimeSpan limit, CancellationToken callerToken = default)
|
||||
: this(limit, null, callerToken)
|
||||
{
|
||||
}
|
||||
|
||||
private TrajectoryObservationPlanningDeadline(TimeSpan limit, TimeSpan? injectedElapsed,
|
||||
CancellationToken callerToken)
|
||||
{
|
||||
if (limit < TimeSpan.Zero)
|
||||
throw new ArgumentOutOfRangeException(nameof(limit), "The planning deadline cannot be negative.");
|
||||
if (injectedElapsed.HasValue && injectedElapsed.Value < TimeSpan.Zero)
|
||||
throw new ArgumentOutOfRangeException(nameof(injectedElapsed));
|
||||
|
||||
this.limit = limit;
|
||||
this.injectedElapsed = injectedElapsed;
|
||||
this.callerToken = callerToken;
|
||||
stopwatch = injectedElapsed.HasValue ? null : Stopwatch.StartNew();
|
||||
expirationCancellation = new CancellationTokenSource();
|
||||
TimeSpan initialRemaining = ComputeRemaining();
|
||||
deadlineExpired = initialRemaining <= TimeSpan.Zero;
|
||||
if (deadlineExpired)
|
||||
expirationCancellation.Cancel();
|
||||
linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
||||
callerToken, expirationCancellation.Token);
|
||||
publicationAuthorization = new DeadlinePublicationAuthorization(this);
|
||||
if (!deadlineExpired)
|
||||
{
|
||||
TimeSpan timerDelay = initialRemaining < MaximumTimerDelay
|
||||
? initialRemaining
|
||||
: MaximumTimerDelay;
|
||||
expirationTimer = new Timer(ExpireFromTimer, null, timerDelay, Timeout.InfiniteTimeSpan);
|
||||
}
|
||||
}
|
||||
|
||||
public TimeSpan Remaining
|
||||
{
|
||||
get
|
||||
{
|
||||
bool cancelExpiration = false;
|
||||
TimeSpan remaining;
|
||||
lock (authorizationGate)
|
||||
{
|
||||
remaining = ComputeRemaining();
|
||||
if (!deadlineExpired && remaining <= TimeSpan.Zero)
|
||||
{
|
||||
deadlineExpired = true;
|
||||
cancelExpiration = true;
|
||||
}
|
||||
if (deadlineExpired)
|
||||
remaining = TimeSpan.Zero;
|
||||
}
|
||||
if (cancelExpiration)
|
||||
CancelExpirationToken();
|
||||
return remaining;
|
||||
}
|
||||
}
|
||||
|
||||
public bool IsExpired => callerToken.IsCancellationRequested || DeadlineExpired;
|
||||
|
||||
public CancellationToken Token => linkedCancellation.Token;
|
||||
|
||||
internal bool CallerCancellationRequested => callerToken.IsCancellationRequested;
|
||||
|
||||
internal CancellationToken CallerToken => callerToken;
|
||||
|
||||
internal CancellationToken DeadlineToken => expirationCancellation.Token;
|
||||
|
||||
internal IEmPlanningPublicationAuthorization PublicationAuthorization => publicationAuthorization;
|
||||
|
||||
internal static TrajectoryObservationPlanningDeadline CreateForTesting(TimeSpan limit,
|
||||
TimeSpan elapsed, CancellationToken callerToken)
|
||||
{
|
||||
return new TrajectoryObservationPlanningDeadline(limit, elapsed, callerToken);
|
||||
}
|
||||
|
||||
public TimeSpan Clamp(TimeSpan configuredLimit)
|
||||
{
|
||||
if (configuredLimit < TimeSpan.Zero)
|
||||
throw new ArgumentOutOfRangeException(nameof(configuredLimit));
|
||||
TimeSpan remaining = Remaining;
|
||||
return configuredLimit < remaining ? configuredLimit : remaining;
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
lock (authorizationGate)
|
||||
disposed = true;
|
||||
expirationTimer?.Dispose();
|
||||
linkedCancellation.Dispose();
|
||||
expirationCancellation.Dispose();
|
||||
}
|
||||
|
||||
private bool DeadlineExpired
|
||||
{
|
||||
get
|
||||
{
|
||||
_ = Remaining;
|
||||
lock (authorizationGate)
|
||||
return deadlineExpired;
|
||||
}
|
||||
}
|
||||
|
||||
private TimeSpan ComputeRemaining()
|
||||
{
|
||||
TimeSpan elapsed = injectedElapsed ?? stopwatch.Elapsed;
|
||||
TimeSpan remaining = limit - elapsed;
|
||||
return remaining > TimeSpan.Zero ? remaining : TimeSpan.Zero;
|
||||
}
|
||||
|
||||
private EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
|
||||
CancellationToken coordinatorCallerCancellationToken, Action publish)
|
||||
{
|
||||
if (publish == null)
|
||||
throw new ArgumentNullException(nameof(publish));
|
||||
|
||||
bool cancelExpiration = false;
|
||||
using CancellationTokenRegistration ownerCallerRegistration =
|
||||
callerToken.Register(ObserveCallerCancellation);
|
||||
using CancellationTokenRegistration requestCallerRegistration =
|
||||
requestCallerCancellationToken.Register(ObserveCallerCancellation);
|
||||
using CancellationTokenRegistration coordinatorCallerRegistration =
|
||||
coordinatorCallerCancellationToken.Register(ObserveCallerCancellation);
|
||||
EmPlanningPublicationDecision decision;
|
||||
lock (authorizationGate)
|
||||
{
|
||||
if (callerToken.IsCancellationRequested ||
|
||||
requestCallerCancellationToken.IsCancellationRequested ||
|
||||
coordinatorCallerCancellationToken.IsCancellationRequested)
|
||||
{
|
||||
callerCancellationObserved = true;
|
||||
}
|
||||
if (!deadlineExpired && ComputeRemaining() <= TimeSpan.Zero)
|
||||
{
|
||||
deadlineExpired = true;
|
||||
cancelExpiration = true;
|
||||
}
|
||||
if (callerCancellationObserved)
|
||||
decision = EmPlanningPublicationDecision.CallerCancelled;
|
||||
else if (deadlineExpired || disposed)
|
||||
decision = EmPlanningPublicationDecision.DeadlineExpired;
|
||||
else
|
||||
{
|
||||
publish();
|
||||
decision = EmPlanningPublicationDecision.Published;
|
||||
}
|
||||
}
|
||||
if (cancelExpiration)
|
||||
CancelExpirationToken();
|
||||
return decision;
|
||||
}
|
||||
|
||||
private void ObserveCallerCancellation()
|
||||
{
|
||||
lock (authorizationGate)
|
||||
callerCancellationObserved = true;
|
||||
}
|
||||
|
||||
private void ExpireFromTimer(object state)
|
||||
{
|
||||
bool cancelExpiration = false;
|
||||
lock (authorizationGate)
|
||||
{
|
||||
if (!disposed && !deadlineExpired)
|
||||
{
|
||||
deadlineExpired = true;
|
||||
cancelExpiration = true;
|
||||
}
|
||||
}
|
||||
if (cancelExpiration)
|
||||
CancelExpirationToken();
|
||||
}
|
||||
|
||||
private void CancelExpirationToken()
|
||||
{
|
||||
try
|
||||
{
|
||||
expirationCancellation.Cancel();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// Disposal can race a timer callback only after the owning cycle has completed.
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class DeadlinePublicationAuthorization : IEmPlanningPublicationAuthorization
|
||||
{
|
||||
private readonly TrajectoryObservationPlanningDeadline owner;
|
||||
|
||||
public DeadlinePublicationAuthorization(TrajectoryObservationPlanningDeadline owner)
|
||||
{
|
||||
this.owner = owner;
|
||||
}
|
||||
|
||||
public TimeSpan Remaining => owner.Remaining;
|
||||
|
||||
public bool IsExpired => owner.DeadlineExpired;
|
||||
|
||||
public EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
|
||||
CancellationToken coordinatorCallerCancellationToken, Action publish) =>
|
||||
owner.TryPublish(requestCallerCancellationToken, coordinatorCallerCancellationToken, publish);
|
||||
}
|
||||
}
|
||||
+2
@@ -13,6 +13,7 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>将只读观察结果转换为面向操作者的中文文本，不包含控制指令下发逻辑。</summary>
|
||||
public static class TrajectoryObservationPresentationText
|
||||
{
|
||||
public static string Create(TrajectoryObservationObservation observation, TrajectoryObservationCharts charts)
|
||||
@@ -77,6 +78,7 @@ public sealed class TrajectoryObservationLsPresentationModel
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>组合当前会话的文本、图层和图表展示模型。</summary>
|
||||
public sealed class TrajectoryObservationPresentation
|
||||
{
|
||||
private const float MillimetersPerMeter = 1000f;
|
||||
|
||||
+2
@@ -6,6 +6,7 @@ using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>观察器对当前方向段和真实换向确认过程的只读阶段。</summary>
|
||||
public enum TrajectoryObservationSegmentPhase
|
||||
{
|
||||
Planning,
|
||||
@@ -54,6 +55,7 @@ public sealed class TrajectoryObservationSegmentUpdate
|
||||
public TrajectoryObservationSegmentState State { get; }
|
||||
}
|
||||
|
||||
/// <summary>根据真实状态和方向确认推进活动段；不会自行请求或执行车辆换向。</summary>
|
||||
public sealed class TrajectoryObservationSegmentTracker
|
||||
{
|
||||
private readonly IReadOnlyList<DirectionSegmentView> segments;
|
||||
|
||||
+1
@@ -8,6 +8,7 @@ using TrajectoryPlanningVisualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
/// <summary>从冻结的规划输入建立世界和参考路径静态快照，供会话期间重复只读使用。</summary>
|
||||
public sealed class TrajectoryObservationStaticSnapshotBuilder
|
||||
{
|
||||
public PlanningVisualizationStaticSnapshot Build(TrajectoryObservationBootstrapResult bootstrap,
|
||||
|
||||
@@ -1,576 +0,0 @@
|
||||
// 将统一命令转换为原 Chassis API 调用
|
||||
using System;
|
||||
using CommonUsage.Chassis;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
|
||||
/// 车体坐标系固定为X向前、Y向左、逆时针为正。
|
||||
/// </summary>
|
||||
public sealed class MultiWheelChassisAdapter
|
||||
{
|
||||
#region 辅助内容
|
||||
private const double RadiansToDegrees = 180.0 / Math.PI;
|
||||
private const float BiasTolerance = 0.001f;
|
||||
private readonly MultiWheelChassis _chassis;
|
||||
/// <summary>
|
||||
/// 当前适配器对应的车辆编号。
|
||||
/// </summary>
|
||||
public int VehicleId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Maximum distance from the body origin to a wheel center, in metres.
|
||||
/// </summary>
|
||||
public double MaximumWheelRadiusMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Maximum longitudinal wheel offset from the body origin, in metres.
|
||||
/// For a symmetric four-wheel-steering chassis this is half the wheelbase.
|
||||
/// </summary>
|
||||
public double HalfWheelBaseMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
|
||||
/// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。
|
||||
/// </summary>
|
||||
public double HalfTrackWidthMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Width of the steering-alignment speed gate, in degrees.
|
||||
/// </summary>
|
||||
public double SteeringAlignmentSigmaDegrees
|
||||
{
|
||||
get => _chassis.SteeringAlignmentSigmaDegrees;
|
||||
set
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value) ||
|
||||
value <= 0.0 ||
|
||||
value > float.MaxValue)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(value),
|
||||
"Steering alignment sigma must be a positive finite value.");
|
||||
}
|
||||
|
||||
_chassis.SteeringAlignmentSigmaDegrees =
|
||||
(float)value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查旧底盘是否仍处于无偏置的真实车体坐标系。
|
||||
/// </summary>
|
||||
private void EnsureBodyFrameIsActive()
|
||||
{
|
||||
EnsureMotionFrameIsActive(0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查旧底盘当前是否处于指定的运动坐标系。
|
||||
/// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。
|
||||
/// </summary>
|
||||
private void EnsureMotionFrameIsActive(
|
||||
double motionDirectionRadians)
|
||||
{
|
||||
ValidateFinite(
|
||||
motionDirectionRadians,
|
||||
nameof(motionDirectionRadians));
|
||||
|
||||
var expectedBiasDegrees =
|
||||
(float)(
|
||||
-FrameTransform2D.NormalizeAngle(
|
||||
motionDirectionRadians) *
|
||||
RadiansToDegrees);
|
||||
var bias = _chassis.GetOriginBias();
|
||||
var angleErrorDegrees =
|
||||
NormalizeDegrees(
|
||||
bias.Z - expectedBiasDegrees);
|
||||
|
||||
if (Math.Abs(bias.X) <= BiasTolerance &&
|
||||
Math.Abs(bias.Y) <= BiasTolerance &&
|
||||
Math.Abs(angleErrorDegrees) <=
|
||||
BiasTolerance)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
"MultiWheelChassis当前运动坐标系与命令不一致。" +
|
||||
$"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°，" +
|
||||
$"期望Th={expectedBiasDegrees}°。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度归一化到[-180°,180°]附近。
|
||||
/// </summary>
|
||||
private static float NormalizeDegrees(float degrees)
|
||||
{
|
||||
return (float)(
|
||||
degrees -
|
||||
Math.Round(degrees / 360.0) * 360.0);
|
||||
}
|
||||
/// <summary>
|
||||
/// 检查底盘命令是否包含无效数值。
|
||||
/// </summary>
|
||||
private static void ValidateTwist(Twist2D twist)
|
||||
{
|
||||
ValidateFinite(
|
||||
twist.VxMetersPerSecond,
|
||||
nameof(twist.VxMetersPerSecond));
|
||||
|
||||
ValidateFinite(
|
||||
twist.VyMetersPerSecond,
|
||||
nameof(twist.VyMetersPerSecond));
|
||||
|
||||
ValidateFinite(
|
||||
twist.OmegaRadiansPerSecond,
|
||||
nameof(twist.OmegaRadiansPerSecond));
|
||||
}
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void ValidateFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"底盘速度命令不能是NaN或无穷大。");
|
||||
}
|
||||
|
||||
if (value > float.MaxValue ||
|
||||
value < -float.MaxValue)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"底盘速度命令超过float可表示范围。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次底盘运动分解失败原因。
|
||||
/// </summary>
|
||||
public string LastFailureReason =>
|
||||
_chassis.LastMotionDecomposeFailureReason;
|
||||
|
||||
#endregion
|
||||
|
||||
/// <summary>
|
||||
/// 将旧底盘的原点偏置恢复为真实单车车体坐标系。
|
||||
/// </summary>
|
||||
public void ResetToBodyFrame()
|
||||
{
|
||||
ActivateMotionFrame(0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 激活指定运动方向对应的SendMotion坐标系。
|
||||
/// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
|
||||
/// </summary>
|
||||
public void ActivateMotionFrame(
|
||||
double motionDirectionRadians)
|
||||
{
|
||||
ValidateFinite(
|
||||
motionDirectionRadians,
|
||||
nameof(motionDirectionRadians));
|
||||
|
||||
var biasDegrees =
|
||||
(float)(
|
||||
-FrameTransform2D.NormalizeAngle(
|
||||
motionDirectionRadians) *
|
||||
RadiansToDegrees);
|
||||
var currentBias =
|
||||
_chassis.GetOriginBias();
|
||||
|
||||
if (Math.Abs(currentBias.X) <=
|
||||
BiasTolerance &&
|
||||
Math.Abs(currentBias.Y) <=
|
||||
BiasTolerance &&
|
||||
Math.Abs(
|
||||
NormalizeDegrees(
|
||||
currentBias.Z -
|
||||
biasDegrees)) <=
|
||||
BiasTolerance)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_chassis.SetOriginBias(
|
||||
x: 0.0f,
|
||||
y: 0.0f,
|
||||
th: biasDegrees);
|
||||
}
|
||||
public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
|
||||
{
|
||||
_chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
|
||||
if (vehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleId),
|
||||
"车辆编号必须大于零。");
|
||||
}
|
||||
VehicleId = vehicleId;
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var wheels = _chassis.GetSteerWheels();
|
||||
#pragma warning restore CS0612, CS0618
|
||||
|
||||
if (wheels.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"MultiWheelChassis尚未完成舵轮初始化，" +
|
||||
"不能创建底盘适配器。");
|
||||
}
|
||||
// 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
|
||||
// 保证SendXYThSpeed直接使用真实车体坐标系。
|
||||
var maximumWheelRadiusMillimeters = 0.0;
|
||||
var maximumLongitudinalOffsetMillimeters = 0.0;
|
||||
var maximumLateralOffsetMillimeters = 0.0;
|
||||
foreach (var wheel in wheels)
|
||||
{
|
||||
maximumWheelRadiusMillimeters = Math.Max(
|
||||
maximumWheelRadiusMillimeters,
|
||||
wheel.PhysicalPosition.Length());
|
||||
|
||||
maximumLongitudinalOffsetMillimeters = Math.Max(
|
||||
maximumLongitudinalOffsetMillimeters,
|
||||
Math.Abs(wheel.PhysicalPosition.X));
|
||||
|
||||
maximumLateralOffsetMillimeters = Math.Max(
|
||||
maximumLateralOffsetMillimeters,
|
||||
Math.Abs(wheel.PhysicalPosition.Y));
|
||||
}
|
||||
|
||||
MaximumWheelRadiusMeters =
|
||||
maximumWheelRadiusMillimeters / 1000.0;
|
||||
HalfWheelBaseMeters =
|
||||
maximumLongitudinalOffsetMillimeters / 1000.0;
|
||||
HalfTrackWidthMeters =
|
||||
maximumLateralOffsetMillimeters / 1000.0;
|
||||
|
||||
if (MaximumWheelRadiusMeters <= 0.0 ||
|
||||
HalfWheelBaseMeters <= 0.0 ||
|
||||
HalfTrackWidthMeters <= 0.0)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"Wheel positions cannot produce valid chassis dimensions.");
|
||||
}
|
||||
|
||||
// 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。
|
||||
_chassis.PreferMinimumSteeringTravel = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将车体坐标系速度命令发送给多舵轮底盘。
|
||||
/// </summary>
|
||||
public bool Send(ChassisCommand command, TimeSpan? interval = null)
|
||||
{
|
||||
if (command.VehicleId != VehicleId)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"命令车辆编号{command.VehicleId}与适配器车辆编号" +
|
||||
$"{VehicleId}不一致。");
|
||||
}
|
||||
ValidateTwist(command.BodyTwist);
|
||||
// 防止其他旧逻辑再次调用DirectionAngle或
|
||||
// SetOriginBias改变底盘坐标语义。
|
||||
EnsureBodyFrameIsActive();
|
||||
var vxMetersPerSecond =
|
||||
(float)command.BodyTwist.VxMetersPerSecond;
|
||||
var vyMetersPerSecond =
|
||||
(float)command.BodyTwist.VyMetersPerSecond;
|
||||
var omegaDegreesPerSecond =
|
||||
(float)(
|
||||
command.BodyTwist.OmegaRadiansPerSecond *
|
||||
RadiansToDegrees);
|
||||
var success = _chassis.SendXYThSpeed(
|
||||
vxMetersPerSecond,
|
||||
vyMetersPerSecond,
|
||||
omegaDegreesPerSecond,
|
||||
interval,
|
||||
enableDifferentialSteerFeedforward: true);
|
||||
if (!success)
|
||||
{
|
||||
// 防止分解失败后继续执行上一条运动命令。
|
||||
_chassis.PredefinedDriveStop();
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
|
||||
/// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
|
||||
/// </summary>
|
||||
public bool SendVirtualAckermannMotion(
|
||||
double motionDirectionRadians,
|
||||
double speedMetersPerSecond,
|
||||
double steeringRadians,
|
||||
TimeSpan? interval = null)
|
||||
{
|
||||
ValidateFinite(
|
||||
motionDirectionRadians,
|
||||
nameof(motionDirectionRadians));
|
||||
ValidateFinite(
|
||||
speedMetersPerSecond,
|
||||
nameof(speedMetersPerSecond));
|
||||
ValidateFinite(
|
||||
steeringRadians,
|
||||
nameof(steeringRadians));
|
||||
EnsureMotionFrameIsActive(
|
||||
motionDirectionRadians);
|
||||
|
||||
if (Math.Abs(steeringRadians) >=
|
||||
Math.PI / 2.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(steeringRadians),
|
||||
"虚拟阿克曼转向角必须位于正负90度以内。");
|
||||
}
|
||||
|
||||
var steeringDegrees =
|
||||
(float)(
|
||||
steeringRadians *
|
||||
RadiansToDegrees);
|
||||
var success =
|
||||
_chassis.SendMotion(
|
||||
(float)speedMetersPerSecond,
|
||||
steeringDegrees,
|
||||
-steeringDegrees,
|
||||
interval);
|
||||
|
||||
if (!success)
|
||||
{
|
||||
_chassis.PredefinedDriveStop();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。
|
||||
/// </summary>
|
||||
public bool SendGcpMotion(
|
||||
double speedMetersPerSecond,
|
||||
double frontAngleRadians,
|
||||
double rearAngleRadians,
|
||||
TimeSpan? interval = null)
|
||||
{
|
||||
ValidateFinite(
|
||||
speedMetersPerSecond,
|
||||
nameof(speedMetersPerSecond));
|
||||
ValidateFinite(
|
||||
frontAngleRadians,
|
||||
nameof(frontAngleRadians));
|
||||
ValidateFinite(
|
||||
rearAngleRadians,
|
||||
nameof(rearAngleRadians));
|
||||
EnsureBodyFrameIsActive();
|
||||
|
||||
if (Math.Abs(frontAngleRadians) >=
|
||||
Math.PI / 2.0 ||
|
||||
Math.Abs(rearAngleRadians) >=
|
||||
Math.PI / 2.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(frontAngleRadians),
|
||||
"前后GCP角度必须位于正负90度以内，避免四轮几何解算出现奇异值。");
|
||||
}
|
||||
|
||||
var success = _chassis.SendMotion(
|
||||
(float)speedMetersPerSecond,
|
||||
(float)(frontAngleRadians *
|
||||
RadiansToDegrees),
|
||||
(float)(rearAngleRadians *
|
||||
RadiansToDegrees),
|
||||
interval);
|
||||
|
||||
if (!success)
|
||||
{
|
||||
// 分解失败后立即清除上一条驱动速度，避免车辆继续执行陈旧命令。
|
||||
_chassis.PredefinedDriveStop();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 立即将所有驱动轮速度下发为零。
|
||||
/// </summary>
|
||||
public void StopImmediately()
|
||||
{
|
||||
_chassis.PredefinedDriveStop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清零XYTh驱动速度，但保留已经准备好的自转舵角和轮速方向。
|
||||
/// </summary>
|
||||
public void StopXYThDrivePreserveSteeringState()
|
||||
{
|
||||
_chassis.StopXYThDrivePreserveSteeringState();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停车并将所有舵轮转到指定的车体角度。
|
||||
/// 只调整舵轮角度，不产生车辆线速度。
|
||||
/// </summary>
|
||||
public bool PrepareParallelDirection(
|
||||
double directionRadians)
|
||||
{
|
||||
EnsureBodyFrameIsActive();
|
||||
var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
|
||||
RadiansToDegrees);
|
||||
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var wheels = _chassis.GetSteerWheels();
|
||||
#pragma warning restore CS0612, CS0618
|
||||
|
||||
// 没有舵轮时不能认为预对齐成功。
|
||||
if (wheels.Count == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// 先检查所有舵轮能否到达目标机械角度。
|
||||
foreach (var wheel in wheels)
|
||||
{
|
||||
if (targetDegrees < wheel.AngleLowerLimit ||
|
||||
targetDegrees > wheel.AngleUpperLimit)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// 模式切换前立即停止驱动轮。
|
||||
_chassis.PredefinedDriveStop();
|
||||
// 检查完成后再统一下发，避免只转动一部分舵轮。
|
||||
foreach (var wheel in wheels)
|
||||
{
|
||||
wheel.WriteAngle(targetDegrees);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查所有舵轮是否已经对准给定方向。
|
||||
/// </summary>
|
||||
public bool AreParallelWheelsAligned(
|
||||
double directionRadians,
|
||||
double toleranceRadians)
|
||||
{
|
||||
if (double.IsNaN(toleranceRadians) ||
|
||||
double.IsInfinity(toleranceRadians) ||
|
||||
toleranceRadians < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(toleranceRadians),
|
||||
"舵轮到位容差必须是非负有限值。");
|
||||
}
|
||||
|
||||
EnsureBodyFrameIsActive();
|
||||
var targetDegrees = (float)(
|
||||
FrameTransform2D.NormalizeAngle(directionRadians) *
|
||||
180.0 / Math.PI);
|
||||
|
||||
var toleranceDegrees = (float)(
|
||||
Math.Abs(toleranceRadians) *
|
||||
180.0 / Math.PI);
|
||||
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var wheels = _chassis.GetSteerWheels();
|
||||
#pragma warning restore CS0612, CS0618
|
||||
|
||||
foreach (var wheel in wheels)
|
||||
{
|
||||
var angleErrorDegrees = targetDegrees - wheel.ReadAngle();
|
||||
|
||||
if (Math.Abs(angleErrorDegrees) >
|
||||
toleranceDegrees)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停车并将舵轮预对齐到原地自转方向。
|
||||
/// 返回是否成功生成舵轮目标。
|
||||
/// </summary>
|
||||
public bool PrepareSpin(
|
||||
TimeSpan? interval = null,
|
||||
double alignmentToleranceDegrees = 2.0)
|
||||
{
|
||||
ValidateFinite(
|
||||
alignmentToleranceDegrees,
|
||||
nameof(alignmentToleranceDegrees));
|
||||
|
||||
if (alignmentToleranceDegrees < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(alignmentToleranceDegrees),
|
||||
"自转舵轮到位容差必须是非负有限值。");
|
||||
}
|
||||
|
||||
EnsureBodyFrameIsActive();
|
||||
|
||||
var success =
|
||||
_chassis.PrepareRotateWheels(
|
||||
alignmentToleranceDegrees:
|
||||
(float)alignmentToleranceDegrees);
|
||||
|
||||
if (!success)
|
||||
{
|
||||
_chassis.PredefinedDriveStop();
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将已到位的自转舵角和轮速方向一次性交接给XYTh，
|
||||
/// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。
|
||||
/// </summary>
|
||||
public bool AdoptPreparedSpinForXYTh(
|
||||
double toleranceRadians =
|
||||
2.0 * Math.PI / 180.0)
|
||||
{
|
||||
if (double.IsNaN(toleranceRadians) ||
|
||||
double.IsInfinity(toleranceRadians) ||
|
||||
toleranceRadians < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(toleranceRadians),
|
||||
"自转状态交接容差必须是非负有限值。");
|
||||
}
|
||||
|
||||
EnsureBodyFrameIsActive();
|
||||
|
||||
var success =
|
||||
_chassis
|
||||
.AdoptPreparedRotateWheelsForXYTh(
|
||||
(float)(
|
||||
toleranceRadians *
|
||||
RadiansToDegrees));
|
||||
|
||||
if (!success)
|
||||
{
|
||||
_chassis.PredefinedDriveStop();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
/// <summary>
|
||||
/// 所有舵轮是否已对齐到原地自转方向。
|
||||
/// </summary>
|
||||
public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,138 +0,0 @@
|
||||
using System;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供与坐标系无关的角度归一化、角度差和单位转换功能。
|
||||
/// </summary>
|
||||
public static class AngleMath
|
||||
{
|
||||
public const double TwoPi = 2.0 * Math.PI;
|
||||
|
||||
/// <summary>
|
||||
/// 将弧度归一化到[-π, π)区间。
|
||||
/// -π包含在结果中，+π不包含在结果中，因此+π会返回-π。
|
||||
/// </summary>
|
||||
public static double NormalizeRadians(double angleRadians)
|
||||
{
|
||||
EnsureFinite(angleRadians, nameof(angleRadians));
|
||||
|
||||
var normalized = angleRadians % TwoPi;
|
||||
|
||||
if (normalized >= Math.PI)
|
||||
{
|
||||
normalized -= TwoPi;
|
||||
}
|
||||
else if (normalized < -Math.PI)
|
||||
{
|
||||
normalized += TwoPi;
|
||||
}
|
||||
|
||||
return normalized == 0.0 ? 0.0 : normalized;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度归一化到[-180°, 180°)区间。
|
||||
/// -180°包含在结果中，+180°不包含在结果中，因此+180°会返回-180°。
|
||||
/// </summary>
|
||||
public static double NormalizeDegrees(double angleDegrees)
|
||||
{
|
||||
EnsureFinite(angleDegrees, nameof(angleDegrees));
|
||||
|
||||
var normalized = angleDegrees % 360.0;
|
||||
|
||||
if (normalized >= 180.0)
|
||||
{
|
||||
normalized -= 360.0;
|
||||
}
|
||||
else if (normalized < -180.0)
|
||||
{
|
||||
normalized += 360.0;
|
||||
}
|
||||
|
||||
return normalized == 0.0 ? 0.0 : normalized;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为弧度。
|
||||
/// 返回值位于[-π, π)；正值表示逆时针，负值表示顺时针。
|
||||
/// </summary>
|
||||
public static double ShortestDifferenceRadians(
|
||||
double targetRadians,
|
||||
double currentRadians)
|
||||
{
|
||||
EnsureFinite(targetRadians, nameof(targetRadians));
|
||||
EnsureFinite(currentRadians, nameof(currentRadians));
|
||||
|
||||
return NormalizeRadians(targetRadians - currentRadians);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为度。
|
||||
/// 返回值位于[-180°, 180°)；正值表示逆时针，负值表示顺时针。
|
||||
/// </summary>
|
||||
public static double ShortestDifferenceDegrees(
|
||||
double targetDegrees,
|
||||
double currentDegrees)
|
||||
{
|
||||
EnsureFinite(targetDegrees, nameof(targetDegrees));
|
||||
EnsureFinite(currentDegrees, nameof(currentDegrees));
|
||||
|
||||
return NormalizeDegrees(targetDegrees - currentDegrees);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 沿圆周最短方向在两个航向角之间插值，输入和结果单位均为弧度。
|
||||
/// ratio为0时返回起始角，ratio为1时返回终止角；本方法不限制ratio，
|
||||
/// 轨迹线段内插值时应先使用InterpolationMath.Clamp01进行限制。
|
||||
/// 结果归一化到[-π, π)区间；角度差恰好为π时按负方向插值。
|
||||
/// </summary>
|
||||
public static double LerpRadians(
|
||||
double startRadians,
|
||||
double endRadians,
|
||||
double ratio)
|
||||
{
|
||||
EnsureFinite(startRadians, nameof(startRadians));
|
||||
EnsureFinite(endRadians, nameof(endRadians));
|
||||
EnsureFinite(ratio, nameof(ratio));
|
||||
|
||||
var shortestDifference = ShortestDifferenceRadians(
|
||||
endRadians,
|
||||
startRadians);
|
||||
|
||||
return NormalizeRadians(
|
||||
startRadians + ratio * shortestDifference);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度从度转换为弧度，不进行归一化。
|
||||
/// </summary>
|
||||
public static double DegreesToRadians(double angleDegrees)
|
||||
{
|
||||
EnsureFinite(angleDegrees, nameof(angleDegrees));
|
||||
return angleDegrees * Math.PI / 180.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度从弧度转换为度，不进行归一化。
|
||||
/// </summary>
|
||||
public static double RadiansToDegrees(double angleRadians)
|
||||
{
|
||||
EnsureFinite(angleRadians, nameof(angleRadians));
|
||||
return angleRadians * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证角度是可用于计算的有限数值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(double angle, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(angle) || double.IsInfinity(angle))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"角度必须是有限数值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,165 +0,0 @@
|
||||
// 车体、运动、车队坐标系之间的转换
|
||||
using System;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
|
||||
/// 坐标系采用X向前、Y向左、逆时针为正的右手系。
|
||||
/// </summary>
|
||||
public static class FrameTransform2D
|
||||
{
|
||||
/// <summary>
|
||||
/// 将角度归一化到[-π, π)范围。
|
||||
/// </summary>
|
||||
public static double NormalizeAngle(double angleRadians)
|
||||
{
|
||||
return AngleMath.NormalizeRadians(angleRadians);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算从current到target的最短角度差。
|
||||
/// 返回正值表示逆时针旋转。
|
||||
/// </summary>
|
||||
public static double ShortestAngleDifference(
|
||||
double targetRadians,
|
||||
double currentRadians)
|
||||
{
|
||||
return AngleMath.ShortestDifferenceRadians(
|
||||
targetRadians,
|
||||
currentRadians);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将源坐标系中的点变换到目标坐标系。
|
||||
/// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
|
||||
/// </summary>
|
||||
public static Point2D TransformPoint(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Point2D pointInSource)
|
||||
{
|
||||
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
||||
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
||||
|
||||
return new Point2D(
|
||||
sourcePoseInTarget.XMeters +
|
||||
cos * pointInSource.XMeters -
|
||||
sin * pointInSource.YMeters,
|
||||
|
||||
sourcePoseInTarget.YMeters +
|
||||
sin * pointInSource.XMeters +
|
||||
cos * pointInSource.YMeters);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将目标坐标系中的点反向变换到源坐标系。
|
||||
/// </summary>
|
||||
public static Point2D InverseTransformPoint(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Point2D pointInTarget)
|
||||
{
|
||||
var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;
|
||||
|
||||
var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;
|
||||
|
||||
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
||||
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
||||
|
||||
return new Point2D(
|
||||
cos * dx + sin * dy,
|
||||
-sin * dx + cos * dy);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将源坐标系中的向量旋转到目标坐标系。
|
||||
/// 向量没有位置，因此不叠加平移量。
|
||||
/// </summary>
|
||||
public static Point2D TransformVector(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Point2D vectorInSource)
|
||||
{
|
||||
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
||||
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
||||
|
||||
return new Point2D(
|
||||
cos * vectorInSource.XMeters -
|
||||
sin * vectorInSource.YMeters,
|
||||
|
||||
sin * vectorInSource.XMeters +
|
||||
cos * vectorInSource.YMeters);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 组合两级坐标变换。
|
||||
/// parentFromMiddle表示middle在parent中的位姿；
|
||||
/// middleFromChild表示child在middle中的位姿；
|
||||
/// 返回child在parent中的位姿。
|
||||
/// </summary>
|
||||
public static Pose2D Compose(
|
||||
Pose2D parentFromMiddle,
|
||||
Pose2D middleFromChild)
|
||||
{
|
||||
var childPositionInParent = TransformPoint(
|
||||
parentFromMiddle,
|
||||
middleFromChild.Position);
|
||||
|
||||
return new Pose2D(
|
||||
childPositionInParent.XMeters,
|
||||
childPositionInParent.YMeters,
|
||||
NormalizeAngle(
|
||||
parentFromMiddle.YawRadians +
|
||||
middleFromChild.YawRadians));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对坐标变换求逆。
|
||||
/// 输入child在parent中的位姿，返回parent在child中的位姿。
|
||||
/// </summary>
|
||||
public static Pose2D Inverse(Pose2D childPoseInParent)
|
||||
{
|
||||
var cos = Math.Cos(childPoseInParent.YawRadians);
|
||||
var sin = Math.Sin(childPoseInParent.YawRadians);
|
||||
|
||||
return new Pose2D(
|
||||
-cos * childPoseInParent.XMeters -
|
||||
sin * childPoseInParent.YMeters,
|
||||
|
||||
sin * childPoseInParent.XMeters -
|
||||
cos * childPoseInParent.YMeters,
|
||||
|
||||
NormalizeAngle(
|
||||
-childPoseInParent.YawRadians));
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 将源坐标系中的位姿变换到目标坐标系。
|
||||
/// </summary>
|
||||
public static Pose2D TransformPose(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Pose2D poseInSource)
|
||||
{
|
||||
return Compose(sourcePoseInTarget, poseInSource);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 转换同一物理参考点处的速度表达坐标系。
|
||||
/// 只旋转线速度，角速度保持不变。
|
||||
/// </summary>
|
||||
public static Twist2D TransformTwistAtSamePoint(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Twist2D twistInSource)
|
||||
{
|
||||
var linearVelocityInTarget = TransformVector(
|
||||
sourcePoseInTarget,
|
||||
new Point2D(
|
||||
twistInSource.VxMetersPerSecond,
|
||||
twistInSource.VyMetersPerSecond));
|
||||
|
||||
return new Twist2D(
|
||||
linearVelocityInTarget.XMeters,
|
||||
linearVelocityInTarget.YMeters,
|
||||
twistInSource.OmegaRadiansPerSecond);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,61 +0,0 @@
|
||||
|
||||
using System;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供与具体业务和坐标系无关的基础插值功能。
|
||||
/// </summary>
|
||||
public static class InterpolationMath
|
||||
{
|
||||
/// <summary>
|
||||
/// 将插值比例限制到[0, 1]闭区间。
|
||||
/// </summary>
|
||||
public static double Clamp01(double value)
|
||||
{
|
||||
EnsureFinite(value, nameof(value));
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
if (value >= 1.0)
|
||||
{
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对两个标量执行线性插值。
|
||||
/// ratio为0时返回start，ratio为1时返回end；本方法不限制ratio，
|
||||
/// 因此也支持区间外的线性外插。
|
||||
/// </summary>
|
||||
public static double Lerp(
|
||||
double start,
|
||||
double end,
|
||||
double ratio)
|
||||
{
|
||||
EnsureFinite(start, nameof(start));
|
||||
EnsureFinite(end, nameof(end));
|
||||
EnsureFinite(ratio, nameof(ratio));
|
||||
|
||||
return start + ratio * (end - start);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证输入是可用于插值计算的有限数值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"插值参数必须是有限数值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,183 +0,0 @@
|
||||
// 纯数据层：只描述坐标、速度和命令
|
||||
// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
|
||||
// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
|
||||
// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
|
||||
// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 二维坐标点，X、Y单位均为米。
|
||||
/// </summary>
|
||||
public readonly struct Point2D
|
||||
{
|
||||
public Point2D(double xMeters, double yMeters)
|
||||
{
|
||||
XMeters = xMeters;
|
||||
YMeters = yMeters;
|
||||
}
|
||||
|
||||
public double XMeters { get; }
|
||||
|
||||
public double YMeters { get; }
|
||||
|
||||
public static Point2D Zero => new Point2D(0.0, 0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 二维局部坐标系在父坐标系中的位姿。
|
||||
/// 位置单位为米，朝向单位为弧度，逆时针为正。
|
||||
/// 具体父子关系由变量名称说明，例如RadarPoseInBody。
|
||||
/// </summary>
|
||||
public readonly struct Pose2D
|
||||
{
|
||||
public Pose2D(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double yawRadians)
|
||||
{
|
||||
XMeters = xMeters;
|
||||
YMeters = yMeters;
|
||||
YawRadians = yawRadians;
|
||||
}
|
||||
|
||||
public double XMeters { get; }
|
||||
|
||||
public double YMeters { get; }
|
||||
|
||||
public double YawRadians { get; }
|
||||
|
||||
public Point2D Position =>
|
||||
new Point2D(XMeters, YMeters);
|
||||
|
||||
public static Pose2D Identity =>
|
||||
new Pose2D(0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 二维刚体速度。
|
||||
/// 线速度单位为m/s，角速度单位为rad/s。
|
||||
/// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
|
||||
/// </summary>
|
||||
public readonly struct Twist2D
|
||||
{
|
||||
public Twist2D(
|
||||
double vxMetersPerSecond,
|
||||
double vyMetersPerSecond,
|
||||
double omegaRadiansPerSecond)
|
||||
{
|
||||
VxMetersPerSecond = vxMetersPerSecond;
|
||||
VyMetersPerSecond = vyMetersPerSecond;
|
||||
OmegaRadiansPerSecond = omegaRadiansPerSecond;
|
||||
}
|
||||
|
||||
public double VxMetersPerSecond { get; }
|
||||
|
||||
public double VyMetersPerSecond { get; }
|
||||
|
||||
public double OmegaRadiansPerSecond { get; }
|
||||
|
||||
public static Twist2D Zero =>
|
||||
new Twist2D(0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送给单辆车的车体坐标系速度命令。
|
||||
/// </summary>
|
||||
public readonly struct ChassisCommand
|
||||
{
|
||||
public ChassisCommand(
|
||||
int vehicleId,
|
||||
Twist2D bodyTwist)
|
||||
{
|
||||
VehicleId = vehicleId;
|
||||
BodyTwist = bodyTwist;
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
|
||||
/// </summary>
|
||||
public Twist2D BodyTwist { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 创建指定车辆的停止命令。
|
||||
/// </summary>
|
||||
public static ChassisCommand Stop(int vehicleId)
|
||||
{
|
||||
return new ChassisCommand(
|
||||
vehicleId,
|
||||
Twist2D.Zero);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 单辆车的车体坐标系在车队坐标系中的位姿。
|
||||
/// </summary>
|
||||
public readonly struct VehicleLayout
|
||||
{
|
||||
public VehicleLayout(
|
||||
int vehicleId,
|
||||
Pose2D poseInFleet)
|
||||
{
|
||||
VehicleId = vehicleId;
|
||||
PoseInFleet = poseInFleet;
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
public Pose2D PoseInFleet { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 车队整体运动命令，速度分量均在车队坐标系中表达。
|
||||
/// </summary>
|
||||
public readonly struct FleetMotionCommand
|
||||
{
|
||||
public FleetMotionCommand(
|
||||
Point2D referencePointInFleet,
|
||||
Twist2D twistAtReferencePoint)
|
||||
{
|
||||
ReferencePointInFleet = referencePointInFleet;
|
||||
TwistAtReferencePoint = twistAtReferencePoint;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 速度命令对应的参考点，也可作为自定义旋转中心。
|
||||
/// </summary>
|
||||
public Point2D ReferencePointInFleet { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 参考点处的车队速度。
|
||||
/// </summary>
|
||||
public Twist2D TwistAtReferencePoint { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 创建绕指定中心原地旋转的车队命令。
|
||||
/// </summary>
|
||||
public static FleetMotionCommand RotateAround(
|
||||
Point2D rotationCenterInFleet,
|
||||
double omegaRadiansPerSecond)
|
||||
{
|
||||
return new FleetMotionCommand(
|
||||
rotationCenterInFleet,
|
||||
new Twist2D(
|
||||
0.0,
|
||||
0.0,
|
||||
omegaRadiansPerSecond));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建车队停止命令。
|
||||
/// </summary>
|
||||
public static FleetMotionCommand Stop()
|
||||
{
|
||||
return new FleetMotionCommand(
|
||||
Point2D.Zero,
|
||||
Twist2D.Zero);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -1,512 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace EMPlannerVerificationHost;
|
||||
|
||||
internal static class CoordinatorChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesCallerSuppliedSchedulingDecision();
|
||||
VerifiesCompletedCycleDoesNotPoisonNextCancellationSource();
|
||||
VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults();
|
||||
VerifiesSinkExceptionsAreIsolatedIntoCycleDiagnostics();
|
||||
VerifiesSafePreviousTrajectoryHandoffs();
|
||||
}
|
||||
|
||||
public static void RunRollingEndToEnd()
|
||||
{
|
||||
VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults();
|
||||
VerifiesSafePreviousTrajectoryHandoffs();
|
||||
VerifiesPublishedForwardAndReverseCommands();
|
||||
VerifiesFailureLeavesPublishedZeroSpeedTailExecutable();
|
||||
}
|
||||
|
||||
private static void VerifiesCallerSuppliedSchedulingDecision()
|
||||
{
|
||||
var service = new ControlledPlanningService();
|
||||
var coordinator = new EmPlanningCoordinator(service);
|
||||
DateTimeOffset start = DateTimeOffset.UnixEpoch.AddSeconds(100d);
|
||||
PlanningCycleInput input = CreateInput(CreateMap(1), "schedule-reference", 7L, "prior", 0, "schedule", start);
|
||||
|
||||
Verification.True(coordinator.ShouldStartCycle(start), "first cycle is due");
|
||||
Task<PlanningCycleResult> cycle = coordinator.PlanLatestAsync(input, CancellationToken.None);
|
||||
service.WaitUntilStarted("schedule");
|
||||
service.Complete("schedule");
|
||||
Verification.Equal(EmPlanningStatus.Success, cycle.GetAwaiter().GetResult().Result.Status, "schedule setup result");
|
||||
|
||||
Verification.True(!coordinator.ShouldStartCycle(start.AddSeconds(0.199d)), "cycle remains deferred before period");
|
||||
Verification.True(coordinator.ShouldStartCycle(start.AddSeconds(0.20d)), "cycle becomes due at exact period");
|
||||
}
|
||||
|
||||
private static void VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults()
|
||||
{
|
||||
PlanningGridMap firstMap = CreateMap(10);
|
||||
PlanningGridMap secondMap = CreateMap(11);
|
||||
DateTimeOffset now = DateTimeOffset.UnixEpoch.AddSeconds(200d);
|
||||
|
||||
VerifySuperseded("later-version", CreateInput(firstMap, "reference", 7L, "prior", 0, "later-version-A", now),
|
||||
CreateInput(firstMap, "reference", 7L, "prior", 0, "later-version-B", now.AddSeconds(0.20d)));
|
||||
VerifySuperseded("map", CreateInput(firstMap, "reference", 7L, "prior", 0, "map-A", now),
|
||||
CreateInput(secondMap, "reference", 7L, "prior", 0, "map-B", now.AddSeconds(0.20d)));
|
||||
VerifySuperseded("reference", CreateInput(firstMap, "reference-A", 7L, "prior", 0, "reference-A", now),
|
||||
CreateInput(firstMap, "reference-B", 7L, "prior", 0, "reference-B", now.AddSeconds(0.20d)));
|
||||
VerifySuperseded("state", CreateInput(firstMap, "reference", 7L, "prior", 0, "state-A", now),
|
||||
CreateInput(firstMap, "reference", 8L, "prior", 0, "state-B", now.AddSeconds(0.20d)));
|
||||
VerifySuperseded("segment", CreateInput(firstMap, "reference", 7L, "prior", 0, "segment-A", now),
|
||||
CreateInput(firstMap, "reference", 7L, "prior", 1, "segment-B", now.AddSeconds(0.20d)));
|
||||
VerifySuperseded("previous", CreateInput(firstMap, "reference", 7L, "prior-A", 0, "previous-A", now),
|
||||
CreateInput(firstMap, "reference", 7L, "prior-B", 0, "previous-B", now.AddSeconds(0.20d)));
|
||||
}
|
||||
|
||||
private static void VerifiesCompletedCycleDoesNotPoisonNextCancellationSource()
|
||||
{
|
||||
var service = new ControlledPlanningService();
|
||||
var coordinator = new EmPlanningCoordinator(service);
|
||||
DateTimeOffset now = DateTimeOffset.UnixEpoch.AddSeconds(150d);
|
||||
|
||||
PlanningCycleInput first = CreateInput(CreateMap(5), "completed-reference", 4L, "prior", 0,
|
||||
"completed-first", now);
|
||||
Task<PlanningCycleResult> firstCycle = coordinator.PlanLatestAsync(first, CancellationToken.None);
|
||||
service.WaitUntilStarted(first.Request.OutputTrajectoryId);
|
||||
service.Complete(first.Request.OutputTrajectoryId);
|
||||
Verification.Equal(EmPlanningStatus.Success, firstCycle.GetAwaiter().GetResult().Result.Status,
|
||||
"first completed cycle succeeds");
|
||||
|
||||
PlanningCycleInput second = CreateInput(CreateMap(6), "completed-reference", 5L, "completed-first", 0,
|
||||
"completed-second", now.AddSeconds(0.20d));
|
||||
Task<PlanningCycleResult> secondCycle = coordinator.PlanLatestAsync(second, CancellationToken.None);
|
||||
service.WaitUntilStarted(second.Request.OutputTrajectoryId);
|
||||
service.Complete(second.Request.OutputTrajectoryId);
|
||||
|
||||
PlanningCycleResult result = secondCycle.GetAwaiter().GetResult();
|
||||
Verification.Equal(EmPlanningStatus.Success, result.Result.Status, "next cycle after completion succeeds");
|
||||
Verification.True(result.Published, "next cycle after completion publishes");
|
||||
}
|
||||
|
||||
private static void VerifiesSinkExceptionsAreIsolatedIntoCycleDiagnostics()
|
||||
{
|
||||
var service = new ControlledPlanningService();
|
||||
var coordinator = new EmPlanningCoordinator(service, new ThrowingCycleSink());
|
||||
PlanningCycleInput input = CreateInput(CreateMap(20), "sink-reference", 9L, "prior", 0, "sink", DateTimeOffset.UnixEpoch);
|
||||
|
||||
Task<PlanningCycleResult> cycle = coordinator.PlanLatestAsync(input, CancellationToken.None);
|
||||
service.WaitUntilStarted("sink");
|
||||
service.Complete("sink");
|
||||
PlanningCycleResult result = cycle.GetAwaiter().GetResult();
|
||||
|
||||
Verification.Equal(EmPlanningStatus.Success, result.Result.Status, "sink failure preserves planning result");
|
||||
Verification.True(result.Diagnostic.IndexOf("cycle sink", StringComparison.OrdinalIgnoreCase) >= 0,
|
||||
"sink failure is reported in diagnostic");
|
||||
}
|
||||
|
||||
private static void VerifiesSafePreviousTrajectoryHandoffs()
|
||||
{
|
||||
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(300d);
|
||||
EmPlannerConfiguration configuration = CreateHandoffConfiguration();
|
||||
var selector = new TrajectoryHandoffSelector();
|
||||
EmTrajectory forward = CreateHandoffTrajectory("handoff-forward", TravelDirection.Forward, 2, effectiveAt,
|
||||
EmBoundaryType.RollingSafetyStop, false);
|
||||
VehicleMotionState forwardMeasured = CreateMeasuredState(0.01d, 0d, 3.1666666666666665d, 0.10d,
|
||||
effectiveAt.AddSeconds(0.10d), 40L);
|
||||
|
||||
TrajectoryHandoffSelection accepted = selector.Select(forward, forwardMeasured, 2, TravelDirection.Forward,
|
||||
effectiveAt.AddSeconds(0.10d), configuration);
|
||||
Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, accepted.Source, "forward handoff source");
|
||||
Verification.Equal(TrajectoryHandoffRejectionReason.None, accepted.RejectionReason, "forward handoff reason");
|
||||
Verification.True(object.ReferenceEquals(forward, accepted.PreviousTrajectory), "forward handoff seed");
|
||||
Verification.NearlyEqual(0.04d, accepted.StartState.Pose.X, "forward handoff position interpolation");
|
||||
Verification.NearlyEqual(3.3666666666666667d, accepted.StartState.Pose.Heading,
|
||||
"forward handoff yaw remains unwrapped");
|
||||
Verification.NearlyEqual(0.10d, accepted.StartState.SignedLongitudinalSpeedMetersPerSecond,
|
||||
"forward handoff signed speed interpolation");
|
||||
Verification.Equal(40L, accepted.StartState.SequenceId, "handoff retains measured-state identity");
|
||||
|
||||
EmTrajectory reverse = CreateHandoffTrajectory("handoff-reverse", TravelDirection.Reverse, 2, effectiveAt,
|
||||
EmBoundaryType.RollingSafetyStop, false);
|
||||
VehicleMotionState reverseMeasured = CreateMeasuredState(-0.01d, 0d, 3.1666666666666665d, -0.10d,
|
||||
effectiveAt.AddSeconds(0.10d), 41L);
|
||||
TrajectoryHandoffSelection reverseAccepted = selector.Select(reverse, reverseMeasured, 2, TravelDirection.Reverse,
|
||||
effectiveAt.AddSeconds(0.10d), configuration);
|
||||
Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, reverseAccepted.Source,
|
||||
"reverse same-segment handoff source");
|
||||
Verification.NearlyEqual(-0.04d, reverseAccepted.StartState.Pose.X, "reverse handoff position interpolation");
|
||||
Verification.NearlyEqual(-0.10d, reverseAccepted.StartState.SignedLongitudinalSpeedMetersPerSecond,
|
||||
"reverse handoff signed speed interpolation");
|
||||
|
||||
AssertHandoffRejected(selector, forward, forwardMeasured, 2, TravelDirection.Forward,
|
||||
effectiveAt.AddSeconds(0.21d), configuration, TrajectoryHandoffRejectionReason.TrajectoryTooOld,
|
||||
"stale trajectory handoff");
|
||||
configuration.Scheduling.MaximumVehicleStateAgeSeconds = 0.60d;
|
||||
AssertHandoffRejected(selector, forward,
|
||||
CreateMeasuredState(1d, 0d, 3.1666666666666665d, 0.10d, effectiveAt.AddSeconds(0.10d), 42L), 2,
|
||||
TravelDirection.Forward, effectiveAt.AddSeconds(0.10d), configuration,
|
||||
TrajectoryHandoffRejectionReason.TrackingErrorExceeded, "large tracking-error handoff");
|
||||
VehicleMotionState terminalMeasured = CreateMeasuredState(0.03d, 0d, 3.30d, 0.10d,
|
||||
effectiveAt.AddSeconds(0.30d), 43L);
|
||||
AssertHandoffRejected(selector, forward, terminalMeasured, 2, TravelDirection.Forward,
|
||||
effectiveAt.AddSeconds(0.30d), configuration, TrajectoryHandoffRejectionReason.TerminalBoundary,
|
||||
"terminal-proximity handoff");
|
||||
AssertHandoffRejected(selector, forward, forwardMeasured, 3, TravelDirection.Forward,
|
||||
effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.SegmentMismatch,
|
||||
"segment-mismatch handoff");
|
||||
AssertHandoffRejected(selector, forward, forwardMeasured, 2, TravelDirection.Reverse,
|
||||
effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.DirectionMismatch,
|
||||
"direction-mismatch handoff");
|
||||
VehicleMotionState beyondMeasured = CreateMeasuredState(0.031d, 0d, 3.3066666666666666d, 0.10d,
|
||||
effectiveAt.AddSeconds(0.31d), 44L);
|
||||
AssertHandoffRejected(selector, forward, beyondMeasured, 2, TravelDirection.Forward,
|
||||
effectiveAt.AddSeconds(0.31d), configuration, TrajectoryHandoffRejectionReason.HandoffBeyondTrajectory,
|
||||
"beyond-trajectory handoff");
|
||||
|
||||
EmTrajectory gearBoundary = CreateHandoffTrajectory("handoff-gear", TravelDirection.Forward, 2, effectiveAt,
|
||||
EmBoundaryType.RollingSafetyStop, true);
|
||||
AssertHandoffRejected(selector, gearBoundary, forwardMeasured, 2, TravelDirection.Forward,
|
||||
effectiveAt.AddSeconds(0.10d), configuration, TrajectoryHandoffRejectionReason.GearBoundary,
|
||||
"gear-boundary handoff");
|
||||
Verification.True(!new TrajectorySampler().TrySample(gearBoundary, 0.15d, out _),
|
||||
"sampler never interpolates across different boundary types");
|
||||
|
||||
var service = new ControlledPlanningService();
|
||||
var coordinator = new EmPlanningCoordinator(service);
|
||||
PlanningCycleInput coordinatorInput = CreateInput(CreateMap(22), "handoff-reference", 40L, "handoff-forward",
|
||||
2, "handoff-publish", effectiveAt.AddSeconds(0.10d), configuration, forwardMeasured);
|
||||
Task<PlanningCycleResult> publication = coordinator.PlanLatestAsync(coordinatorInput, CancellationToken.None);
|
||||
service.WaitUntilStarted(coordinatorInput.Request.OutputTrajectoryId);
|
||||
service.Complete(coordinatorInput.Request.OutputTrajectoryId, forward);
|
||||
publication.GetAwaiter().GetResult();
|
||||
TrajectoryHandoffSelection coordinatorSelection = coordinator.SelectHandoff(coordinatorInput, TravelDirection.Forward);
|
||||
Verification.Equal(TrajectoryHandoffSource.PreviousTrajectory, coordinatorSelection.Source,
|
||||
"coordinator consumes only its published immutable trajectory");
|
||||
}
|
||||
|
||||
private static void VerifiesPublishedForwardAndReverseCommands()
|
||||
{
|
||||
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(800d);
|
||||
var service = new ControlledPlanningService();
|
||||
var coordinator = new EmPlanningCoordinator(service);
|
||||
var executor = new TrajectoryExecutor();
|
||||
|
||||
PlanningCycleInput forwardInput = CreateInput(CreateMap(30), "rolling-reference", 90L, string.Empty, 6,
|
||||
"rolling-forward", effectiveAt, null, CreateMeasuredState(0d, 0d, 0d, 0.10d, effectiveAt, 90L));
|
||||
EmTrajectory forward = CreateExecutionTrajectory("rolling-forward", TravelDirection.Forward, effectiveAt,
|
||||
EmBoundaryType.RollingSafetyStop, EmTerminalType.RollingSafetyStop);
|
||||
PlanningCycleResult forwardResult = Publish(service, coordinator, forwardInput, forward);
|
||||
Verification.True(forwardResult.Published, "first forward rolling cycle publishes");
|
||||
TrajectoryControlCommand forwardCommand = executor.UpdateCommand(effectiveAt,
|
||||
forwardInput.Request.VehicleState, coordinator.PublishedTrajectory, TravelDirection.Forward,
|
||||
TravelDirection.Forward, false);
|
||||
AssertPublishedMotion(forwardCommand, coordinator.PublishedTrajectory.Points[0], "forward rolling command");
|
||||
|
||||
PlanningCycleInput forwardRepeat = CreateInput(CreateMap(31), "rolling-reference", 91L, "rolling-forward", 6,
|
||||
"rolling-forward-repeat", effectiveAt.AddSeconds(0.20d), null,
|
||||
CreateMeasuredState(0.02d, 0d, 0d, 0.10d, effectiveAt.AddSeconds(0.20d), 91L));
|
||||
EmTrajectory repeatedForward = CreateExecutionTrajectory("rolling-forward-repeat", TravelDirection.Forward,
|
||||
effectiveAt.AddSeconds(0.20d), EmBoundaryType.RollingSafetyStop, EmTerminalType.RollingSafetyStop);
|
||||
Publish(service, coordinator, forwardRepeat, repeatedForward);
|
||||
Verification.Equal("rolling-forward-repeat", coordinator.PublishedTrajectory.Metadata.TrajectoryId,
|
||||
"repeated forward replan replaces only the published trajectory");
|
||||
|
||||
PlanningCycleInput reverseInput = CreateInput(CreateMap(32), "rolling-reference", 92L,
|
||||
"rolling-forward-repeat", 7, "rolling-reverse", effectiveAt.AddSeconds(0.40d), null,
|
||||
CreateMeasuredState(0d, 0d, 0d, -0.10d, effectiveAt.AddSeconds(0.40d), 92L));
|
||||
EmTrajectory reverse = CreateExecutionTrajectory("rolling-reverse", TravelDirection.Reverse,
|
||||
effectiveAt.AddSeconds(0.40d), EmBoundaryType.Goal, EmTerminalType.Goal);
|
||||
PlanningCycleResult reverseResult = Publish(service, coordinator, reverseInput, reverse);
|
||||
Verification.True(reverseResult.Published, "reverse rolling cycle publishes");
|
||||
TrajectoryControlCommand reverseCommand = executor.UpdateCommand(effectiveAt.AddSeconds(0.40d),
|
||||
reverseInput.Request.VehicleState, coordinator.PublishedTrajectory, TravelDirection.Reverse,
|
||||
TravelDirection.Reverse, false);
|
||||
AssertPublishedMotion(reverseCommand, coordinator.PublishedTrajectory.Points[0], "reverse rolling command");
|
||||
Verification.True(reverseCommand.SignedLongitudinalVelocity < 0d,
|
||||
"reverse rolling command preserves negative longitudinal velocity");
|
||||
|
||||
PlanningCycleInput unsafeTracking = CreateInput(CreateMap(32), "rolling-reference", 93L, "rolling-reverse", 7,
|
||||
"unsafe-tracking", effectiveAt.AddSeconds(0.41d), CreateHandoffConfiguration(),
|
||||
CreateMeasuredState(5d, 0d, 0d, -0.10d, effectiveAt.AddSeconds(0.41d), 93L));
|
||||
TrajectoryHandoffSelection reset = coordinator.SelectHandoff(unsafeTracking, TravelDirection.Reverse);
|
||||
Verification.Equal(TrajectoryHandoffSource.MeasuredState, reset.Source,
|
||||
"unsafe tracking resets rolling handoff to caller measurement");
|
||||
Verification.True(object.ReferenceEquals(unsafeTracking.Request.VehicleState, reset.StartState),
|
||||
"unsafe tracking preserves the supplied measurement snapshot");
|
||||
}
|
||||
|
||||
private static void VerifiesFailureLeavesPublishedZeroSpeedTailExecutable()
|
||||
{
|
||||
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(900d);
|
||||
var service = new ControlledPlanningService();
|
||||
var coordinator = new EmPlanningCoordinator(service);
|
||||
var executor = new TrajectoryExecutor();
|
||||
PlanningCycleInput successfulInput = CreateInput(CreateMap(40), "tail-reference", 100L, string.Empty, 8,
|
||||
"tail-published", effectiveAt, null, CreateMeasuredState(0d, 0d, 0d, 0.10d, effectiveAt, 100L));
|
||||
EmTrajectory published = CreateExecutionTrajectory("tail-published", TravelDirection.Forward, effectiveAt,
|
||||
EmBoundaryType.RollingSafetyStop, EmTerminalType.RollingSafetyStop);
|
||||
Publish(service, coordinator, successfulInput, published);
|
||||
|
||||
for (int attempt = 1; attempt <= 2; attempt++)
|
||||
{
|
||||
DateTimeOffset now = effectiveAt.AddSeconds(0.20d * attempt);
|
||||
PlanningCycleInput failedInput = CreateInput(CreateMap(40 + attempt), "tail-reference", 100L + attempt,
|
||||
"tail-published", 8, "tail-failed-" + attempt, now, null,
|
||||
CreateMeasuredState(0.02d, 0d, 0d, 0.10d, now, 100L + attempt));
|
||||
Task<PlanningCycleResult> failedCycle = coordinator.PlanLatestAsync(failedInput, CancellationToken.None);
|
||||
service.WaitUntilStarted(failedInput.Request.OutputTrajectoryId);
|
||||
service.Fail(failedInput.Request.OutputTrajectoryId);
|
||||
PlanningCycleResult failure = failedCycle.GetAwaiter().GetResult();
|
||||
Verification.Equal(EmPlanningStatus.Failed, failure.Result.Status, "failed replan status " + attempt);
|
||||
Verification.True(!failure.Published, "failed replan cannot publish " + attempt);
|
||||
Verification.True(object.ReferenceEquals(published, coordinator.PublishedTrajectory),
|
||||
"failed replan retains the complete prior trajectory " + attempt);
|
||||
}
|
||||
|
||||
TrajectoryControlCommand beforeTail = executor.UpdateCommand(effectiveAt.AddSeconds(0.10d),
|
||||
CreateMeasuredState(0.10d, 0d, 0d, 0.10d, effectiveAt.AddSeconds(0.10d), 103L),
|
||||
coordinator.PublishedTrajectory, TravelDirection.Forward, TravelDirection.Forward, false);
|
||||
Verification.True(beforeTail.SignedLongitudinalVelocity > 0d,
|
||||
"old trajectory remains executable before its safety tail");
|
||||
|
||||
TrajectoryControlCommand atTail = executor.UpdateCommand(effectiveAt.AddSeconds(0.30d),
|
||||
CreateMeasuredState(0d, 0d, 0d, 0d, effectiveAt.AddSeconds(0.30d), 104L), coordinator.PublishedTrajectory,
|
||||
TravelDirection.Forward, TravelDirection.Forward, false);
|
||||
TrajectoryControlCommand afterTail = executor.UpdateCommand(effectiveAt.AddSeconds(10d),
|
||||
CreateMeasuredState(0d, 0d, 0d, 0d, effectiveAt.AddSeconds(10d), 105L), coordinator.PublishedTrajectory,
|
||||
TravelDirection.Forward, TravelDirection.Forward, false);
|
||||
AssertZeroTail(atTail, "exact rolling safety-stop terminal");
|
||||
AssertZeroTail(afterTail, "after rolling safety-stop terminal");
|
||||
}
|
||||
|
||||
private static PlanningCycleResult Publish(ControlledPlanningService service, EmPlanningCoordinator coordinator,
|
||||
PlanningCycleInput input, EmTrajectory trajectory)
|
||||
{
|
||||
Task<PlanningCycleResult> cycle = coordinator.PlanLatestAsync(input, CancellationToken.None);
|
||||
service.WaitUntilStarted(input.Request.OutputTrajectoryId);
|
||||
service.Complete(input.Request.OutputTrajectoryId, trajectory);
|
||||
return cycle.GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
private static EmTrajectory CreateExecutionTrajectory(string trajectoryId, TravelDirection direction,
|
||||
DateTimeOffset effectiveAt, EmBoundaryType terminalBoundary, EmTerminalType terminalType)
|
||||
{
|
||||
double signedVelocity = direction == TravelDirection.Forward ? 0.10d : -0.10d;
|
||||
var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 110L, "rolling-reference", 100L,
|
||||
string.Empty, 8, direction, terminalType, terminalType == EmTerminalType.RollingSafetyStop
|
||||
? EmLongitudinalMode.RollingContinuation
|
||||
: EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon);
|
||||
return new EmTrajectory(metadata, new[]
|
||||
{
|
||||
new EmTrajectoryPoint(0d, 0d, 0d, signedVelocity, 0d, 0.20d, 8, 0d, 0d, direction,
|
||||
EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(signedVelocity * 3d, 0d, 0d, 0d, 0.30d, 0.20d, 8, 0.03d, 0.03d, direction,
|
||||
terminalBoundary, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private static void AssertPublishedMotion(TrajectoryControlCommand command, EmTrajectoryPoint point, string name)
|
||||
{
|
||||
Verification.NearlyEqual(point.SignedLongitudinalVelocity, command.SignedLongitudinalVelocity,
|
||||
name + " originates from the currently published trajectory");
|
||||
Verification.NearlyEqual(point.YawRate, command.YawRate,
|
||||
name + " preserves the currently published trajectory yaw rate");
|
||||
Verification.True(!command.HoldBrake && !command.IsTrajectoryComplete,
|
||||
name + " is neither a brake hold nor an extrapolated completion command");
|
||||
}
|
||||
|
||||
private static void AssertZeroTail(TrajectoryControlCommand command, string name)
|
||||
{
|
||||
Verification.NearlyEqual(0d, command.SignedLongitudinalVelocity, name + " signed velocity");
|
||||
Verification.NearlyEqual(0d, command.YawRate, name + " yaw rate");
|
||||
Verification.True(command.HoldBrake && command.IsTrajectoryComplete, name + " holds a completed trajectory");
|
||||
}
|
||||
|
||||
private static void AssertHandoffRejected(TrajectoryHandoffSelector selector, EmTrajectory trajectory,
|
||||
VehicleMotionState measuredState, int segmentIndex, TravelDirection direction, DateTimeOffset now,
|
||||
EmPlannerConfiguration configuration, TrajectoryHandoffRejectionReason reason, string name)
|
||||
{
|
||||
TrajectoryHandoffSelection selection = selector.Select(trajectory, measuredState, segmentIndex, direction, now,
|
||||
configuration);
|
||||
Verification.Equal(TrajectoryHandoffSource.MeasuredState, selection.Source, name + " source");
|
||||
Verification.Equal(reason, selection.RejectionReason, name + " reason");
|
||||
Verification.True(selection.PreviousTrajectory == null, name + " has no seed");
|
||||
Verification.True(object.ReferenceEquals(measuredState, selection.StartState), name + " returns measured state");
|
||||
}
|
||||
|
||||
private static EmPlannerConfiguration CreateHandoffConfiguration()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Validation.SpatialToleranceMeters = 0.05d;
|
||||
configuration.Validation.KinematicTolerance = 0.05d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static VehicleMotionState CreateMeasuredState(double x, double y, double yaw, double signedSpeed,
|
||||
DateTimeOffset capturedAt, long sequenceId)
|
||||
{
|
||||
return new VehicleMotionState(new Pose2D(x, y, yaw), signedSpeed, 0d, capturedAt, sequenceId);
|
||||
}
|
||||
|
||||
private static EmTrajectory CreateHandoffTrajectory(string trajectoryId, TravelDirection direction, int segmentIndex,
|
||||
DateTimeOffset effectiveAt, EmBoundaryType terminalBoundary, bool includeGearBoundary)
|
||||
{
|
||||
double sign = direction == TravelDirection.Forward ? 1d : -1d;
|
||||
EmBoundaryType middleBoundary = includeGearBoundary ? EmBoundaryType.GearSwitchApproach : EmBoundaryType.None;
|
||||
var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 55L, "handoff-reference", 39L,
|
||||
string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
|
||||
EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
|
||||
return new EmTrajectory(metadata, new[]
|
||||
{
|
||||
new EmTrajectoryPoint(0d, 0d, 3.10d, sign * 0.10d, 0d, 0.25d, segmentIndex, 0d, 0d,
|
||||
direction, EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(sign * 0.01d, 0d, 3.1666666666666665d, sign * 0.10d, 0.10d, 0.25d,
|
||||
segmentIndex, 0.01d, 0.01d, direction, EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(sign * 0.03d, 0d, 3.30d, sign * 0.10d, 0.30d, 0.25d, segmentIndex, 0.03d,
|
||||
0.03d, direction, middleBoundary, 0d, 0d),
|
||||
new EmTrajectoryPoint(sign * 0.055d, 0d, 3.4666666666666668d, sign * 0.10d, 0.55d, 0.25d,
|
||||
segmentIndex, 0.055d, 0.055d, direction, EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(sign * 0.06d, 0d, 3.50d, 0d, 0.60d, 0.25d, segmentIndex, 0.06d, 0.06d,
|
||||
direction, terminalBoundary, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private static void VerifySuperseded(string name, PlanningCycleInput older, PlanningCycleInput newer)
|
||||
{
|
||||
var service = new ControlledPlanningService();
|
||||
var coordinator = new EmPlanningCoordinator(service);
|
||||
|
||||
Task<PlanningCycleResult> slow = coordinator.PlanLatestAsync(older, CancellationToken.None);
|
||||
service.WaitUntilStarted(older.Request.OutputTrajectoryId);
|
||||
Task<PlanningCycleResult> current = coordinator.PlanLatestAsync(newer, CancellationToken.None);
|
||||
service.WaitUntilStarted(newer.Request.OutputTrajectoryId);
|
||||
|
||||
Verification.True(service.WasCancelled(older.Request.OutputTrajectoryId), name + " cancels older cycle token");
|
||||
service.Complete(newer.Request.OutputTrajectoryId);
|
||||
PlanningCycleResult currentResult = current.GetAwaiter().GetResult();
|
||||
service.Complete(older.Request.OutputTrajectoryId);
|
||||
PlanningCycleResult staleResult = slow.GetAwaiter().GetResult();
|
||||
|
||||
Verification.Equal(EmPlanningStatus.Success, currentResult.Result.Status, name + " current result succeeds");
|
||||
Verification.True(currentResult.Published, name + " current result publishes");
|
||||
Verification.Equal(EmPlanningStatus.Superseded, staleResult.Result.Status, name + " old result is superseded");
|
||||
Verification.True(staleResult.Result.Trajectory == null, name + " stale result exposes no trajectory");
|
||||
Verification.Equal(newer.Request.OutputTrajectoryId, coordinator.PublishedTrajectory.Metadata.TrajectoryId,
|
||||
name + " only latest trajectory is published");
|
||||
}
|
||||
|
||||
private static PlanningCycleInput CreateInput(PlanningGridMap map, string referencePathId, long stateSequenceId,
|
||||
string previousTrajectoryId, int segmentIndex, string outputTrajectoryId, DateTimeOffset now,
|
||||
EmPlannerConfiguration? configuration = null, VehicleMotionState? state = null)
|
||||
{
|
||||
configuration ??= EmPlannerConfiguration.CreateDefault();
|
||||
state ??= new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId);
|
||||
var request = new EmPlanningRequest(null, map, null, state, configuration, segmentIndex, null, now, now,
|
||||
outputTrajectoryId, referencePathId, previousTrajectoryId, EmMotionModel.NonholonomicForwardReverse,
|
||||
EmPlanningScope.RollingHorizon);
|
||||
return new PlanningCycleInput(request, now);
|
||||
}
|
||||
|
||||
private static PlanningGridMap CreateMap(int widthOffset)
|
||||
{
|
||||
PlanningMapBuildResult build = new PlanningMapFactory().Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(-1000f, 1000f + widthOffset * 20f, -1000f, 1000f),
|
||||
ResolutionMm = 20f,
|
||||
ObstacleSources = Array.Empty<IMapObstacleSource>(),
|
||||
AllowExplicitEmptyMap = true,
|
||||
});
|
||||
Verification.True(build.Succeeded && build.Map != null && build.Map.PlanningReady, "coordinator map builds");
|
||||
return build.Map!;
|
||||
}
|
||||
|
||||
private sealed class ControlledPlanningService : IEmPlanningService
|
||||
{
|
||||
private readonly object gate = new object();
|
||||
private readonly Dictionary<string, PendingCycle> pending = new Dictionary<string, PendingCycle>();
|
||||
|
||||
public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
||||
{
|
||||
var cycle = new PendingCycle(request, cancellationToken);
|
||||
lock (gate)
|
||||
{
|
||||
pending.Add(request.OutputTrajectoryId, cycle);
|
||||
Monitor.PulseAll(gate);
|
||||
}
|
||||
return cycle.Completion.Task.GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
public void WaitUntilStarted(string outputTrajectoryId)
|
||||
{
|
||||
DateTimeOffset timeout = DateTimeOffset.UtcNow.AddSeconds(5d);
|
||||
lock (gate)
|
||||
{
|
||||
while (!pending.ContainsKey(outputTrajectoryId))
|
||||
{
|
||||
TimeSpan remaining = timeout - DateTimeOffset.UtcNow;
|
||||
if (remaining <= TimeSpan.Zero || !Monitor.Wait(gate, remaining))
|
||||
throw new InvalidOperationException("Planner cycle did not start: " + outputTrajectoryId);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public bool WasCancelled(string outputTrajectoryId)
|
||||
{
|
||||
lock (gate)
|
||||
return pending[outputTrajectoryId].CancellationToken.IsCancellationRequested;
|
||||
}
|
||||
|
||||
public void Complete(string outputTrajectoryId, EmTrajectory? trajectory = null)
|
||||
{
|
||||
PendingCycle cycle;
|
||||
lock (gate)
|
||||
cycle = pending[outputTrajectoryId];
|
||||
EmPlanningResult result = trajectory == null
|
||||
? CreateSuccess(cycle.Request)
|
||||
: new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
|
||||
cycle.Completion.TrySetResult(result);
|
||||
}
|
||||
|
||||
public void Fail(string outputTrajectoryId)
|
||||
{
|
||||
PendingCycle cycle;
|
||||
lock (gate)
|
||||
cycle = pending[outputTrajectoryId];
|
||||
cycle.Completion.TrySetResult(new EmPlanningResult(EmPlanningStatus.Failed, null, "scripted rolling failure"));
|
||||
}
|
||||
|
||||
private static EmPlanningResult CreateSuccess(EmPlanningRequest request)
|
||||
{
|
||||
var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
|
||||
request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
|
||||
request.SegmentIndex, TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
|
||||
EmLongitudinalMode.RollingContinuation, request.PlanningScope);
|
||||
var point = new EmTrajectoryPoint(0d, 0d, 0d, 0d, 0d, 0d, request.SegmentIndex, 0d, 0d,
|
||||
TravelDirection.Forward, EmBoundaryType.RollingSafetyStop, 0d, 0d);
|
||||
return new EmPlanningResult(EmPlanningStatus.Success, new EmTrajectory(metadata, new[] { point }), string.Empty);
|
||||
}
|
||||
|
||||
private sealed class PendingCycle
|
||||
{
|
||||
public PendingCycle(EmPlanningRequest request, CancellationToken cancellationToken)
|
||||
{
|
||||
Request = request;
|
||||
CancellationToken = cancellationToken;
|
||||
Completion = new TaskCompletionSource<EmPlanningResult>(TaskCreationOptions.RunContinuationsAsynchronously);
|
||||
}
|
||||
|
||||
public EmPlanningRequest Request { get; }
|
||||
public CancellationToken CancellationToken { get; }
|
||||
public TaskCompletionSource<EmPlanningResult> Completion { get; }
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class ThrowingCycleSink : IEmPlanningCycleSink
|
||||
{
|
||||
public void OnCycleCompleted(PlanningCycleResult result)
|
||||
{
|
||||
throw new InvalidOperationException("cycle sink failure");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,182 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class CorridorChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesEmptyAndNarrowedCorridors();
|
||||
VerifiesSeedConnectedIntervalIsPreserved();
|
||||
VerifiesDisappearingSeedIntervalFails();
|
||||
}
|
||||
|
||||
private static void VerifiesEmptyAndNarrowedCorridors()
|
||||
{
|
||||
DirectionSegmentView segment = CreateStraightSegment();
|
||||
CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor;
|
||||
VehicleParameters vehicle = CreateVehicle();
|
||||
var builder = new StaticCorridorBuilder();
|
||||
|
||||
Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, Array.Empty<FrenetProjection>(),
|
||||
CreateMap(Array.Empty<IMapObstacle>()), vehicle, configuration, out StaticCorridor empty, out string emptyReason),
|
||||
"empty map corridor succeeds: " + emptyReason);
|
||||
VerifyAnchors(empty, 0.2d, 1.8d);
|
||||
for (int index = 1; index + 1 < empty.Stations.Count; index++)
|
||||
{
|
||||
Verification.NearlyEqual(-0.3d, empty.Stations[index].MinimumL, "empty map minimum l");
|
||||
Verification.NearlyEqual(0.3d, empty.Stations[index].MaximumL, "empty map maximum l");
|
||||
}
|
||||
|
||||
IMapObstacle[] leftNarrowing =
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(800f, 1200f, 150f, 400f),
|
||||
};
|
||||
PlanningGridMap narrowedMap = CreateMap(leftNarrowing);
|
||||
IReadOnlyList<FrenetProjection> seed = CreateSeed(segment, -0.2d);
|
||||
Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, seed, narrowedMap, vehicle, configuration,
|
||||
out StaticCorridor narrowed, out string narrowedReason), "narrowed corridor succeeds: " + narrowedReason);
|
||||
LateralInterval narrowedStation = FindStation(narrowed, 1d);
|
||||
Verification.True(narrowedStation.MaximumL < 0.3d, "left obstacle narrows positive-l side");
|
||||
VerifyAcceptedSamplesUseExactFootprints(segment, narrowed, narrowedMap, vehicle, configuration);
|
||||
}
|
||||
|
||||
private static void VerifiesSeedConnectedIntervalIsPreserved()
|
||||
{
|
||||
DirectionSegmentView segment = CreateStraightSegment();
|
||||
CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor;
|
||||
VehicleParameters vehicle = CreateVehicle();
|
||||
IMapObstacle[] split =
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(800f, 1200f, -50f, 50f),
|
||||
};
|
||||
IReadOnlyList<FrenetProjection> seed = CreateSeed(segment, -0.2d);
|
||||
var builder = new StaticCorridorBuilder();
|
||||
|
||||
Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, seed, CreateMap(split), vehicle, configuration,
|
||||
out StaticCorridor corridor, out string reason), "split corridor succeeds for left seed: " + reason);
|
||||
for (int index = 0; index < corridor.Stations.Count; index++)
|
||||
{
|
||||
LateralInterval station = corridor.Stations[index];
|
||||
Verification.True(station.MinimumL <= station.SeedL && station.SeedL <= station.MaximumL,
|
||||
"chosen interval contains seed at station " + index);
|
||||
}
|
||||
Verification.True(FindStation(corridor, 1d).MaximumL < 0d,
|
||||
"split station retains seed-connected left interval instead of right interval");
|
||||
}
|
||||
|
||||
private static void VerifiesDisappearingSeedIntervalFails()
|
||||
{
|
||||
DirectionSegmentView segment = CreateStraightSegment();
|
||||
CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor;
|
||||
VehicleParameters vehicle = CreateVehicle();
|
||||
IMapObstacle[] removesLeft =
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(800f, 1200f, -400f, -50f),
|
||||
};
|
||||
var builder = new StaticCorridorBuilder();
|
||||
|
||||
Verification.True(!builder.TryBuild(segment, 0.2d, 1.8d, CreateSeed(segment, -0.2d), CreateMap(removesLeft),
|
||||
vehicle, configuration, out _, out string failureReason),
|
||||
"disappearing seed-connected interval does not switch sides");
|
||||
Verification.True(failureReason.IndexOf("S=", StringComparison.Ordinal) >= 0,
|
||||
"failure identifies the first failed reference-s station: " + failureReason);
|
||||
}
|
||||
|
||||
private static void VerifyAnchors(StaticCorridor corridor, double startS, double endS)
|
||||
{
|
||||
Verification.NearlyEqual(startS, corridor.Stations[0].ReferenceS, "first station is exact requested start");
|
||||
Verification.NearlyEqual(endS, corridor.Stations[corridor.Stations.Count - 1].ReferenceS,
|
||||
"last station is exact requested end");
|
||||
}
|
||||
|
||||
private static void VerifyAcceptedSamplesUseExactFootprints(DirectionSegmentView segment, StaticCorridor corridor,
|
||||
PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration)
|
||||
{
|
||||
var checker = new FootprintCollisionChecker();
|
||||
for (int stationIndex = 0; stationIndex < corridor.Stations.Count; stationIndex++)
|
||||
{
|
||||
LateralInterval station = corridor.Stations[stationIndex];
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, station.ReferenceS);
|
||||
for (double l = station.MinimumL; l <= station.MaximumL + 1e-12d; l += configuration.LateralSampleSpacingMeters)
|
||||
{
|
||||
Verification.True(FrenetTransform.TryReconstruct(reference, l, 0d, 0.2d, out Pose2D pose),
|
||||
"accepted sample reconstructs");
|
||||
Verification.True(checker.IsPoseCollisionFree(pose, map, vehicle, configuration.AdditionalClearanceReserveMeters,
|
||||
out _), "accepted sample passes exact rotated footprint");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static DirectionSegmentView CreateStraightSegment()
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d),
|
||||
Point(1d, 1d),
|
||||
Point(2d, 2d),
|
||||
};
|
||||
return new DirectionSegmentView(0, TravelDirection.Forward, points,
|
||||
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
||||
new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<FrenetProjection> CreateSeed(DirectionSegmentView segment, double l)
|
||||
{
|
||||
return new FrenetProjection[]
|
||||
{
|
||||
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0.2d), l, 0d, 0d),
|
||||
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 1.8d), l, 0d, 0d),
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothedPathPoint Point(double x, double s)
|
||||
{
|
||||
return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d,
|
||||
false, SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
|
||||
private static VehicleParameters CreateVehicle()
|
||||
{
|
||||
return new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.10d,
|
||||
WidthMeters = 0.10d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 1d,
|
||||
};
|
||||
}
|
||||
|
||||
private static PlanningGridMap CreateMap(IReadOnlyList<IMapObstacle> obstacles)
|
||||
{
|
||||
IMapObstacleSource[] sources = obstacles.Count == 0
|
||||
? Array.Empty<IMapObstacleSource>()
|
||||
: new IMapObstacleSource[] { new ManualObstacleSource("corridor-test", 1L, true, obstacles) };
|
||||
var result = new PlanningMapFactory().Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f),
|
||||
ResolutionMm = 20f,
|
||||
ObstacleSources = sources,
|
||||
AllowExplicitEmptyMap = obstacles.Count == 0,
|
||||
});
|
||||
Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady,
|
||||
"corridor test map builds: " + result.FailureReason);
|
||||
return result.Map!;
|
||||
}
|
||||
|
||||
private static LateralInterval FindStation(StaticCorridor corridor, double referenceS)
|
||||
{
|
||||
for (int index = 0; index < corridor.Stations.Count; index++)
|
||||
{
|
||||
if (Math.Abs(corridor.Stations[index].ReferenceS - referenceS) <= 1e-12d)
|
||||
return corridor.Stations[index];
|
||||
}
|
||||
throw new InvalidOperationException("Requested corridor station was not sampled.");
|
||||
}
|
||||
}
|
||||
@@ -1,12 +0,0 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
<PropertyGroup>
|
||||
<OutputType>Exe</OutputType>
|
||||
<TargetFramework>net10.0-windows</TargetFramework>
|
||||
<ImplicitUsings>disable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\..\ClumsyPilot.csproj"
|
||||
AdditionalProperties="ExcludeLegacyAutoAvoidance=true" />
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
@@ -1,44 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace EMPlannerVerificationHost;
|
||||
|
||||
internal static class EmFixtureFactory
|
||||
{
|
||||
public static PathSmoothingResult CreateGearPairReferencePath(double firstSegmentLength = 2d)
|
||||
{
|
||||
if (firstSegmentLength <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(firstSegmentLength));
|
||||
double midpoint = 0.5d * firstSegmentLength;
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
||||
Point(midpoint, midpoint, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
||||
Point(firstSegmentLength, firstSegmentLength, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
||||
Point(firstSegmentLength, firstSegmentLength, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
|
||||
Point(midpoint, firstSegmentLength + midpoint, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
|
||||
Point(0d, 2d * firstSegmentLength, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
var segments = new List<SmoothedPathSegment>
|
||||
{
|
||||
new SmoothedPathSegment(0, TravelDirection.Forward, 0, 2, false, true),
|
||||
new SmoothedPathSegment(1, TravelDirection.Reverse, 3, 5, true, false),
|
||||
};
|
||||
var metrics = new PathQualityMetrics(true, 2d * firstSegmentLength, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
|
||||
return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
|
||||
new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
|
||||
}
|
||||
|
||||
private static SmoothedPathPoint Point(
|
||||
double x,
|
||||
double arcLength,
|
||||
TravelDirection direction,
|
||||
bool isGearSwitchPoint,
|
||||
SmoothedPathPointSource source)
|
||||
{
|
||||
return new SmoothedPathPoint(x, 0d, 0d, 0d, arcLength, direction, 0d, 0d, 0d, 1d,
|
||||
isGearSwitchPoint, source);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,296 +0,0 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace EMPlannerVerificationHost;
|
||||
|
||||
internal static class ExecutorChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesGearSwitchSequence(TravelDirection.Forward, TravelDirection.Reverse, "forward-to-reverse");
|
||||
VerifiesGearSwitchSequence(TravelDirection.Reverse, TravelDirection.Forward, "reverse-to-forward");
|
||||
VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals();
|
||||
VerifiesGenericControlCommandMapping();
|
||||
VerifiesVehicleStateProviderRemainsSnapshotOnly();
|
||||
}
|
||||
|
||||
public static void RunRollingEndToEnd()
|
||||
{
|
||||
VerifiesGearSwitchSequence(TravelDirection.Forward, TravelDirection.Reverse,
|
||||
"end-to-end forward-to-reverse");
|
||||
VerifiesGearSwitchSequence(TravelDirection.Reverse, TravelDirection.Forward,
|
||||
"end-to-end reverse-to-forward");
|
||||
VerifiesGoalCommandStopsAtAndAfterTerminal();
|
||||
}
|
||||
|
||||
private static void VerifiesGearSwitchSequence(TravelDirection currentDirection, TravelDirection desiredDirection,
|
||||
string name)
|
||||
{
|
||||
var machine = new GearSwitchStateMachine(0.01d, 0.20d);
|
||||
DateTimeOffset start = DateTimeOffset.UnixEpoch.AddSeconds(500d);
|
||||
|
||||
GearSwitchStateUpdate approaching = machine.Update(start, 0.10d, desiredDirection, currentDirection, false, false,
|
||||
false);
|
||||
Verification.Equal(GearSwitchState.ApproachingGearSwitch, approaching.State, name + " approaches switch");
|
||||
Verification.True(approaching.AllowsTrajectoryMotion, name + " approach permits trajectory motion");
|
||||
|
||||
GearSwitchStateUpdate holding = machine.Update(start.AddMilliseconds(100), 0.005d, desiredDirection,
|
||||
currentDirection, false, true, false);
|
||||
AssertHeld(holding, GearSwitchState.HoldingZero, name + " holds at boundary");
|
||||
|
||||
GearSwitchStateUpdate movingAgain = machine.Update(start.AddMilliseconds(110), 0.01d, desiredDirection,
|
||||
currentDirection, false, true, false);
|
||||
AssertHeld(movingAgain, GearSwitchState.HoldingZero, name + " holds while measured speed is at tolerance");
|
||||
|
||||
GearSwitchStateUpdate dwellRestart = machine.Update(start.AddMilliseconds(120), 0d, desiredDirection,
|
||||
currentDirection, false, true, false);
|
||||
AssertHeld(dwellRestart, GearSwitchState.HoldingZero, name + " starts zero-speed dwell");
|
||||
GearSwitchStateUpdate dwellShort = machine.Update(start.AddMilliseconds(319), 0d, desiredDirection,
|
||||
currentDirection, false, true, false);
|
||||
AssertHeld(dwellShort, GearSwitchState.HoldingZero, name + " does not request before full dwell");
|
||||
|
||||
GearSwitchStateUpdate requesting = machine.Update(start.AddMilliseconds(320), 0d, desiredDirection,
|
||||
currentDirection, false, true, false);
|
||||
AssertHeld(requesting, GearSwitchState.RequestingDirectionChange, name + " requests after continuous dwell");
|
||||
Verification.True(requesting.RequestDirectionChange, name + " emits exactly one direction request");
|
||||
|
||||
GearSwitchStateUpdate awaiting = machine.Update(start.AddMilliseconds(321), 0d, desiredDirection,
|
||||
currentDirection, false, true, false);
|
||||
AssertHeld(awaiting, GearSwitchState.AwaitingDirectionConfirmation, name + " awaits confirmation");
|
||||
Verification.True(!awaiting.RequestDirectionChange, name + " does not repeat direction request");
|
||||
|
||||
GearSwitchStateUpdate following = machine.Update(start.AddMilliseconds(322), 0d, desiredDirection,
|
||||
desiredDirection, true, false, false);
|
||||
Verification.Equal(GearSwitchState.Following, following.State, name + " follows confirmed next segment");
|
||||
Verification.True(!following.HoldZero && following.AllowsTrajectoryMotion,
|
||||
name + " resumes only after confirmation");
|
||||
|
||||
GearSwitchStateUpdate completed = machine.Update(start.AddMilliseconds(323), 0d, desiredDirection,
|
||||
desiredDirection, false, false, true);
|
||||
AssertHeld(completed, GearSwitchState.Completed, name + " completes at terminal while holding zero");
|
||||
Verification.True(completed.IsTrajectoryComplete, name + " marks terminal completion");
|
||||
}
|
||||
|
||||
private static void VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals()
|
||||
{
|
||||
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(600d);
|
||||
var executor = new TrajectoryExecutor();
|
||||
EmTrajectory gearTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward,
|
||||
EmBoundaryType.GearSwitchApproach, EmTerminalType.GearSwitch);
|
||||
|
||||
TrajectoryExecutionState approaching = executor.Update(effectiveAt.AddSeconds(0.10d),
|
||||
CreateMeasuredState(0.10d, effectiveAt.AddSeconds(0.10d), 60L), gearTrajectory, TravelDirection.Reverse,
|
||||
TravelDirection.Forward, false);
|
||||
Verification.Equal(GearSwitchState.ApproachingGearSwitch, approaching.GearSwitchState,
|
||||
"executor enters approach before exact gear boundary");
|
||||
Verification.NearlyEqual(0.10d, approaching.SelectedPoint.SignedLongitudinalVelocity,
|
||||
"executor samples moving approach point");
|
||||
|
||||
TrajectoryExecutionState holding = executor.Update(effectiveAt.AddSeconds(0.30d),
|
||||
CreateMeasuredState(0.005d, effectiveAt.AddSeconds(0.30d), 61L), gearTrajectory, TravelDirection.Reverse,
|
||||
TravelDirection.Forward, false);
|
||||
AssertHeld(holding, GearSwitchState.HoldingZero, "executor holds at exact gear boundary");
|
||||
Verification.Equal(EmBoundaryType.GearSwitchApproach, holding.SelectedPoint.BoundaryType,
|
||||
"executor preserves exact gear boundary point");
|
||||
Verification.NearlyEqual(0d, holding.SelectedPoint.SignedLongitudinalVelocity,
|
||||
"executor does not release nonzero speed while holding");
|
||||
|
||||
foreach (EmBoundaryType terminalBoundary in new[] { EmBoundaryType.Goal, EmBoundaryType.RollingSafetyStop })
|
||||
{
|
||||
var terminalExecutor = new TrajectoryExecutor();
|
||||
EmTrajectory terminal = CreateTrajectory(effectiveAt, TravelDirection.Forward, terminalBoundary,
|
||||
terminalBoundary == EmBoundaryType.Goal ? EmTerminalType.Goal : EmTerminalType.RollingSafetyStop);
|
||||
TrajectoryExecutionState completed = terminalExecutor.Update(effectiveAt.AddSeconds(0.30d),
|
||||
CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 62L), terminal, TravelDirection.Forward,
|
||||
TravelDirection.Forward, false);
|
||||
AssertHeld(completed, GearSwitchState.Completed, "executor completes " + terminalBoundary);
|
||||
Verification.True(completed.IsTrajectoryComplete, "executor marks " + terminalBoundary + " completion");
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifiesGenericControlCommandMapping()
|
||||
{
|
||||
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(700d);
|
||||
var adapter = new TrajectoryControlAdapter();
|
||||
|
||||
VerifiesFollowingCommand(adapter, effectiveAt, TravelDirection.Forward, 0.12d, 0.35d, 0.042d,
|
||||
"forward command");
|
||||
VerifiesFollowingCommand(adapter, effectiveAt, TravelDirection.Reverse, -0.12d, 0.35d, -0.042d,
|
||||
"reverse command");
|
||||
|
||||
var executor = new TrajectoryExecutor();
|
||||
EmTrajectory gearTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward,
|
||||
EmBoundaryType.GearSwitchApproach, EmTerminalType.GearSwitch);
|
||||
executor.Update(effectiveAt, CreateMeasuredState(0.10d, effectiveAt, 80L), gearTrajectory,
|
||||
TravelDirection.Reverse, TravelDirection.Forward, false);
|
||||
TrajectoryExecutionState holding = executor.Update(effectiveAt.AddSeconds(0.30d),
|
||||
CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 81L), gearTrajectory,
|
||||
TravelDirection.Reverse, TravelDirection.Forward, false);
|
||||
TrajectoryControlCommand holdCommand = adapter.CreateCommand(holding.SelectedPoint, holding);
|
||||
Verification.NearlyEqual(0d, holdCommand.SignedLongitudinalVelocity,
|
||||
"holding command overrides signed longitudinal velocity");
|
||||
Verification.NearlyEqual(0d, holdCommand.YawRate, "holding command overrides yaw rate");
|
||||
Verification.True(holdCommand.HoldBrake && !holdCommand.IsTrajectoryComplete,
|
||||
"gear holding command requests brake without completing trajectory");
|
||||
|
||||
TrajectoryExecutionState requesting = executor.Update(effectiveAt.AddSeconds(0.50d),
|
||||
CreateMeasuredState(0d, effectiveAt.AddSeconds(0.50d), 82L), gearTrajectory,
|
||||
TravelDirection.Reverse, TravelDirection.Forward, false);
|
||||
TrajectoryControlCommand requestCommand = adapter.CreateCommand(requesting.SelectedPoint, requesting);
|
||||
Verification.True(requestCommand.RequestDirectionChange && requestCommand.HoldBrake,
|
||||
"direction request remains a generic zero-speed brake command");
|
||||
|
||||
EmTrajectory terminalTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward, EmBoundaryType.Goal,
|
||||
EmTerminalType.Goal);
|
||||
var terminalExecutor = new TrajectoryExecutor();
|
||||
TrajectoryExecutionState completed = terminalExecutor.Update(effectiveAt.AddSeconds(0.30d),
|
||||
CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 82L), terminalTrajectory,
|
||||
TravelDirection.Forward, TravelDirection.Forward, false);
|
||||
TrajectoryControlCommand completeCommand = adapter.CreateCommand(completed.SelectedPoint, completed);
|
||||
Verification.NearlyEqual(0d, completeCommand.SignedLongitudinalVelocity,
|
||||
"completed command keeps signed longitudinal velocity at zero");
|
||||
Verification.NearlyEqual(0d, completeCommand.YawRate, "completed command keeps yaw rate at zero");
|
||||
Verification.True(completeCommand.HoldBrake && completeCommand.IsTrajectoryComplete,
|
||||
"completed command holds brake and reports completion");
|
||||
|
||||
Verification.True(typeof(TrajectoryControlCommand).GetProperty("BodyLateralVelocity") == null,
|
||||
"generic command exposes no body lateral velocity");
|
||||
}
|
||||
|
||||
private static void VerifiesFollowingCommand(TrajectoryControlAdapter adapter, DateTimeOffset effectiveAt,
|
||||
TravelDirection direction, double signedVelocity, double curvature, double expectedYawRate, string name)
|
||||
{
|
||||
EmTrajectory trajectory = CreateMotionTrajectory(effectiveAt, direction, signedVelocity, curvature);
|
||||
var executor = new TrajectoryExecutor();
|
||||
VehicleMotionState measured = CreateMeasuredState(signedVelocity, effectiveAt, 83L);
|
||||
TrajectoryExecutionState following = executor.Update(effectiveAt, measured, trajectory, direction, direction, false);
|
||||
TrajectoryControlCommand command = adapter.CreateCommand(following.SelectedPoint, following);
|
||||
TrajectoryControlCommand executorCommand = executor.UpdateCommand(effectiveAt, measured, trajectory, direction,
|
||||
direction, false);
|
||||
|
||||
Verification.NearlyEqual(following.SelectedPoint.SignedLongitudinalVelocity, command.SignedLongitudinalVelocity,
|
||||
name + " copies signed longitudinal velocity exactly");
|
||||
Verification.NearlyEqual(following.SelectedPoint.YawRate, command.YawRate,
|
||||
name + " copies yaw rate exactly");
|
||||
Verification.NearlyEqual(expectedYawRate, command.YawRate, name + " preserves signed yaw rate");
|
||||
Verification.Equal(direction, command.Direction, name + " preserves direction");
|
||||
Verification.True(!command.HoldBrake && !command.RequestDirectionChange && !command.IsTrajectoryComplete,
|
||||
name + " remains a normal following command");
|
||||
Verification.NearlyEqual(command.SignedLongitudinalVelocity, executorCommand.SignedLongitudinalVelocity,
|
||||
name + " executor command preserves signed longitudinal velocity");
|
||||
Verification.NearlyEqual(command.YawRate, executorCommand.YawRate,
|
||||
name + " executor command preserves yaw rate");
|
||||
Verification.True(command.SignedLongitudinalVelocity != 0d || command.YawRate == 0d,
|
||||
name + " never creates in-place rotation");
|
||||
Verification.NearlyEqual(Math.Abs(signedVelocity), following.SelectedPoint.Speed,
|
||||
name + " leaves speed available in execution telemetry");
|
||||
Verification.NearlyEqual(signedVelocity * Math.Cos(following.SelectedPoint.Yaw), following.SelectedPoint.VelocityX,
|
||||
name + " leaves world velocity X in execution telemetry");
|
||||
Verification.NearlyEqual(signedVelocity * Math.Sin(following.SelectedPoint.Yaw), following.SelectedPoint.VelocityY,
|
||||
name + " leaves world velocity Y in execution telemetry");
|
||||
Verification.NearlyEqual(curvature, following.SelectedPoint.VehicleCurvature,
|
||||
name + " leaves curvature available in execution telemetry");
|
||||
}
|
||||
|
||||
private static void VerifiesVehicleStateProviderRemainsSnapshotOnly()
|
||||
{
|
||||
DateTimeOffset capturedAt = DateTimeOffset.UnixEpoch.AddSeconds(710d);
|
||||
IVehicleStateProvider provider = new FixedVehicleStateProvider(CreateMeasuredState(0.02d, capturedAt, 84L));
|
||||
Verification.Equal(84L, provider.Capture().SequenceId, "vehicle state provider returns caller-owned snapshot");
|
||||
}
|
||||
|
||||
private static void VerifiesGoalCommandStopsAtAndAfterTerminal()
|
||||
{
|
||||
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(950d);
|
||||
EmTrajectory goalTrajectory = CreateMotionTrajectory(effectiveAt, TravelDirection.Forward, 0.10d, 0.20d);
|
||||
var executor = new TrajectoryExecutor();
|
||||
VehicleMotionState moving = CreateMeasuredState(0.10d, effectiveAt, 95L);
|
||||
TrajectoryControlCommand following = executor.UpdateCommand(effectiveAt, moving, goalTrajectory,
|
||||
TravelDirection.Forward, TravelDirection.Forward, false);
|
||||
Verification.NearlyEqual(goalTrajectory.Points[0].SignedLongitudinalVelocity, following.SignedLongitudinalVelocity,
|
||||
"goal approach command originates from trajectory point");
|
||||
Verification.NearlyEqual(goalTrajectory.Points[0].YawRate, following.YawRate,
|
||||
"goal approach command preserves trajectory yaw rate");
|
||||
|
||||
TrajectoryControlCommand atGoal = executor.UpdateCommand(effectiveAt.AddSeconds(0.30d),
|
||||
CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 96L), goalTrajectory, TravelDirection.Forward,
|
||||
TravelDirection.Forward, false);
|
||||
TrajectoryControlCommand afterGoal = executor.UpdateCommand(effectiveAt.AddSeconds(5d),
|
||||
CreateMeasuredState(0d, effectiveAt.AddSeconds(5d), 97L), goalTrajectory, TravelDirection.Forward,
|
||||
TravelDirection.Forward, false);
|
||||
AssertCompletedCommand(atGoal, "exact goal terminal command");
|
||||
AssertCompletedCommand(afterGoal, "after goal terminal command");
|
||||
}
|
||||
|
||||
private static void AssertCompletedCommand(TrajectoryControlCommand command, string name)
|
||||
{
|
||||
Verification.NearlyEqual(0d, command.SignedLongitudinalVelocity, name + " signed velocity");
|
||||
Verification.NearlyEqual(0d, command.YawRate, name + " yaw rate");
|
||||
Verification.True(command.HoldBrake && command.IsTrajectoryComplete, name + " holds completed trajectory");
|
||||
}
|
||||
|
||||
private static void AssertHeld(GearSwitchStateUpdate update, GearSwitchState expectedState, string name)
|
||||
{
|
||||
Verification.Equal(expectedState, update.State, name + " state");
|
||||
Verification.True(update.HoldZero && !update.AllowsTrajectoryMotion, name + " forbids nonzero motion output");
|
||||
}
|
||||
|
||||
private static void AssertHeld(TrajectoryExecutionState state, GearSwitchState expectedState, string name)
|
||||
{
|
||||
Verification.Equal(expectedState, state.GearSwitchState, name + " state");
|
||||
Verification.True(state.HoldZero && !state.AllowsTrajectoryMotion, name + " forbids nonzero motion output");
|
||||
}
|
||||
|
||||
private static VehicleMotionState CreateMeasuredState(double speed, DateTimeOffset capturedAt, long sequenceId)
|
||||
{
|
||||
return new VehicleMotionState(new Pose2D(0d, 0d, 0d), speed, 0d, capturedAt, sequenceId);
|
||||
}
|
||||
|
||||
private static EmTrajectory CreateTrajectory(DateTimeOffset effectiveAt, TravelDirection direction,
|
||||
EmBoundaryType terminalBoundary, EmTerminalType terminalType)
|
||||
{
|
||||
double signedSpeed = direction == TravelDirection.Forward ? 0.10d : -0.10d;
|
||||
var metadata = new EmTrajectoryMetadata("executor-" + terminalBoundary, effectiveAt, effectiveAt, 70L,
|
||||
"executor-reference", 60L, string.Empty, 4, direction, terminalType,
|
||||
terminalType == EmTerminalType.RollingSafetyStop
|
||||
? EmLongitudinalMode.RollingContinuation
|
||||
: EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon);
|
||||
return new EmTrajectory(metadata, new[]
|
||||
{
|
||||
new EmTrajectoryPoint(0d, 0d, 0d, signedSpeed, 0d, 0d, 4, 0d, 0d, direction, EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(signedSpeed * 3d, 0d, 0d, 0d, 0.30d, 0d, 4, 0.03d, 0.03d, direction,
|
||||
terminalBoundary, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private static EmTrajectory CreateMotionTrajectory(DateTimeOffset effectiveAt, TravelDirection direction,
|
||||
double signedVelocity, double curvature)
|
||||
{
|
||||
var metadata = new EmTrajectoryMetadata("control-" + direction, effectiveAt, effectiveAt, 85L,
|
||||
"control-reference", 84L, string.Empty, 5, direction, EmTerminalType.Goal,
|
||||
EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon);
|
||||
return new EmTrajectory(metadata, new[]
|
||||
{
|
||||
new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, signedVelocity, 0d, curvature, 5, 0d, 0d, direction,
|
||||
EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, 0d, 0.30d, curvature, 5, 0.04d, 0.04d, direction,
|
||||
EmBoundaryType.Goal, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private sealed class FixedVehicleStateProvider : IVehicleStateProvider
|
||||
{
|
||||
private readonly VehicleMotionState state;
|
||||
|
||||
public FixedVehicleStateProvider(VehicleMotionState state)
|
||||
{
|
||||
this.state = state;
|
||||
}
|
||||
|
||||
public VehicleMotionState Capture()
|
||||
{
|
||||
return state;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,88 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace EMPlannerVerificationHost;
|
||||
|
||||
internal sealed class FakeQpSolver : IQpSolver
|
||||
{
|
||||
private readonly Queue<QpSolveResult> _results;
|
||||
private readonly List<QuadraticProgram> _problems = new List<QuadraticProgram>();
|
||||
private readonly List<IReadOnlyList<double>> _warmStarts = new List<IReadOnlyList<double>>();
|
||||
|
||||
public FakeQpSolver(QpSolveResult result)
|
||||
: this(new[] { result })
|
||||
{
|
||||
}
|
||||
|
||||
public FakeQpSolver(IEnumerable<QpSolveResult> results)
|
||||
{
|
||||
if (results == null)
|
||||
throw new ArgumentNullException(nameof(results));
|
||||
_results = new Queue<QpSolveResult>();
|
||||
foreach (QpSolveResult result in results)
|
||||
_results.Enqueue(result ?? throw new ArgumentException("Fake solver results cannot contain null values.", nameof(results)));
|
||||
if (_results.Count == 0)
|
||||
throw new ArgumentException("At least one fake solver result is required.", nameof(results));
|
||||
LastWarmStart = Array.Empty<double>();
|
||||
}
|
||||
|
||||
public QuadraticProgram? LastProblem { get; private set; }
|
||||
|
||||
public QpSolverSettings? LastSettings { get; private set; }
|
||||
|
||||
public IReadOnlyList<double> LastWarmStart { get; private set; }
|
||||
|
||||
public IReadOnlyList<QuadraticProgram> Problems => new ReadOnlyCollection<QuadraticProgram>(_problems);
|
||||
|
||||
public IReadOnlyList<IReadOnlyList<double>> WarmStarts => new ReadOnlyCollection<IReadOnlyList<double>>(_warmStarts);
|
||||
|
||||
public int SolveCallCount => _problems.Count;
|
||||
|
||||
public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
LastProblem = problem ?? throw new ArgumentNullException(nameof(problem));
|
||||
LastSettings = settings ?? throw new ArgumentNullException(nameof(settings));
|
||||
var copy = new List<double>(warmStart == null ? 0 : warmStart.Count);
|
||||
if (warmStart != null)
|
||||
{
|
||||
for (int index = 0; index < warmStart.Count; index++)
|
||||
copy.Add(warmStart[index]);
|
||||
}
|
||||
LastWarmStart = new ReadOnlyCollection<double>(copy);
|
||||
_problems.Add(LastProblem);
|
||||
_warmStarts.Add(LastWarmStart);
|
||||
if (_results.Count == 0)
|
||||
throw new InvalidOperationException("Fake solver was called more often than its scripted result sequence.");
|
||||
return _results.Dequeue();
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class CancellingQpSolver : IQpSolver
|
||||
{
|
||||
private readonly IQpSolver inner;
|
||||
private readonly CancellationTokenSource cancellation;
|
||||
private readonly int cancelAfterSolveCount;
|
||||
private int solveCount;
|
||||
|
||||
public CancellingQpSolver(IQpSolver inner, CancellationTokenSource cancellation, int cancelAfterSolveCount = 1)
|
||||
{
|
||||
this.inner = inner ?? throw new ArgumentNullException(nameof(inner));
|
||||
this.cancellation = cancellation ?? throw new ArgumentNullException(nameof(cancellation));
|
||||
if (cancelAfterSolveCount <= 0) throw new ArgumentOutOfRangeException(nameof(cancelAfterSolveCount));
|
||||
this.cancelAfterSolveCount = cancelAfterSolveCount;
|
||||
}
|
||||
|
||||
public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
QpSolveResult result = inner.Solve(problem, settings, warmStart, cancellationToken);
|
||||
solveCount++;
|
||||
if (solveCount == cancelAfterSolveCount)
|
||||
cancellation.Cancel();
|
||||
return result;
|
||||
}
|
||||
}
|
||||
@@ -1,345 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class FoundationChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifyContracts();
|
||||
VerifyConfigurationAndRequestValidation();
|
||||
VerifyFullDirectionScopeContractsConfigurationAndValidation();
|
||||
}
|
||||
|
||||
private static void VerifyContracts()
|
||||
{
|
||||
var reverseState = new VehicleMotionState(
|
||||
new Pose2D(1.5d, -2d, 0.25d),
|
||||
-0.15d,
|
||||
-0.03d,
|
||||
new DateTimeOffset(2026, 8, 3, 0, 0, 0, TimeSpan.Zero),
|
||||
17L);
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(-0.15d, reverseState.SignedLongitudinalSpeedMetersPerSecond,
|
||||
"reverse signed speed");
|
||||
|
||||
var point = new EmTrajectoryPoint(
|
||||
1.25d,
|
||||
-0.75d,
|
||||
0.5d,
|
||||
-0.12d,
|
||||
0.4d,
|
||||
-0.2d,
|
||||
3,
|
||||
0.6d,
|
||||
0.8d,
|
||||
TravelDirection.Reverse,
|
||||
EmBoundaryType.GearSwitchApproach,
|
||||
-0.04d,
|
||||
0.03d);
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1.25d, point.X, "point x");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(-0.75d, point.Y, "point y");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, point.Yaw, "point yaw");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d, point.SignedLongitudinalVelocity, "point signed speed");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.4d, point.TimeFromStart, "point time");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(-0.2d, point.VehicleCurvature, "point curvature");
|
||||
EMPlannerVerificationHost.Verification.Equal(3, point.SegmentIndex, "point segment index");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.6d, point.SegmentLocalS, "point segment local s");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.8d, point.PathS, "point path s");
|
||||
EMPlannerVerificationHost.Verification.Equal(TravelDirection.Reverse, point.Direction, "point direction");
|
||||
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach, point.BoundaryType, "point boundary type");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.12d, point.Speed, "point derived speed");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Cos(0.5d), point.VelocityX, "point derived velocity x");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Sin(0.5d), point.VelocityY, "point derived velocity y");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual((-0.12d) * (-0.2d), point.YawRate, "point derived yaw rate");
|
||||
}
|
||||
|
||||
private static void VerifyConfigurationAndRequestValidation()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.ReplanPeriodSeconds, "replan period");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(6d, configuration.Scheduling.TimeHorizonSeconds, "time horizon");
|
||||
EMPlannerVerificationHost.Verification.True(configuration.Scheduling.DistanceHorizonMeters > 0d,
|
||||
"distance horizon remains positive");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Scheduling.OutputTimeStepSeconds, "output time step");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Scheduling.SolverTimeoutSeconds, "solver timeout");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Scheduling.HandoffLookaheadSeconds, "handoff lookahead");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.MaximumVehicleStateAgeSeconds, "vehicle state age");
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Corridor.LongitudinalSampleSpacingMeters, "longitudinal sample spacing");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.LateralSampleSpacingMeters, "lateral sample spacing");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Corridor.MaximumLateralOffsetMeters, "maximum lateral offset");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.02d, configuration.Corridor.AdditionalClearanceReserveMeters, "clearance reserve");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.MaximumCollisionCheckStepMeters, "collision check step");
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Frenet.MaximumProjectionDistanceMeters, "maximum projection distance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Frenet.MinimumFrenetDenominator, "minimum Frenet denominator");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1e-8d, configuration.Frenet.BoundaryAnchorToleranceMeters, "boundary anchor tolerance");
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Lateral.MaximumLateralStepPerIterationMeters, "lateral trust region");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Lateral.MaximumLateralSlope, "maximum lateral slope");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, configuration.Lateral.MaximumLateralSecondDerivativePerMeter, "maximum lateral second derivative");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(2d, configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter, "maximum lateral third derivative");
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond, "maximum forward speed");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond, "maximum reverse speed");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared, "maximum acceleration");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared, "maximum deceleration");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed, "maximum jerk");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared, "maximum lateral acceleration");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond, "maximum curvature rate");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.01d, configuration.Longitudinal.StopSpeedToleranceMetersPerSecond, "stop speed tolerance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.ZeroSpeedHoldSeconds, "zero speed hold");
|
||||
|
||||
EMPlannerVerificationHost.Verification.Equal(5, configuration.Solver.MaximumOuterIterations, "maximum outer iterations");
|
||||
EMPlannerVerificationHost.Verification.Equal(4000, configuration.Solver.MaximumOsqpIterations, "maximum OSQP iterations");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.AbsoluteTolerance, "absolute tolerance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.RelativeTolerance, "relative tolerance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.StrictResidualTolerance, "strict residual tolerance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Validation.KinematicTolerance,
|
||||
"kinematic tolerance");
|
||||
EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.WarmStart, "warm start");
|
||||
EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.Polish, "polish");
|
||||
EMPlannerVerificationHost.Verification.Equal(false, configuration.Solver.NativeVerbose, "native verbose");
|
||||
|
||||
VerifyLateralWeights(configuration.Lateral.Weights);
|
||||
VerifyLongitudinalWeights(configuration.Longitudinal.Weights);
|
||||
|
||||
EmPlannerConfiguration insufficientPreview = EmPlannerConfiguration.CreateDefault();
|
||||
insufficientPreview.Scheduling.DistanceHorizonMeters = 0.01d;
|
||||
EmPlanningRequestValidationResult insufficientPreviewValidation = EmPlanningRequestValidator.Validate(
|
||||
CreateValidRequest(insufficientPreview));
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, insufficientPreviewValidation, "insufficient stopping preview");
|
||||
EMPlannerVerificationHost.Verification.True(
|
||||
insufficientPreviewValidation.FailureReason.Contains("DistanceHorizonMeters") &&
|
||||
insufficientPreviewValidation.FailureReason.Contains("required") &&
|
||||
insufficientPreviewValidation.FailureReason.Contains("configured"),
|
||||
"insufficient stopping preview describes configured and required distance");
|
||||
|
||||
EmPlanningRequest valid = CreateValidRequest(configuration);
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(null!, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null reference path");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, null!, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null map");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, null!, 0, EmMotionModel.NonholonomicForwardReverse)), "null configuration");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, CreateMap(false), valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "map not ready");
|
||||
AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(CreateFailedReferencePath(), valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "smoothing failure");
|
||||
AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse)), "segment index");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(double.NaN, 9L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "non-finite speed");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, -1L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "negative sequence ID");
|
||||
AssertStatus(EmPlanningStatus.StaleVehicleState, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, 9L, valid.RequestedAtUtc.AddSeconds(-1d)), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "stale state");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d }, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "missing curvature limit");
|
||||
AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.CrabTranslation)), "crab motion");
|
||||
AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.InPlaceRotation)), "in-place rotation");
|
||||
|
||||
EmPlanningRequestValidationResult first = EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse));
|
||||
EmPlanningRequestValidationResult second = EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse));
|
||||
EMPlannerVerificationHost.Verification.Equal(first.FailureReason, second.FailureReason, "deterministic rejection message");
|
||||
}
|
||||
|
||||
private static void VerifyFullDirectionScopeContractsConfigurationAndValidation()
|
||||
{
|
||||
EmPlannerConfiguration defaults = EmPlannerConfiguration.CreateDefault();
|
||||
EmPlanningRequest fullRequest = CreateValidRequest(defaults, EmPlanningScope.FullDirectionSegment);
|
||||
EMPlannerVerificationHost.Verification.Equal(EmPlanningScope.FullDirectionSegment, fullRequest.PlanningScope,
|
||||
"full request freezes its scope");
|
||||
var fullMetadata = new EmTrajectoryMetadata("full-scope", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch,
|
||||
1L, "reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal,
|
||||
EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.FullDirectionSegment);
|
||||
EMPlannerVerificationHost.Verification.Equal(EmPlanningScope.FullDirectionSegment, fullMetadata.PlanningScope,
|
||||
"metadata freezes its scope");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d,
|
||||
defaults.Longitudinal.DesiredForwardSpeedMetersPerSecond, "forward desired speed");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.5d,
|
||||
defaults.Longitudinal.DesiredReverseSpeedMetersPerSecond, "reverse desired speed");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d,
|
||||
defaults.Longitudinal.MaximumForwardSpeedMetersPerSecond, "forward hard limit");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.5d,
|
||||
defaults.Longitudinal.MaximumReverseSpeedMetersPerSecond, "reverse hard limit");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d,
|
||||
defaults.Scheduling.MaximumOptimizationTimeStepSeconds, "adaptive maximum dt");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.10d,
|
||||
defaults.Scheduling.MaximumOptimizationSpatialStepMeters, "adaptive maximum ds");
|
||||
EMPlannerVerificationHost.Verification.Equal(401, defaults.Scheduling.MaximumOptimizationKnotCount,
|
||||
"optimization knot cap");
|
||||
EMPlannerVerificationHost.Verification.Equal(5001, defaults.Scheduling.MaximumPublishedSampleCount,
|
||||
"publication sample cap");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0d,
|
||||
defaults.Validation.TerminalPositionToleranceMeters, "terminal position tolerance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0d,
|
||||
defaults.Validation.TerminalYawToleranceRadians, "terminal yaw tolerance");
|
||||
EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.NoProgress, EmPlanningStatus.NoProgress,
|
||||
"no-progress status exists");
|
||||
EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.TerminalPoseMismatch,
|
||||
EmPlanningStatus.TerminalPoseMismatch, "terminal-pose status exists");
|
||||
EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded,
|
||||
EmPlanningStatus.FullSegmentResourceLimitExceeded, "resource-limit status exists");
|
||||
|
||||
EmPlannerConfiguration copied = (EmPlannerConfiguration)typeof(EmPlannerConfiguration)
|
||||
.GetMethod("Copy", System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic)!
|
||||
.Invoke(defaults, null)!;
|
||||
defaults.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.25d;
|
||||
defaults.Scheduling.MaximumOptimizationKnotCount = 3;
|
||||
defaults.Validation.TerminalPositionToleranceMeters = 0.01d;
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d,
|
||||
copied.Longitudinal.DesiredForwardSpeedMetersPerSecond, "copy keeps desired forward speed");
|
||||
EMPlannerVerificationHost.Verification.Equal(401, copied.Scheduling.MaximumOptimizationKnotCount,
|
||||
"copy keeps knot cap");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0d,
|
||||
copied.Validation.TerminalPositionToleranceMeters, "copy keeps terminal tolerance");
|
||||
|
||||
EmPlannerConfiguration invalidKnotCap = EmPlannerConfiguration.CreateDefault();
|
||||
invalidKnotCap.Scheduling.MaximumOptimizationKnotCount = 2;
|
||||
AssertStatus(EmPlanningStatus.InvalidInput,
|
||||
EmPlanningRequestValidator.Validate(CreateValidRequest(invalidKnotCap, EmPlanningScope.FullDirectionSegment)),
|
||||
"invalid optimization knot cap");
|
||||
EmPlannerConfiguration invalidDesiredSpeed = EmPlannerConfiguration.CreateDefault();
|
||||
invalidDesiredSpeed.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1.01d;
|
||||
AssertStatus(EmPlanningStatus.InvalidInput,
|
||||
EmPlanningRequestValidator.Validate(CreateValidRequest(invalidDesiredSpeed, EmPlanningScope.FullDirectionSegment)),
|
||||
"desired speed above hard limit");
|
||||
EmPlannerConfiguration invalidYawTolerance = EmPlannerConfiguration.CreateDefault();
|
||||
invalidYawTolerance.Validation.TerminalYawToleranceRadians = Math.PI + 0.01d;
|
||||
AssertStatus(EmPlanningStatus.InvalidInput,
|
||||
EmPlanningRequestValidator.Validate(CreateValidRequest(invalidYawTolerance, EmPlanningScope.FullDirectionSegment)),
|
||||
"terminal yaw tolerance above pi");
|
||||
|
||||
EmPlannerConfiguration shortLegacyWindow = EmPlannerConfiguration.CreateDefault();
|
||||
shortLegacyWindow.Scheduling.DistanceHorizonMeters = 0.01d;
|
||||
AssertStatus(EmPlanningStatus.Success,
|
||||
EmPlanningRequestValidator.Validate(CreateValidRequest(shortLegacyWindow, EmPlanningScope.FullDirectionSegment)),
|
||||
"full scope ignores legacy stopping-preview truncation");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput,
|
||||
EmPlanningRequestValidator.Validate(CreateValidRequest(shortLegacyWindow, EmPlanningScope.RollingHorizon)),
|
||||
"rolling scope retains legacy stopping-preview validation");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput,
|
||||
EmPlanningRequestValidator.Validate(CreateValidRequest(EmPlannerConfiguration.CreateDefault(),
|
||||
(EmPlanningScope)99)), "undefined planning scope");
|
||||
}
|
||||
|
||||
private static void VerifyLateralWeights(LateralWeights weights)
|
||||
{
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceOffset, "lateral reference weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.HeadingDeviation, "lateral heading weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.SecondDerivative, "lateral second derivative weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ThirdDerivative, "lateral third derivative weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.Curvature, "lateral curvature weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(20d, weights.CurvatureVariation, "lateral curvature variation weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "lateral previous weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.RollingTerminal, "lateral rolling terminal weight");
|
||||
}
|
||||
|
||||
private static void VerifyLongitudinalWeights(LongitudinalWeights weights)
|
||||
{
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceSpeed, "longitudinal speed weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.Acceleration, "longitudinal acceleration weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.Jerk, "longitudinal jerk weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "longitudinal previous weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.TerminalAcceleration, "longitudinal terminal acceleration weight");
|
||||
}
|
||||
|
||||
private static void AssertStatus(EmPlanningStatus expected, EmPlanningRequestValidationResult actual, string name)
|
||||
{
|
||||
EMPlannerVerificationHost.Verification.Equal(expected, actual.Status, name);
|
||||
}
|
||||
|
||||
private static EmPlanningRequest CreateValidRequest(EmPlannerConfiguration configuration,
|
||||
EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
|
||||
{
|
||||
DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
|
||||
return CreateRequest(
|
||||
CreateValidReferencePath(),
|
||||
CreateMap(true),
|
||||
new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d, SafetyMarginMeters = 0.05d, MaximumCurvaturePerMeter = 0.5d },
|
||||
CreateState(0d, 9L, requestedAtUtc),
|
||||
configuration,
|
||||
0,
|
||||
EmMotionModel.NonholonomicForwardReverse,
|
||||
planningScope);
|
||||
}
|
||||
|
||||
private static EmPlanningRequest CreateRequest(
|
||||
PathSmoothingResult referencePath,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
VehicleMotionState vehicleState,
|
||||
EmPlannerConfiguration configuration,
|
||||
int segmentIndex,
|
||||
EmMotionModel motionModel,
|
||||
EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
|
||||
{
|
||||
DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
|
||||
return new EmPlanningRequest(
|
||||
referencePath,
|
||||
map,
|
||||
vehicle,
|
||||
vehicleState,
|
||||
configuration,
|
||||
segmentIndex,
|
||||
null!,
|
||||
requestedAtUtc,
|
||||
requestedAtUtc.AddSeconds(0.1d),
|
||||
"trajectory-1",
|
||||
"reference-1",
|
||||
string.Empty,
|
||||
motionModel,
|
||||
planningScope);
|
||||
}
|
||||
|
||||
private static VehicleMotionState CreateState(double speed, long sequenceId, DateTimeOffset capturedAtUtc)
|
||||
{
|
||||
return new VehicleMotionState(new Pose2D(0d, 0d, 0d), speed, null, capturedAtUtc, sequenceId);
|
||||
}
|
||||
|
||||
private static PlanningGridMap CreateMap(bool planningReady)
|
||||
{
|
||||
var result = new PlanningMapFactory().Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(-2000f, 2000f, -2000f, 2000f),
|
||||
ResolutionMm = 100f,
|
||||
ObstacleSources = new List<IMapObstacleSource>(),
|
||||
AllowExplicitEmptyMap = planningReady,
|
||||
});
|
||||
if (!result.Succeeded || result.Map == null)
|
||||
throw new InvalidOperationException("Unable to create test planning map.");
|
||||
return result.Map;
|
||||
}
|
||||
|
||||
private static PathSmoothingResult CreateValidReferencePath()
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
new SmoothedPathPoint(1d, 0d, 0d, 0d, 1d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
var segments = new List<SmoothedPathSegment>
|
||||
{
|
||||
new SmoothedPathSegment(0, TravelDirection.Forward, 0, 1, false, false),
|
||||
};
|
||||
var metrics = new PathQualityMetrics(true, 1d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
|
||||
return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
|
||||
new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
|
||||
}
|
||||
|
||||
private static PathSmoothingResult CreateFailedReferencePath()
|
||||
{
|
||||
return PathSmoothingResult.Failure(PathSmoothingStatus.Failed, new PathSmoothingDiagnostics());
|
||||
}
|
||||
}
|
||||
@@ -1,136 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class FrenetChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesForwardProjectionAndReconstruction();
|
||||
VerifiesReverseProjectionAcrossYawWrap();
|
||||
VerifiesBoundedProjectionOnALoop();
|
||||
}
|
||||
|
||||
private static void VerifiesForwardProjectionAndReconstruction()
|
||||
{
|
||||
DirectionSegmentView segment = CreateStraightSegment(TravelDirection.Forward, 0d, 0d);
|
||||
var world = new Pose2D(1.5d, 0.2d, 0d);
|
||||
var projector = new FrenetProjector();
|
||||
|
||||
Verification.True(projector.TryProject(world, segment, 0d, 4d, 0.5d, out FrenetProjection projection),
|
||||
"forward projection succeeds");
|
||||
Verification.NearlyEqual(1.5d, projection.ReferenceS, "forward reference s");
|
||||
Verification.NearlyEqual(0.2d, projection.LateralOffset, "forward positive l is travel-left");
|
||||
Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d,
|
||||
out Pose2D reconstructed), "forward reconstruction succeeds");
|
||||
Verification.NearlyEqual(world.X, reconstructed.X, "forward reconstructed x");
|
||||
Verification.NearlyEqual(world.Y, reconstructed.Y, "forward reconstructed y");
|
||||
Verification.NearlyEqual(0d, reconstructed.Heading, "forward reconstructed vehicle yaw");
|
||||
}
|
||||
|
||||
private static void VerifiesReverseProjectionAcrossYawWrap()
|
||||
{
|
||||
const double vehicleYaw = -Math.PI + 0.01d;
|
||||
const double travelYaw = 0.01d;
|
||||
DirectionSegmentView segment = CreateStraightSegment(TravelDirection.Reverse, vehicleYaw, travelYaw);
|
||||
double expectedS = 1.5d;
|
||||
double expectedL = 0.2d;
|
||||
var world = new Pose2D(
|
||||
expectedS * Math.Cos(travelYaw) - expectedL * Math.Sin(travelYaw),
|
||||
expectedS * Math.Sin(travelYaw) + expectedL * Math.Cos(travelYaw),
|
||||
vehicleYaw);
|
||||
var projector = new FrenetProjector();
|
||||
|
||||
Verification.True(projector.TryProject(world, segment, 0d, 4d, 0.5d, out FrenetProjection projection),
|
||||
"reverse projection succeeds");
|
||||
Verification.NearlyEqual(expectedS, projection.ReferenceS, "reverse reference s");
|
||||
Verification.NearlyEqual(expectedL, projection.LateralOffset, "reverse positive l is travel-left and body-right");
|
||||
Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d,
|
||||
out Pose2D reconstructed), "reverse reconstruction succeeds");
|
||||
Verification.NearlyEqual(world.X, reconstructed.X, "reverse reconstructed x");
|
||||
Verification.NearlyEqual(world.Y, reconstructed.Y, "reverse reconstructed y");
|
||||
Verification.NearlyEqual(AngleMath.NormalizeRadians(vehicleYaw), reconstructed.Heading,
|
||||
"reverse reconstructed vehicle yaw");
|
||||
|
||||
DirectionSegmentView wrapped = CreateWrappedForwardSegment();
|
||||
FrenetReferencePoint wrappedPoint = ReferencePathInterpolator.Interpolate(wrapped, 1.5d);
|
||||
Verification.NearlyEqual(3.18d, wrappedPoint.UnwrappedVehicleYaw, "interpolated yaw remains unwrapped internally");
|
||||
Verification.NearlyEqual(AngleMath.NormalizeRadians(3.18d), wrappedPoint.VehicleYaw,
|
||||
"interpolated public yaw is normalized");
|
||||
Verification.True(!FrenetTransform.TryReconstruct(new FrenetReferencePoint(0d, 0d, 0d, 0d, 0d,
|
||||
TravelDirection.Forward, 1d, 0d, 0d, 0d), 1d, 0d, 0.2d, out _),
|
||||
"singular Frenet reconstruction is rejected");
|
||||
}
|
||||
|
||||
private static void VerifiesBoundedProjectionOnALoop()
|
||||
{
|
||||
DirectionSegmentView segment = CreateLoopSegment();
|
||||
var projector = new FrenetProjector();
|
||||
var world = new Pose2D(1d, 0.2d, Math.PI);
|
||||
|
||||
Verification.True(projector.TryProject(world, segment, 2.2d, 4.2d, 0.1d, out FrenetProjection projection),
|
||||
"bounded loop projection succeeds");
|
||||
Verification.NearlyEqual(3.2d, projection.ReferenceS, "bounded loop picks the local upper branch");
|
||||
Verification.True(projection.ReferenceS >= 2.2d && projection.ReferenceS <= 4.2d,
|
||||
"projection remains in supplied reference-s interval");
|
||||
Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d,
|
||||
out Pose2D reconstructed), "loop reconstruction succeeds");
|
||||
Verification.NearlyEqual(world.X, reconstructed.X, "loop reconstructed x");
|
||||
Verification.NearlyEqual(world.Y, reconstructed.Y, "loop reconstructed y");
|
||||
}
|
||||
|
||||
private static DirectionSegmentView CreateStraightSegment(TravelDirection direction, double vehicleYaw,
|
||||
double travelYaw)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d, AngleMath.NormalizeRadians(vehicleYaw), vehicleYaw, 0d, direction),
|
||||
Point(4d * Math.Cos(travelYaw), 4d * Math.Sin(travelYaw), AngleMath.NormalizeRadians(vehicleYaw),
|
||||
vehicleYaw, 4d, direction),
|
||||
};
|
||||
return CreateSegment(direction, points);
|
||||
}
|
||||
|
||||
private static DirectionSegmentView CreateWrappedForwardSegment()
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d, AngleMath.NormalizeRadians(3.12d), 3.12d, 0d, TravelDirection.Forward),
|
||||
Point(1d, 0d, AngleMath.NormalizeRadians(3.16d), 3.16d, 1d, TravelDirection.Forward),
|
||||
Point(2d, 0d, AngleMath.NormalizeRadians(3.20d), 3.20d, 2d, TravelDirection.Forward),
|
||||
};
|
||||
return CreateSegment(TravelDirection.Forward, points);
|
||||
}
|
||||
|
||||
private static DirectionSegmentView CreateLoopSegment()
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward),
|
||||
Point(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward),
|
||||
Point(2d, 0.2d, Math.PI / 2d, Math.PI / 2d, 2.2d, TravelDirection.Forward),
|
||||
Point(0d, 0.2d, Math.PI, Math.PI, 4.2d, TravelDirection.Forward),
|
||||
};
|
||||
return CreateSegment(TravelDirection.Forward, points);
|
||||
}
|
||||
|
||||
private static DirectionSegmentView CreateSegment(TravelDirection direction, IReadOnlyList<SmoothedPathPoint> points)
|
||||
{
|
||||
double length = points[points.Count - 1].ArcLength;
|
||||
return new DirectionSegmentView(0, direction, points,
|
||||
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
||||
new ReferenceBoundary(0, length, EmBoundaryType.Goal, length), 0d);
|
||||
}
|
||||
|
||||
private static SmoothedPathPoint Point(double x, double y, double heading, double unwrappedHeading, double s,
|
||||
TravelDirection direction)
|
||||
{
|
||||
return new SmoothedPathPoint(x, y, heading, unwrappedHeading, s, direction, 0d, 0d, 0d, 1d,
|
||||
false, SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
}
|
||||
@@ -1,472 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.IO;
|
||||
using System.Threading;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class LateralIntegrationChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesValidatedCandidateSurvivesLaterTimeout();
|
||||
VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks();
|
||||
VerifiesCancellationAfterStrictCandidateNeverPublishesFallback();
|
||||
VerifiesRejectedCandidateAdvancesTheNextLateralLinearization();
|
||||
VerifiesTrustRegionWarmStartAndOuterIterationLimit();
|
||||
VerifiesCancellationAndTimeoutWithoutCandidate();
|
||||
VerifiesLateralPlannerDelegatesToTheSequentialOptimizer();
|
||||
}
|
||||
|
||||
public static void RunRealOsqp()
|
||||
{
|
||||
foreach (LateralScenario scenario in CreateRealOsqpScenarios())
|
||||
{
|
||||
LateralPlanningResult first = new LateralPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
|
||||
CancellationToken.None);
|
||||
LateralPlanningResult second = new LateralPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
|
||||
CancellationToken.None);
|
||||
VerifyRealScenarioResult(scenario, first);
|
||||
VerifyRealScenarioResult(scenario, second);
|
||||
VerifyDeterministicResult(scenario.Name, first, second);
|
||||
if (scenario.RequiresSeedConnectedInterval)
|
||||
{
|
||||
for (int index = 0; index < first.Path.Points.Count; index++)
|
||||
Verification.True(first.Path.Points[index].L <= -0.05d + 1e-10d,
|
||||
scenario.Name + " remains in the seed-connected obstacle corridor");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static void RunRealOsqpInCleanPluginBundle()
|
||||
{
|
||||
string pluginDirectory = Path.Combine(Path.GetTempPath(), "em-planner-lateral-real-" + Guid.NewGuid().ToString("N"));
|
||||
try
|
||||
{
|
||||
Directory.CreateDirectory(pluginDirectory);
|
||||
foreach (string sourcePath in Directory.GetFiles(AppContext.BaseDirectory))
|
||||
{
|
||||
if (string.Equals(Path.GetFileName(sourcePath), "osqp.dll", StringComparison.OrdinalIgnoreCase))
|
||||
continue;
|
||||
File.Copy(sourcePath, Path.Combine(pluginDirectory, Path.GetFileName(sourcePath)), false);
|
||||
}
|
||||
string nativeSource = Path.GetFullPath(Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ThirdParty",
|
||||
"OSQP", "win-x64", "osqp.dll"));
|
||||
Verification.True(File.Exists(nativeSource), "pinned OSQP DLL is available for the real lateral bundle");
|
||||
File.Copy(nativeSource, Path.Combine(pluginDirectory, "osqp.dll"), false);
|
||||
|
||||
var startInfo = new ProcessStartInfo
|
||||
{
|
||||
FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
|
||||
Arguments = "lateral-real-osqp-probe",
|
||||
WorkingDirectory = pluginDirectory,
|
||||
UseShellExecute = false,
|
||||
CreateNoWindow = true,
|
||||
RedirectStandardOutput = true,
|
||||
RedirectStandardError = true,
|
||||
};
|
||||
using (var process = new Process { StartInfo = startInfo })
|
||||
{
|
||||
process.Start();
|
||||
string standardOutput = process.StandardOutput.ReadToEnd();
|
||||
string standardError = process.StandardError.ReadToEnd();
|
||||
process.WaitForExit();
|
||||
if (process.ExitCode != 0 || standardOutput.IndexOf("PASS lateral-real-osqp", StringComparison.Ordinal) < 0)
|
||||
{
|
||||
throw new InvalidOperationException("Real lateral OSQP clean-plugin probe exited " + process.ExitCode + ": " +
|
||||
standardError + standardOutput);
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (Directory.Exists(pluginDirectory))
|
||||
Directory.Delete(pluginDirectory, true);
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifiesValidatedCandidateSurvivesLaterTimeout()
|
||||
{
|
||||
LateralPlanningInput input = CreateInput();
|
||||
double[] valid = CreatePrimal(input, 0.02d);
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, valid, 10d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
||||
});
|
||||
|
||||
LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
|
||||
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"timeout after an independently validated candidate returns fallback success");
|
||||
LateralPath fallbackPath = result.Path ?? throw new InvalidOperationException("Fallback path was not returned.");
|
||||
Verification.True(fallbackPath.IsIndependentlyValidated,
|
||||
"fallback path remains independently validated");
|
||||
Verification.NearlyEqual(0.02d, fallbackPath.Points[1].L,
|
||||
"first valid candidate remains the fallback path");
|
||||
}
|
||||
|
||||
private static void VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks()
|
||||
{
|
||||
LateralPlanningInput input = CreateInput();
|
||||
double[] valid = CreatePrimal(input, 0.02d);
|
||||
double[] invalid = CreatePrimal(input, 0.40d);
|
||||
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, valid, 10d),
|
||||
Result(QpSolveStatus.Solved, invalid, 9d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 9d),
|
||||
});
|
||||
LateralPlanningResult preserved = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, preserved.Status,
|
||||
"invalid solved vector does not discard an earlier fallback");
|
||||
Verification.NearlyEqual(0.02d, preserved.Path.Points[1].L,
|
||||
"invalid solved vector does not replace the fallback candidate");
|
||||
|
||||
var inaccurateResidual = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.SolvedInaccurate, valid, 10d, 2e-5d, 0d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
||||
});
|
||||
LateralPlanningResult rejectedResidual = new SequentialConvexOptimizer(inaccurateResidual).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SolverTimedOut, rejectedResidual.Status,
|
||||
"SolvedInaccurate above strict residual threshold is rejected");
|
||||
Verification.True(ReferenceEquals(null, rejectedResidual.Path),
|
||||
"rejected inaccurate result does not publish a path");
|
||||
|
||||
var inaccurateGeometry = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.SolvedInaccurate, invalid, 10d, 0d, 0d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
||||
});
|
||||
LateralPlanningResult rejectedGeometry = new SequentialConvexOptimizer(inaccurateGeometry).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SolverTimedOut, rejectedGeometry.Status,
|
||||
"SolvedInaccurate still requires full independent lateral validation");
|
||||
}
|
||||
|
||||
private static void VerifiesCancellationAfterStrictCandidateNeverPublishesFallback()
|
||||
{
|
||||
LateralPlanningInput input = CreateInput();
|
||||
double[] valid = CreatePrimal(input, 0.02d);
|
||||
using var cancellation = new CancellationTokenSource();
|
||||
var solver = new CancellingQpSolver(
|
||||
new FakeQpSolver(Result(QpSolveStatus.Solved, valid, 10d)), cancellation);
|
||||
|
||||
LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, cancellation.Token);
|
||||
|
||||
Verification.Equal(EmPlanningStatus.Cancelled, result.Status,
|
||||
"cancellation after a strict LS candidate is never fallback success");
|
||||
Verification.True(result.Path == null, "cancelled LS result exposes no path");
|
||||
}
|
||||
|
||||
private static void VerifiesRejectedCandidateAdvancesTheNextLateralLinearization()
|
||||
{
|
||||
LateralPlanningInput input = CreateInput(0.10d);
|
||||
double[] strict = CreatePrimal(input, 0.01d);
|
||||
double[] rejected = CreatePrimal(input, 0.02d);
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, strict, 10d),
|
||||
Result(QpSolveStatus.Solved, rejected, 9d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 9d),
|
||||
});
|
||||
|
||||
LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
|
||||
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"later timeout preserves the earlier strict candidate");
|
||||
var layout = new LateralVariableLayout(input.ReferenceStations.Count);
|
||||
Verification.NearlyEqual(0.02d, solver.WarmStarts[2][layout.L(1)],
|
||||
"the next LS QP warm-starts from the rejected but parseable candidate");
|
||||
FindSingleVariableBounds(solver.Problems[2], layout.L(1), out double lower, out double upper);
|
||||
Verification.NearlyEqual(-0.03d, lower,
|
||||
"the next LS trust region is centered on the rejected candidate");
|
||||
Verification.NearlyEqual(0.07d, upper,
|
||||
"the next LS trust region is centered on the rejected candidate");
|
||||
}
|
||||
|
||||
private static void VerifiesTrustRegionWarmStartAndOuterIterationLimit()
|
||||
{
|
||||
LateralPlanningInput input = CreateInput();
|
||||
var trustSolver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, CreatePrimal(input, 0.02d), 10d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
||||
});
|
||||
new SequentialConvexOptimizer(trustSolver).Optimize(input, CancellationToken.None);
|
||||
|
||||
var layout = new LateralVariableLayout(input.ReferenceStations.Count);
|
||||
FindSingleVariableBounds(trustSolver.Problems[0], layout.L(1), out double initialLower, out double initialUpper);
|
||||
FindSingleVariableBounds(trustSolver.Problems[1], layout.L(1), out double nextLower, out double nextUpper);
|
||||
Verification.NearlyEqual(-0.05d, initialLower, "initial trust-region lower bound");
|
||||
Verification.NearlyEqual(0.05d, initialUpper, "initial trust-region upper bound");
|
||||
Verification.NearlyEqual(-0.03d, nextLower, "trust region is centered on previous iterate");
|
||||
Verification.NearlyEqual(0.07d, nextUpper, "trust region never exceeds 0.05m around previous iterate");
|
||||
Verification.Equal(layout.VariableCount, trustSolver.WarmStarts[1].Count,
|
||||
"next QP receives the complete previous primal warm start");
|
||||
Verification.NearlyEqual(0.02d, trustSolver.WarmStarts[1][layout.L(1)],
|
||||
"warm start retains the prior lateral iterate");
|
||||
|
||||
var limitResults = new List<QpSolveResult>();
|
||||
for (int index = 1; index <= 5; index++)
|
||||
limitResults.Add(Result(QpSolveStatus.Solved, CreatePrimal(input, 0.02d * index), 100d - index));
|
||||
var limitSolver = new FakeQpSolver(limitResults);
|
||||
LateralPlanningResult limited = new SequentialConvexOptimizer(limitSolver).Optimize(input, CancellationToken.None);
|
||||
Verification.Equal(5, limitSolver.SolveCallCount, "outer loop stops after at most five QP calls");
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, limited.Status,
|
||||
"last strict candidate is reported as fallback at outer iteration limit");
|
||||
Verification.True(limited.FailureReason.Length > 0,
|
||||
"outer iteration fallback preserves a non-empty diagnostic");
|
||||
}
|
||||
|
||||
private static void VerifiesCancellationAndTimeoutWithoutCandidate()
|
||||
{
|
||||
LateralPlanningInput input = CreateInput();
|
||||
var cancellationSolver = new FakeQpSolver(Result(QpSolveStatus.Solved, CreatePrimal(input, 0d), 1d));
|
||||
using var cancellation = new CancellationTokenSource();
|
||||
cancellation.Cancel();
|
||||
LateralPlanningResult cancelled = new SequentialConvexOptimizer(cancellationSolver).Optimize(input,
|
||||
cancellation.Token);
|
||||
Verification.Equal(EmPlanningStatus.Cancelled, cancelled.Status, "cancellation before a solver call is cancelled");
|
||||
Verification.Equal(0, cancellationSolver.SolveCallCount, "cancelled solve does not invoke the solver");
|
||||
|
||||
var timeoutSolver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
|
||||
LateralPlanningResult timeout = new SequentialConvexOptimizer(timeoutSolver).Optimize(input, CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SolverTimedOut, timeout.Status,
|
||||
"timeout without a feasible candidate is solver timed out");
|
||||
Verification.True(ReferenceEquals(null, timeout.Path), "timeout without candidate does not publish a path");
|
||||
}
|
||||
|
||||
private static void VerifiesLateralPlannerDelegatesToTheSequentialOptimizer()
|
||||
{
|
||||
LateralPlanningInput input = CreateInput();
|
||||
double[] zero = CreatePrimal(input, 0d);
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, zero, 1d),
|
||||
Result(QpSolveStatus.Solved, zero, 1d),
|
||||
});
|
||||
LateralPlanningResult result = new LateralPlanner(solver).Plan(input, CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.Success, result.Status, "lateral planner returns SQP success");
|
||||
}
|
||||
|
||||
private static IReadOnlyList<LateralScenario> CreateRealOsqpScenarios()
|
||||
{
|
||||
return new[]
|
||||
{
|
||||
CreateScenario("straight-empty-forward", TravelDirection.Forward, 0d, EmTerminalType.Goal, -0.3d, 0.3d, 0d, false),
|
||||
CreateScenario("straight-empty-reverse", TravelDirection.Reverse, 0d, EmTerminalType.Goal, -0.3d, 0.3d, 0d, false),
|
||||
CreateScenario("gentle-curve", TravelDirection.Forward, 0.05d, EmTerminalType.Goal, -0.3d, 0.3d, 0d, false),
|
||||
CreateScenario("static-obstacle-narrowing", TravelDirection.Forward, 0d, EmTerminalType.RollingSafetyStop,
|
||||
-0.3d, -0.05d, -0.10d, true),
|
||||
CreateScenario("gear-switch-terminal", TravelDirection.Forward, 0d, EmTerminalType.GearSwitch, -0.3d, 0.3d, 0d, false),
|
||||
CreateScenario("rolling-terminal", TravelDirection.Forward, 0d, EmTerminalType.RollingSafetyStop, -0.3d, 0.3d, 0d, false),
|
||||
};
|
||||
}
|
||||
|
||||
private static LateralScenario CreateScenario(string name, TravelDirection direction, double geometricCurvature,
|
||||
EmTerminalType terminal, double corridorMinimum, double corridorMaximum, double seedL,
|
||||
bool requiresSeedConnectedInterval)
|
||||
{
|
||||
double[] stations = { 0d, 0.5d, 1d, 1.5d, 2d };
|
||||
var points = new List<SmoothedPathPoint>(stations.Length);
|
||||
var intervals = new List<LateralInterval>(stations.Length);
|
||||
var seed = new List<FrenetProjection>(requiresSeedConnectedInterval ? stations.Length : 0);
|
||||
for (int index = 0; index < stations.Length; index++)
|
||||
{
|
||||
double referenceS = stations[index];
|
||||
double travelYaw = geometricCurvature * referenceS;
|
||||
double x = Math.Abs(geometricCurvature) <= 1e-12d ? referenceS : Math.Sin(travelYaw) / geometricCurvature;
|
||||
double y = Math.Abs(geometricCurvature) <= 1e-12d ? 0d : (1d - Math.Cos(travelYaw)) / geometricCurvature;
|
||||
double vehicleYaw = direction == TravelDirection.Forward ? travelYaw : travelYaw - Math.PI;
|
||||
points.Add(new SmoothedPathPoint(x, y, vehicleYaw, vehicleYaw, referenceS, direction, geometricCurvature,
|
||||
direction == TravelDirection.Forward ? geometricCurvature : -geometricCurvature, 0d, 1d, false,
|
||||
SmoothedPathPointSource.Anchor));
|
||||
intervals.Add(new LateralInterval(referenceS, corridorMinimum, corridorMaximum, seedL));
|
||||
}
|
||||
var segment = new DirectionSegmentView(0, direction, points,
|
||||
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
||||
new ReferenceBoundary(0, 2d, terminal == EmTerminalType.GearSwitch ? EmBoundaryType.GearSwitchApproach : EmBoundaryType.Goal,
|
||||
2d), 0d);
|
||||
if (requiresSeedConnectedInterval)
|
||||
{
|
||||
for (int index = 0; index < stations.Length; index++)
|
||||
seed.Add(new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, stations[index]), seedL, 0d, 0d));
|
||||
}
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
||||
configuration.Validation.SpatialToleranceMeters = 1e-5d;
|
||||
configuration.Validation.KinematicTolerance = 1e-5d;
|
||||
var vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.1d,
|
||||
WidthMeters = 0.1d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 1d,
|
||||
};
|
||||
return new LateralScenario(name, new LateralPlanningInput(segment, new StaticCorridor(intervals),
|
||||
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), seedL, 0d, 0d), terminal, vehicle,
|
||||
configuration, seed), requiresSeedConnectedInterval);
|
||||
}
|
||||
|
||||
private static void VerifyRealScenarioResult(LateralScenario scenario, LateralPlanningResult result)
|
||||
{
|
||||
Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
|
||||
scenario.Name + " is solved or has a documented fallback: " + result.FailureReason);
|
||||
LateralPath path = result.Path ?? throw new InvalidOperationException(scenario.Name + " returned no lateral path.");
|
||||
Verification.True(path.IsIndependentlyValidated, scenario.Name + " path is independently validated");
|
||||
Verification.Equal(scenario.Input.ReferenceStations.Count, path.Points.Count, scenario.Name + " point count");
|
||||
double maximumCurvature = scenario.Input.Vehicle.MaximumCurvaturePerMeter.GetValueOrDefault();
|
||||
Verification.True(maximumCurvature > 0d, scenario.Name + " has a maximum vehicle curvature");
|
||||
for (int index = 0; index < path.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint point = path.Points[index];
|
||||
LateralInterval interval = scenario.Input.Corridor.Stations[index];
|
||||
Verification.True(point.L >= interval.MinimumL - 1e-10d && point.L <= interval.MaximumL + 1e-10d,
|
||||
scenario.Name + " remains in corridor at station " + index);
|
||||
Verification.True(Math.Abs(point.VehicleCurvature) <= maximumCurvature + 1e-10d,
|
||||
scenario.Name + " respects vehicle curvature at station " + index);
|
||||
}
|
||||
Verification.NearlyEqual(scenario.Input.ReferenceStations[scenario.Input.ReferenceStations.Count - 1],
|
||||
path.Points[path.Points.Count - 1].ReferenceS, scenario.Name + " ends at exact ReferenceS anchor");
|
||||
}
|
||||
|
||||
private static void VerifyDeterministicResult(string name, LateralPlanningResult first, LateralPlanningResult second)
|
||||
{
|
||||
Verification.Equal(first.Status, second.Status, name + " deterministic status");
|
||||
LateralPath firstPath = first.Path ?? throw new InvalidOperationException(name + " first path was missing.");
|
||||
LateralPath secondPath = second.Path ?? throw new InvalidOperationException(name + " second path was missing.");
|
||||
Verification.Equal(firstPath.Points.Count, secondPath.Points.Count, name + " deterministic point count");
|
||||
for (int index = 0; index < firstPath.Points.Count; index++)
|
||||
ComparePoint(firstPath.Points[index], secondPath.Points[index], name + " deterministic point " + index);
|
||||
}
|
||||
|
||||
private static void ComparePoint(LateralPathPoint left, LateralPathPoint right, string name)
|
||||
{
|
||||
double[] leftValues =
|
||||
{
|
||||
left.ReferenceS, left.PathS, left.L, left.DL, left.DDL, left.DDDL, left.X, left.Y, left.VehicleYaw,
|
||||
left.GeometricCurvature, left.VehicleCurvature, left.VehicleCurvatureDerivative,
|
||||
};
|
||||
double[] rightValues =
|
||||
{
|
||||
right.ReferenceS, right.PathS, right.L, right.DL, right.DDL, right.DDDL, right.X, right.Y, right.VehicleYaw,
|
||||
right.GeometricCurvature, right.VehicleCurvature, right.VehicleCurvatureDerivative,
|
||||
};
|
||||
for (int index = 0; index < leftValues.Length; index++)
|
||||
Verification.True(Math.Abs(leftValues[index] - rightValues[index]) <= 1e-10d, name + " value " + index);
|
||||
}
|
||||
|
||||
private static LateralPlanningInput CreateInput(double maximumVehicleCurvature = 1d)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d),
|
||||
Point(1d, 1d),
|
||||
Point(2d, 2d),
|
||||
};
|
||||
var segment = new DirectionSegmentView(0, TravelDirection.Forward, points,
|
||||
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
||||
new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
|
||||
var corridor = new StaticCorridor(new[]
|
||||
{
|
||||
new LateralInterval(0d, -0.3d, 0.3d, 0d),
|
||||
new LateralInterval(1d, -0.3d, 0.3d, 0d),
|
||||
new LateralInterval(2d, -0.3d, 0.3d, 0d),
|
||||
});
|
||||
var vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.1d,
|
||||
WidthMeters = 0.1d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = maximumVehicleCurvature,
|
||||
};
|
||||
return new LateralPlanningInput(segment, corridor,
|
||||
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),
|
||||
EmTerminalType.Goal, vehicle, EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>());
|
||||
}
|
||||
|
||||
private static SmoothedPathPoint Point(double x, double pathS)
|
||||
{
|
||||
return new SmoothedPathPoint(x, 0d, 0d, 0d, pathS, TravelDirection.Forward, 0d, 0d, 0d, 1d,
|
||||
false, SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
|
||||
private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal, double objective,
|
||||
double primalResidual = 0d, double dualResidual = 0d)
|
||||
{
|
||||
return new QpSolveResult(status, primal, objective, primalResidual, dualResidual, 1, TimeSpan.Zero,
|
||||
status.ToString(), string.Empty);
|
||||
}
|
||||
|
||||
private static double[] CreatePrimal(LateralPlanningInput input, double middleL)
|
||||
{
|
||||
var layout = new LateralVariableLayout(input.ReferenceStations.Count);
|
||||
double c = 6d * middleL;
|
||||
var primal = new double[layout.VariableCount];
|
||||
primal[layout.L(0)] = 0d;
|
||||
primal[layout.L(1)] = middleL;
|
||||
primal[layout.L(2)] = 0d;
|
||||
primal[layout.DL(0)] = 0d;
|
||||
primal[layout.DL(1)] = 0d;
|
||||
primal[layout.DL(2)] = 0d;
|
||||
primal[layout.DDL(0)] = c;
|
||||
primal[layout.DDL(1)] = -c;
|
||||
primal[layout.DDL(2)] = c;
|
||||
primal[layout.DDDL(0)] = -2d * c;
|
||||
primal[layout.DDDL(1)] = 2d * c;
|
||||
return primal;
|
||||
}
|
||||
|
||||
private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
|
||||
{
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
int matchingEntries = 0;
|
||||
double coefficient = 0d;
|
||||
for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
|
||||
{
|
||||
for (int index = problem.ConstraintMatrix.ColumnPointers[column];
|
||||
index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (problem.ConstraintMatrix.RowIndices[index] == row)
|
||||
{
|
||||
matchingEntries++;
|
||||
if (column == variable)
|
||||
coefficient = problem.ConstraintMatrix.Values[index];
|
||||
}
|
||||
}
|
||||
}
|
||||
if (matchingEntries == 1 && Math.Abs(coefficient - 1d) <= 1e-12d &&
|
||||
Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) > 1e-12d)
|
||||
{
|
||||
lower = problem.LowerBounds[row];
|
||||
upper = problem.UpperBounds[row];
|
||||
return;
|
||||
}
|
||||
}
|
||||
throw new InvalidOperationException("Expected single-variable lateral trust-region row was not found.");
|
||||
}
|
||||
|
||||
private sealed class LateralScenario
|
||||
{
|
||||
public LateralScenario(string name, LateralPlanningInput input, bool requiresSeedConnectedInterval)
|
||||
{
|
||||
Name = name;
|
||||
Input = input;
|
||||
RequiresSeedConnectedInterval = requiresSeedConnectedInterval;
|
||||
}
|
||||
|
||||
public string Name { get; }
|
||||
public LateralPlanningInput Input { get; }
|
||||
public bool RequiresSeedConnectedInterval { get; }
|
||||
}
|
||||
}
|
||||
@@ -1,646 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class LateralModelChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesDeterministicVariableLayout();
|
||||
VerifiesExactDiscreteDynamicsForUnequalStations();
|
||||
VerifiesPlanningInputBoundariesAndDefensiveCopies();
|
||||
VerifiesLateralResultPublicationContract();
|
||||
VerifiesNormalizedObjectiveAndHardConstraints();
|
||||
VerifiesVehicleCurvatureIsAHardQpConstraint();
|
||||
VerifiesAllNamedCostScales();
|
||||
VerifiesEmptyHardBoundIntersectionFailsBeforeSolve();
|
||||
VerifiesFakeSolverCapturesTheNeutralQpBoundary();
|
||||
VerifiesNonlinearGeometryInBothDirections();
|
||||
VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths();
|
||||
}
|
||||
|
||||
private static void VerifiesDeterministicVariableLayout()
|
||||
{
|
||||
LateralVariableLayout layout = CreateLayout(4);
|
||||
|
||||
Verification.Equal(15, layout.VariableCount, "layout variable count");
|
||||
for (int index = 0; index < 4; index++)
|
||||
{
|
||||
Verification.Equal(index, layout.L(index), "l index " + index);
|
||||
Verification.Equal(4 + index, layout.DL(index), "dl index " + index);
|
||||
Verification.Equal(8 + index, layout.DDL(index), "ddl index " + index);
|
||||
}
|
||||
for (int index = 0; index < 3; index++)
|
||||
Verification.Equal(12 + index, layout.DDDL(index), "dddl index " + index);
|
||||
|
||||
ExpectArgumentException(() => CreateLayout(1), "layout rejects fewer than two stations");
|
||||
ExpectArgumentException(() => layout.L(4), "l index bounds check");
|
||||
ExpectArgumentException(() => layout.DL(-1), "dl index bounds check");
|
||||
ExpectArgumentException(() => layout.DDL(4), "ddl index bounds check");
|
||||
ExpectArgumentException(() => layout.DDDL(3), "dddl index bounds check");
|
||||
}
|
||||
|
||||
private static void VerifiesExactDiscreteDynamicsForUnequalStations()
|
||||
{
|
||||
double[] stations = { 0d, 0.4d, 1.25d, 2.5d };
|
||||
double[] jerks = { 0.5d, -0.3d, 0.2d };
|
||||
LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, -0.2d, 0.3d, jerks);
|
||||
|
||||
for (int index = 0; index < jerks.Length; index++)
|
||||
{
|
||||
double ds = stations[index + 1] - stations[index];
|
||||
Verification.NearlyEqual(candidate.DDL[index] + ds * candidate.DDDL[index], candidate.DDL[index + 1],
|
||||
"exact ddl integration " + index);
|
||||
Verification.NearlyEqual(candidate.DL[index] + ds * candidate.DDL[index] + 0.5d * ds * ds * candidate.DDDL[index],
|
||||
candidate.DL[index + 1], "exact dl integration " + index);
|
||||
Verification.NearlyEqual(candidate.L[index] + ds * candidate.DL[index] + 0.5d * ds * ds * candidate.DDL[index] +
|
||||
ds * ds * ds * candidate.DDDL[index] / 6d, candidate.L[index + 1], "exact l integration " + index);
|
||||
}
|
||||
Verification.True(candidate.SatisfiesExactDiscreteDynamics(1e-12d), "integrated candidate validates exact dynamics");
|
||||
|
||||
LateralCandidate inconsistent = new LateralCandidate(new[] { 0d, 1d }, new[] { 0d, 1d },
|
||||
new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d });
|
||||
Verification.True(!inconsistent.SatisfiesExactDiscreteDynamics(1e-12d), "candidate detects inconsistent dynamics");
|
||||
ExpectArgumentException(() => new LateralCandidate(new[] { 0d, 0d }, new[] { 0d, 0d },
|
||||
new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d }), "candidate rejects non-increasing stations");
|
||||
}
|
||||
|
||||
private static void VerifiesPlanningInputBoundariesAndDefensiveCopies()
|
||||
{
|
||||
DirectionSegmentView segment = CreateStraightSegment();
|
||||
var corridorStations = new[]
|
||||
{
|
||||
new LateralInterval(0d, -0.3d, 0.3d, 0d),
|
||||
new LateralInterval(1d, -0.3d, 0.3d, 0d),
|
||||
new LateralInterval(2d, -0.3d, 0.3d, 0d),
|
||||
};
|
||||
var seeds = new[]
|
||||
{
|
||||
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),
|
||||
};
|
||||
LateralPlanningInput input = new LateralPlanningInput(segment, new StaticCorridor(corridorStations), seeds[0],
|
||||
EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), seeds);
|
||||
corridorStations[1] = new LateralInterval(1d, -0.1d, 0.1d, 0d);
|
||||
seeds[0] = new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d);
|
||||
|
||||
Verification.NearlyEqual(-0.3d, input.Corridor.Stations[1].MinimumL, "input copies corridor stations");
|
||||
Verification.NearlyEqual(0d, input.PreviousTrajectorySeed[0].LateralOffset, "input copies seed list");
|
||||
ExpectArgumentException(() => new LateralPlanningInput(segment,
|
||||
new StaticCorridor(new[] { new LateralInterval(0d, -0.3d, 0.3d, 0d) }),
|
||||
input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
|
||||
"input rejects fewer than two stations");
|
||||
ExpectArgumentException(() => new LateralPlanningInput(segment,
|
||||
new StaticCorridor(new[]
|
||||
{
|
||||
new LateralInterval(0d, -0.3d, 0.3d, 0d),
|
||||
new LateralInterval(0d, -0.3d, 0.3d, 0d),
|
||||
}), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
|
||||
"input rejects non-increasing corridor stations");
|
||||
ExpectArgumentException(() => new LateralPlanningInput(segment,
|
||||
new StaticCorridor(new[]
|
||||
{
|
||||
new LateralInterval(0.1d, -0.3d, 0.3d, 0d),
|
||||
new LateralInterval(2d, -0.3d, 0.3d, 0d),
|
||||
}), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
|
||||
"input rejects start-corridor station mismatch");
|
||||
ExpectArgumentException(() => new LateralPlanningInput(segment,
|
||||
new StaticCorridor(new[]
|
||||
{
|
||||
new LateralInterval(0d, -0.1d, 0.1d, 0d),
|
||||
new LateralInterval(2d, -0.1d, 0.1d, 0d),
|
||||
}), new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d),
|
||||
EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>()),
|
||||
"input rejects start projection outside first hard interval");
|
||||
}
|
||||
|
||||
private static void VerifiesLateralResultPublicationContract()
|
||||
{
|
||||
LateralPath unvalidated = new LateralPath(new[] { CreatePathPoint(0d) }, false);
|
||||
LateralPath validated = new LateralPath(new[] { CreatePathPoint(0d), CreatePathPoint(1d) }, true);
|
||||
|
||||
ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.Success, unvalidated, string.Empty),
|
||||
"success requires an independently validated path");
|
||||
ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback,
|
||||
new LateralPath(Array.Empty<LateralPathPoint>(), true), string.Empty),
|
||||
"fallback requires a non-empty path");
|
||||
ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.LateralInfeasible, validated, string.Empty),
|
||||
"failed result has no candidate");
|
||||
LateralPlanningResult result = new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, validated, "fallback");
|
||||
Verification.Equal(validated, result.Path, "fallback path is preserved");
|
||||
}
|
||||
|
||||
private static void VerifiesNormalizedObjectiveAndHardConstraints()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
||||
LateralPlanningInput input = CreateModelInput(EmTerminalType.Goal, configuration,
|
||||
new[] { 0.2d, -0.1d, 0.3d });
|
||||
LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
|
||||
new[] { 0d, 0d });
|
||||
LateralConstraintBuilder builder = CreateConstraintBuilder();
|
||||
|
||||
Verification.True(builder.TryBuild(input, linearization, out QuadraticProgram problem, out string failureReason),
|
||||
"unit-scale QP builds: " + failureReason);
|
||||
var layout = new LateralVariableLayout(3);
|
||||
Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.L(0), layout.L(0)),
|
||||
"reference plus previous P coefficient");
|
||||
Verification.NearlyEqual(20d, MatrixValue(problem.UpperTriangularP, layout.DDDL(0), layout.DDDL(0)),
|
||||
"jerk P coefficient");
|
||||
Verification.NearlyEqual(-2d, problem.LinearCost[layout.L(0)], "previous-seed q coefficient");
|
||||
|
||||
for (int interval = 0; interval < 2; interval++)
|
||||
{
|
||||
double ds = input.ReferenceStations[interval + 1] - input.ReferenceStations[interval];
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.DDL(interval), -1d },
|
||||
{ layout.DDL(interval + 1), 1d },
|
||||
{ layout.DDDL(interval), -ds },
|
||||
}), "ddl dynamics equality " + interval);
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.DL(interval), -1d },
|
||||
{ layout.DL(interval + 1), 1d },
|
||||
{ layout.DDL(interval), -ds },
|
||||
{ layout.DDDL(interval), -0.5d * ds * ds },
|
||||
}), "dl dynamics equality " + interval);
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.L(interval), -1d },
|
||||
{ layout.L(interval + 1), 1d },
|
||||
{ layout.DL(interval), -ds },
|
||||
{ layout.DDL(interval), -0.5d * ds * ds },
|
||||
{ layout.DDDL(interval), -ds * ds * ds / 6d },
|
||||
}), "l dynamics equality " + interval);
|
||||
}
|
||||
|
||||
for (int station = 0; station < layout.StationCount; station++)
|
||||
{
|
||||
Verification.True(HasFiniteNonEqualityBound(problem, layout.L(station)), "finite lateral hard bound " + station);
|
||||
Verification.True(HasFiniteNonEqualityBound(problem, layout.DL(station)), "finite slope hard bound " + station);
|
||||
Verification.True(HasFiniteNonEqualityBound(problem, layout.DDL(station)), "finite second-derivative hard bound " + station);
|
||||
}
|
||||
for (int interval = 0; interval < layout.StationCount - 1; interval++)
|
||||
Verification.True(HasFiniteNonEqualityBound(problem, layout.DDDL(interval)), "finite jerk hard bound " + interval);
|
||||
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.L(2), 1d } }),
|
||||
"goal terminal l equality");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.DL(2), 1d } }),
|
||||
"goal terminal dl equality");
|
||||
|
||||
LateralPlanningInput rolling = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
|
||||
new[] { 0.2d, -0.1d, 0.3d });
|
||||
Verification.True(builder.TryBuild(rolling, linearization, out QuadraticProgram rollingProblem, out string rollingReason),
|
||||
"rolling QP builds: " + rollingReason);
|
||||
Verification.NearlyEqual(50d, MatrixValue(rollingProblem.UpperTriangularP, layout.L(2), layout.L(2)),
|
||||
"rolling terminal adds normalized objective cost");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem, new Dictionary<int, double> { { layout.L(2), 1d } }),
|
||||
"rolling terminal has no l equality");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem, new Dictionary<int, double> { { layout.DL(2), 1d } }),
|
||||
"rolling terminal has no dl equality");
|
||||
}
|
||||
|
||||
private static void VerifiesAllNamedCostScales()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Corridor.MaximumLateralOffsetMeters = 2d;
|
||||
configuration.Lateral.MaximumLateralSlope = 4d;
|
||||
configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 5d;
|
||||
configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 6d;
|
||||
LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
|
||||
new[] { 0.2d, 0.2d, 0.2d }, referenceCurvatureDerivative: 4d, maximumVehicleCurvature: 7d);
|
||||
LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
|
||||
new[] { 0d, 0d });
|
||||
LateralConstraintBuilder builder = CreateConstraintBuilder();
|
||||
|
||||
Verification.True(builder.TryBuild(input, linearization, out QuadraticProgram problem, out string failureReason),
|
||||
"non-unit-scale QP builds: " + failureReason);
|
||||
var layout = new LateralVariableLayout(3);
|
||||
Verification.NearlyEqual(7.5d, MatrixValue(problem.UpperTriangularP, layout.L(0), layout.L(0)),
|
||||
"reference and previous costs divide by lateral scale squared");
|
||||
Verification.NearlyEqual(-0.5d, problem.LinearCost[layout.L(0)],
|
||||
"previous target coefficient divides by lateral scale squared");
|
||||
Verification.NearlyEqual(0.125d, MatrixValue(problem.UpperTriangularP, layout.DL(0), layout.DL(0)),
|
||||
"heading cost divides by slope scale squared");
|
||||
Verification.NearlyEqual(5d / 9d, MatrixValue(problem.UpperTriangularP, layout.DDDL(0), layout.DDDL(0)),
|
||||
"jerk cost divides by third-derivative scale squared");
|
||||
Verification.NearlyEqual(0.4d + 10d / 49d + 3.125d,
|
||||
MatrixValue(problem.UpperTriangularP, layout.DDL(0), layout.DDL(0)),
|
||||
"second derivative, curvature, and curvature variation use their named scales");
|
||||
Verification.NearlyEqual(12.5d, MatrixValue(problem.UpperTriangularP, layout.L(2), layout.L(2)),
|
||||
"rolling terminal cost divides by lateral scale squared");
|
||||
|
||||
EmPlannerConfiguration denominatorConfiguration = CreateUnitScaleConfiguration();
|
||||
denominatorConfiguration.Lateral.MaximumLateralStepPerIterationMeters = 0.5d;
|
||||
LateralPlanningInput denominatorInput = CreateModelInput(EmTerminalType.RollingSafetyStop, denominatorConfiguration,
|
||||
Array.Empty<double>(), 0d, 2d);
|
||||
Verification.True(builder.TryBuild(denominatorInput, linearization, out QuadraticProgram denominatorProblem,
|
||||
out string denominatorReason), "denominator QP builds: " + denominatorReason);
|
||||
Verification.True(HasBoundWithUpper(denominatorProblem, layout.L(0), 0.4d),
|
||||
"Frenet denominator is intersected as a finite hard lateral bound");
|
||||
}
|
||||
|
||||
private static void VerifiesVehicleCurvatureIsAHardQpConstraint()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
||||
const double maximumVehicleCurvature = 0.25d;
|
||||
LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
|
||||
Array.Empty<double>(), maximumVehicleCurvature: maximumVehicleCurvature);
|
||||
LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
|
||||
new[] { 0d, 0d });
|
||||
|
||||
Verification.True(CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
|
||||
out string failureReason), "curvature-constrained QP builds: " + failureReason);
|
||||
|
||||
var layout = new LateralVariableLayout(input.ReferenceStations.Count);
|
||||
Verification.Equal(8 * layout.StationCount - 2, problem.ConstraintCount,
|
||||
"each lateral station adds one curvature hard-constraint row");
|
||||
for (int station = 0; station < layout.StationCount; station++)
|
||||
{
|
||||
Verification.True(HasBound(problem, new Dictionary<int, double> { { layout.DDL(station), 1d } },
|
||||
-maximumVehicleCurvature, maximumVehicleCurvature),
|
||||
"straight-reference curvature is a hard DDL bound at station " + station);
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifiesEmptyHardBoundIntersectionFailsBeforeSolve()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
||||
LateralPlanningInput input = CreateModelInput(EmTerminalType.RollingSafetyStop, configuration,
|
||||
Array.Empty<double>(), 0d, 0d, 0.9d, 0.9d, 1d);
|
||||
LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
|
||||
new[] { 0d, 0d });
|
||||
|
||||
Verification.True(!CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
|
||||
out string failureReason), "empty corridor/trust intersection is infeasible before solve");
|
||||
Verification.True(problem == null && failureReason.Length > 0, "infeasible build returns no QP and a reason");
|
||||
}
|
||||
|
||||
private static void VerifiesFakeSolverCapturesTheNeutralQpBoundary()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
||||
LateralPlanningInput input = CreateModelInput(EmTerminalType.Goal, configuration, Array.Empty<double>());
|
||||
LateralCandidate linearization = LateralCandidate.Integrate(input.ReferenceStations, 0d, 0d, 0d,
|
||||
new[] { 0d, 0d });
|
||||
Verification.True(CreateConstraintBuilder().TryBuild(input, linearization, out QuadraticProgram problem,
|
||||
out string reason), "fake solver problem builds: " + reason);
|
||||
var expected = new QpSolveResult(QpSolveStatus.Solved, new double[problem.VariableCount], 0d, 0d, 0d, 1,
|
||||
TimeSpan.Zero, "fake", string.Empty);
|
||||
var solver = new FakeQpSolver(expected);
|
||||
var settings = new QpSolverSettings(10, 1e-5d, 1e-5d, TimeSpan.FromSeconds(1d), true, false, false);
|
||||
QpSolveResult actual = solver.Solve(problem, settings, new[] { 1d, 2d }, CancellationToken.None);
|
||||
|
||||
Verification.Equal(expected, actual, "fake solver returns configured result");
|
||||
Verification.Equal(problem, solver.LastProblem, "fake solver records QP");
|
||||
Verification.Equal(settings, solver.LastSettings, "fake solver records settings");
|
||||
Verification.NearlyEqual(2d, solver.LastWarmStart[1], "fake solver records a defensive warm-start copy");
|
||||
}
|
||||
|
||||
private static void VerifiesNonlinearGeometryInBothDirections()
|
||||
{
|
||||
double[] stations = { 0d, 0.7d, 2d };
|
||||
LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, 0.1d, 0.05d,
|
||||
new[] { 0.02d, -0.03d });
|
||||
LateralGeometryEvaluator evaluator = CreateGeometryEvaluator();
|
||||
LateralSolutionValidator validator = CreateGeometryValidator();
|
||||
|
||||
VerifyGeometryForDirection(TravelDirection.Forward, 0d, candidate, evaluator, validator);
|
||||
VerifyGeometryForDirection(TravelDirection.Reverse, 0d, candidate, evaluator, validator);
|
||||
VerifyGeometryForDirection(TravelDirection.Forward, 0.2d, candidate, evaluator, validator);
|
||||
VerifyGeometryForDirection(TravelDirection.Reverse, 0.2d, candidate, evaluator, validator);
|
||||
}
|
||||
|
||||
private static void VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths()
|
||||
{
|
||||
LateralGeometryEvaluator evaluator = CreateGeometryEvaluator();
|
||||
LateralSolutionValidator validator = CreateGeometryValidator();
|
||||
double[] stations = { 0d, 1d, 2d };
|
||||
|
||||
LateralPlanningInput singularInput = CreateGeometryInput(TravelDirection.Forward, 1d, stations, 0.81d, 0d, 10d);
|
||||
LateralCandidate singular = LateralCandidate.Integrate(stations, 0.81d, 0d, 0d, new[] { 0d, 0d });
|
||||
Verification.True(!evaluator.TryEvaluate(singularInput, singular, out LateralPath singularPath, out string singularReason),
|
||||
"Frenet denominator below 0.20 is rejected by reconstruction");
|
||||
Verification.True(singularPath == null && singularReason.Length > 0, "singular reconstruction has no path");
|
||||
|
||||
LateralPlanningInput curvatureInput = CreateGeometryInput(TravelDirection.Forward, 0d, stations, 0d, 0d, 1d);
|
||||
LateralCandidate excessiveCurvature = LateralCandidate.Integrate(stations, 0d, 0d, 2d, new[] { 0d, 0d });
|
||||
Verification.True(evaluator.TryEvaluate(curvatureInput, excessiveCurvature, out LateralPath excessivePath,
|
||||
out string excessiveReason), "curvature reconstruction remains geometric: " + excessiveReason);
|
||||
Verification.True(!validator.TryValidate(curvatureInput, excessiveCurvature, excessivePath,
|
||||
out LateralPath rejectedCurvature, out string curvatureReason), "curvature above vehicle limit is rejected");
|
||||
Verification.True(rejectedCurvature == null && curvatureReason.Length > 0, "curvature failure has no validated path");
|
||||
|
||||
LateralCandidate valid = LateralCandidate.Integrate(stations, 0d, 0d, 0d, new[] { 0d, 0d });
|
||||
Verification.True(evaluator.TryEvaluate(curvatureInput, valid, out LateralPath rawPath, out string rawReason),
|
||||
"valid path reconstructs: " + rawReason);
|
||||
var tamperedPoints = new List<LateralPathPoint>(rawPath.Points);
|
||||
LateralPathPoint original = tamperedPoints[1];
|
||||
tamperedPoints[1] = new LateralPathPoint(original.ReferenceS, original.PathS, original.L, original.DL,
|
||||
original.DDL, original.DDDL, original.X + 0.01d, original.Y, original.VehicleYaw,
|
||||
original.GeometricCurvature, original.VehicleCurvature, original.VehicleCurvatureDerivative);
|
||||
Verification.True(!validator.TryValidate(curvatureInput, valid, new LateralPath(tamperedPoints, false),
|
||||
out LateralPath rejectedTampered, out string tamperedReason),
|
||||
"validator independently rejects a world-coordinate mismatch");
|
||||
Verification.True(rejectedTampered == null && tamperedReason.Length > 0, "tampered path has no validated copy");
|
||||
|
||||
ExpectArgumentException(() => new LateralCandidate(stations, new[] { double.NaN, 0d, 0d },
|
||||
new[] { 0d, 0d, 0d }, new[] { 0d, 0d, 0d }, new[] { 0d, 0d }), "non-finite lateral values are rejected");
|
||||
ExpectArgumentException(() => new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, double.NaN, 0d, 0d, 0d, 0d, 0d),
|
||||
"non-finite world geometry is rejected");
|
||||
}
|
||||
|
||||
private static void VerifyGeometryForDirection(TravelDirection direction, double referenceCurvature,
|
||||
LateralCandidate candidate,
|
||||
LateralGeometryEvaluator evaluator, LateralSolutionValidator validator)
|
||||
{
|
||||
LateralPlanningInput input = CreateGeometryInput(direction, referenceCurvature, candidate.ReferenceStations,
|
||||
candidate.L[0], candidate.DL[0], 10d);
|
||||
Verification.True(evaluator.TryEvaluate(input, candidate, out LateralPath rawPath, out string evaluationReason),
|
||||
direction + " geometry reconstructs: " + evaluationReason);
|
||||
Verification.True(!rawPath.IsIndependentlyValidated, direction + " evaluator does not self-validate");
|
||||
Verification.True(validator.TryValidate(input, candidate, rawPath, out LateralPath validatedPath,
|
||||
out string validationReason), direction + " geometry validates: " + validationReason);
|
||||
Verification.True(validatedPath.IsIndependentlyValidated, direction + " validation creates a marked immutable path");
|
||||
|
||||
double directionSign = direction == TravelDirection.Forward ? 1d : -1d;
|
||||
Verification.NearlyEqual(0d, rawPath.Points[0].PathS, direction + " PathS starts at zero");
|
||||
for (int index = 0; index < rawPath.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint point = rawPath.Points[index];
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment, point.ReferenceS);
|
||||
double denominator = 1d - reference.GeometricCurvature * point.L;
|
||||
double expectedX = reference.X - point.L * Math.Sin(reference.TravelYaw);
|
||||
double expectedY = reference.Y + point.L * Math.Cos(reference.TravelYaw);
|
||||
double expectedTravelYaw = reference.TravelYaw + Math.Atan2(point.DL, denominator);
|
||||
double expectedVehicleYaw = direction == TravelDirection.Forward
|
||||
? AngleMath.NormalizeRadians(expectedTravelYaw)
|
||||
: AngleMath.NormalizeRadians(expectedTravelYaw + Math.PI);
|
||||
double expectedGeometricCurvature = CalculateGeometricCurvature(reference, point.L, point.DL, point.DDL,
|
||||
directionSign * reference.VehicleCurvatureDerivative);
|
||||
Verification.NearlyEqual(expectedX, point.X, direction + " world x " + index);
|
||||
Verification.NearlyEqual(expectedY, point.Y, direction + " world y " + index);
|
||||
Verification.NearlyEqual(expectedVehicleYaw, point.VehicleYaw, direction + " vehicle yaw " + index);
|
||||
Verification.NearlyEqual(expectedGeometricCurvature, point.GeometricCurvature,
|
||||
direction + " full Frenet curvature " + index);
|
||||
Verification.NearlyEqual(directionSign * point.GeometricCurvature, point.VehicleCurvature,
|
||||
direction + " vehicle curvature sign " + index);
|
||||
if (index > 0)
|
||||
{
|
||||
LateralPathPoint previous = rawPath.Points[index - 1];
|
||||
double chord = Math.Sqrt((point.X - previous.X) * (point.X - previous.X) +
|
||||
(point.Y - previous.Y) * (point.Y - previous.Y));
|
||||
Verification.NearlyEqual(previous.PathS + chord, point.PathS, direction + " actual chord PathS " + index);
|
||||
Verification.True(point.PathS > previous.PathS, direction + " PathS strictly increases " + index);
|
||||
}
|
||||
}
|
||||
|
||||
LateralPathPoint check = rawPath.Points[1];
|
||||
double signedSpeed = directionSign * 0.3d;
|
||||
var trajectoryPoint = new EmTrajectoryPoint(check.X, check.Y, check.VehicleYaw, signedSpeed, 0d,
|
||||
check.VehicleCurvature, 0, check.ReferenceS, check.PathS, direction, EmBoundaryType.None, 0d, 0d);
|
||||
Verification.NearlyEqual(signedSpeed * check.VehicleCurvature, trajectoryPoint.YawRate,
|
||||
direction + " yaw-rate identity");
|
||||
Verification.NearlyEqual(0.3d * check.GeometricCurvature, trajectoryPoint.YawRate,
|
||||
direction + " travel curvature yaw-rate identity");
|
||||
}
|
||||
|
||||
private static DirectionSegmentView CreateStraightSegment(double referenceCurvatureDerivative = 0d,
|
||||
double referenceCurvature = 0d)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d, referenceCurvatureDerivative, referenceCurvature),
|
||||
Point(1d, 1d, referenceCurvatureDerivative, referenceCurvature),
|
||||
Point(2d, 2d, referenceCurvatureDerivative, referenceCurvature),
|
||||
};
|
||||
return new DirectionSegmentView(0, TravelDirection.Forward, points,
|
||||
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
||||
new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
|
||||
}
|
||||
|
||||
private static SmoothedPathPoint Point(double x, double s, double curvatureDerivative = 0d, double curvature = 0d)
|
||||
{
|
||||
return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, curvature, curvature,
|
||||
curvatureDerivative, 1d,
|
||||
false, SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
|
||||
private static EmPlannerConfiguration CreateUnitScaleConfiguration()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Corridor.MaximumLateralOffsetMeters = 1d;
|
||||
configuration.Lateral.MaximumLateralSlope = 1d;
|
||||
configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 1d;
|
||||
configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 1d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static LateralPlanningInput CreateModelInput(EmTerminalType terminalType, EmPlannerConfiguration configuration,
|
||||
IReadOnlyList<double> previousL, double referenceCurvatureDerivative = 0d, double referenceCurvature = 0d,
|
||||
double startL = 0d, double corridorMinimum = -1d, double corridorMaximum = 1d,
|
||||
double maximumVehicleCurvature = 1d)
|
||||
{
|
||||
DirectionSegmentView segment = CreateStraightSegment(referenceCurvatureDerivative, referenceCurvature);
|
||||
double corridorSeed = Math.Max(corridorMinimum, Math.Min(corridorMaximum, 0d));
|
||||
var stations = new[]
|
||||
{
|
||||
new LateralInterval(0d, corridorMinimum, corridorMaximum, startL),
|
||||
new LateralInterval(1d, corridorMinimum, corridorMaximum, corridorSeed),
|
||||
new LateralInterval(2d, corridorMinimum, corridorMaximum, corridorSeed),
|
||||
};
|
||||
var seed = new List<FrenetProjection>();
|
||||
for (int index = 0; index < previousL.Count; index++)
|
||||
seed.Add(new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, index), previousL[index], 0d, 0d));
|
||||
return new LateralPlanningInput(segment, new StaticCorridor(stations),
|
||||
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), startL, 0d, 0d), terminalType,
|
||||
CreateVehicle(maximumVehicleCurvature), configuration, seed);
|
||||
}
|
||||
|
||||
private static LateralPlanningInput CreateGeometryInput(TravelDirection direction, double referenceCurvature,
|
||||
IReadOnlyList<double> stations, double startL, double startDL, double maximumVehicleCurvature)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>(stations.Count);
|
||||
for (int index = 0; index < stations.Count; index++)
|
||||
{
|
||||
double s = stations[index];
|
||||
double travelYaw = referenceCurvature * s;
|
||||
double x = Math.Abs(referenceCurvature) <= 1e-12d ? s : Math.Sin(travelYaw) / referenceCurvature;
|
||||
double y = Math.Abs(referenceCurvature) <= 1e-12d ? 0d : (1d - Math.Cos(travelYaw)) / referenceCurvature;
|
||||
double vehicleYaw = direction == TravelDirection.Forward ? travelYaw : travelYaw - Math.PI;
|
||||
points.Add(new SmoothedPathPoint(x, y, AngleMath.NormalizeRadians(vehicleYaw), vehicleYaw, s, direction,
|
||||
referenceCurvature, direction == TravelDirection.Forward ? referenceCurvature : -referenceCurvature,
|
||||
0d, 1d, false, SmoothedPathPointSource.Anchor));
|
||||
}
|
||||
double end = stations[stations.Count - 1];
|
||||
var segment = new DirectionSegmentView(0, direction, points,
|
||||
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
||||
new ReferenceBoundary(0, end, EmBoundaryType.RollingSafetyStop, end), 0d);
|
||||
var corridorStations = new List<LateralInterval>(stations.Count);
|
||||
for (int index = 0; index < stations.Count; index++)
|
||||
corridorStations.Add(new LateralInterval(stations[index], -1d, 1d, startL));
|
||||
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
||||
configuration.Lateral.MaximumLateralSlope = 5d;
|
||||
configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 5d;
|
||||
configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 5d;
|
||||
double startDenominator = 1d - referenceCurvature * startL;
|
||||
return new LateralPlanningInput(segment, new StaticCorridor(corridorStations),
|
||||
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, stations[0]), startL,
|
||||
Math.Atan2(startDL, startDenominator), 0d), EmTerminalType.RollingSafetyStop,
|
||||
CreateVehicle(maximumVehicleCurvature), configuration, Array.Empty<FrenetProjection>());
|
||||
}
|
||||
|
||||
private static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
|
||||
double referenceCurvatureDerivative)
|
||||
{
|
||||
double a = 1d - reference.GeometricCurvature * l;
|
||||
return (a * a * reference.GeometricCurvature + a * ddl + referenceCurvatureDerivative * l * dl +
|
||||
2d * reference.GeometricCurvature * dl * dl) / Math.Pow(a * a + dl * dl, 1.5d);
|
||||
}
|
||||
|
||||
private static LateralGeometryEvaluator CreateGeometryEvaluator()
|
||||
{
|
||||
return new LateralGeometryEvaluator();
|
||||
}
|
||||
|
||||
private static LateralSolutionValidator CreateGeometryValidator()
|
||||
{
|
||||
return new LateralSolutionValidator();
|
||||
}
|
||||
|
||||
private static LateralObjectiveBuilder CreateObjectiveBuilder()
|
||||
{
|
||||
return new LateralObjectiveBuilder();
|
||||
}
|
||||
|
||||
private static LateralConstraintBuilder CreateConstraintBuilder()
|
||||
{
|
||||
return new LateralConstraintBuilder(CreateObjectiveBuilder());
|
||||
}
|
||||
|
||||
private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
|
||||
{
|
||||
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (matrix.RowIndices[index] == row)
|
||||
return matrix.Values[index];
|
||||
}
|
||||
return 0d;
|
||||
}
|
||||
|
||||
private static int CountExactEqualityRows(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected)
|
||||
{
|
||||
int count = 0;
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
if (Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d &&
|
||||
RowMatches(problem.ConstraintMatrix, row, expected))
|
||||
{
|
||||
count++;
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
private static bool HasFiniteNonEqualityBound(QuadraticProgram problem, int variable)
|
||||
{
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
if (Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) > 1e-12d &&
|
||||
RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }) &&
|
||||
!double.IsInfinity(problem.LowerBounds[row]) && !double.IsInfinity(problem.UpperBounds[row]))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool HasBoundWithUpper(QuadraticProgram problem, int variable, double upper)
|
||||
{
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
if (RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }) &&
|
||||
Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool HasBound(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
|
||||
double lower, double upper)
|
||||
{
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
if (RowMatches(problem.ConstraintMatrix, row, expected) &&
|
||||
Math.Abs(problem.LowerBounds[row] - lower) <= 1e-12d &&
|
||||
Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary<int, double> expected)
|
||||
{
|
||||
var actual = new Dictionary<int, double>();
|
||||
for (int column = 0; column < matrix.ColumnCount; column++)
|
||||
{
|
||||
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (matrix.RowIndices[index] == targetRow)
|
||||
actual[column] = matrix.Values[index];
|
||||
}
|
||||
}
|
||||
if (actual.Count != expected.Count)
|
||||
return false;
|
||||
foreach (KeyValuePair<int, double> expectedEntry in expected)
|
||||
{
|
||||
if (!actual.TryGetValue(expectedEntry.Key, out double value) || Math.Abs(value - expectedEntry.Value) > 1e-12d)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static VehicleParameters CreateVehicle(double maximumCurvature = 1d)
|
||||
{
|
||||
return new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.1d,
|
||||
WidthMeters = 0.1d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = maximumCurvature,
|
||||
};
|
||||
}
|
||||
|
||||
private static LateralPathPoint CreatePathPoint(double referenceS)
|
||||
{
|
||||
return new LateralPathPoint(referenceS, referenceS, 0d, 0d, 0d, 0d, referenceS, 0d, 0d, 0d, 0d, 0d);
|
||||
}
|
||||
|
||||
private static LateralVariableLayout CreateLayout(int stationCount)
|
||||
{
|
||||
return new LateralVariableLayout(stationCount);
|
||||
}
|
||||
|
||||
private static void ExpectArgumentException(Action action, string name)
|
||||
{
|
||||
try
|
||||
{
|
||||
action();
|
||||
}
|
||||
catch (ArgumentException)
|
||||
{
|
||||
return;
|
||||
}
|
||||
throw new InvalidOperationException(name + " did not throw ArgumentException.");
|
||||
}
|
||||
}
|
||||
@@ -1,829 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.IO;
|
||||
using System.Threading;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class LongitudinalIntegrationChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
|
||||
VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
|
||||
VerifiesFullDirectionStaticStartMakesProgress();
|
||||
VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed();
|
||||
VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed();
|
||||
VerifiesFullDirectionNearZeroScheduleDurationIsReasonable();
|
||||
VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
|
||||
VerifiesExactStopIncludesAStabilizationTail();
|
||||
VerifiesLastStrictCandidateSurvivesLaterTimeout();
|
||||
VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
|
||||
VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection();
|
||||
VerifiesExactStopTailDoesNotDistortEnvelope();
|
||||
VerifiesValidatedEndpointsAreCanonical();
|
||||
VerifiesWarmStartAndFiveIterationLimit();
|
||||
VerifiesNonzeroSpeedSeedIsStrictlyFeasible();
|
||||
VerifiesCancellationInfeasibilityAndPlannerDelegation();
|
||||
RunRealOsqpInCleanPluginBundle();
|
||||
}
|
||||
|
||||
private static void VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.DistanceHorizonMeters = 5d;
|
||||
configuration.Scheduling.TimeHorizonSeconds = 2d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, 2.5d, 0d),
|
||||
Point(2d, 5d, 0d),
|
||||
}, true);
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
|
||||
|
||||
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"rolling seed remains a strict timeout fallback: " + result.FailureReason);
|
||||
LongitudinalCandidate candidate = result.Candidate ??
|
||||
throw new InvalidOperationException("Rolling timeout fallback candidate was missing.");
|
||||
Verification.Equal(21, candidate.KnotTimes.Count, "two-second ST emits twenty-one knots");
|
||||
Verification.True(candidate.S[candidate.S.Count - 1] < input.PathUpperBoundS,
|
||||
"two-second ST does not consume the five-metre LS window");
|
||||
Verification.True(candidate.U[candidate.U.Count - 1] > 0.01d,
|
||||
"rolling ST keeps nonzero terminal speed");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 10d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
|
||||
configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationKnotCount = 401;
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, 1d, 0d),
|
||||
Point(2d, 2d, 0d),
|
||||
}, true);
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.10d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "full schedule envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule coarsePublication, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.10 s publication: " + failureReason);
|
||||
|
||||
EmPlannerConfiguration densePublicationConfiguration = configuration.Copy();
|
||||
densePublicationConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
|
||||
densePublicationConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond, densePublicationConfiguration,
|
||||
out LongitudinalKnotSchedule densePublication, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.05 s publication: " + failureReason);
|
||||
Verification.Equal(coarsePublication.KnotTimes.Count, densePublication.KnotTimes.Count,
|
||||
"publication cadence does not determine full-segment optimization knot count");
|
||||
|
||||
var publicationCandidate = new LongitudinalCandidate(new[] { 0d, 0.50d, 1d },
|
||||
new[] { 0d, 1d / 60d, 1d / 60d }, new[] { 0.10d, 0d, 0d }, new[] { -0.40d, 0d, 0d },
|
||||
new[] { 0.80d, 0d });
|
||||
var publicationResult = new LongitudinalPlanningResult(EmPlanningStatus.Success, publicationCandidate, string.Empty);
|
||||
DateTimeOffset now = DateTimeOffset.UtcNow;
|
||||
var metadata = new EmTrajectoryMetadata("publication-cadence", now, now, 1L, "publication-path", 1L,
|
||||
string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
|
||||
EmPlanningScope.FullDirectionSegment);
|
||||
configuration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
|
||||
densePublicationConfiguration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
|
||||
EmTrajectory coarseTrajectory = new EmTrajectoryAssembler(configuration).Assemble(path, publicationResult, metadata);
|
||||
EmTrajectory denseTrajectory = new EmTrajectoryAssembler(densePublicationConfiguration).Assemble(path,
|
||||
publicationResult, metadata);
|
||||
Verification.Equal(2 * (coarseTrajectory.Points.Count - 1), denseTrajectory.Points.Count - 1,
|
||||
"halving publication cadence doubles emitted trajectory intervals without changing optimization knots");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionStaticStartMakesProgress()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(2.5d, 2.5d, 0d),
|
||||
Point(5d, 5d, 0d),
|
||||
}, true);
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "static-start envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule schedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "static-start schedule: " + failureReason);
|
||||
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
|
||||
LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
|
||||
CancellationToken.None);
|
||||
|
||||
Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
|
||||
"static start succeeds: " + result.FailureReason);
|
||||
LongitudinalCandidate candidate = result.Candidate ??
|
||||
throw new InvalidOperationException("static start must expose a successful candidate.");
|
||||
Verification.True(HasValueGreaterThan(candidate.S, 0.05d), "static start makes measurable progress");
|
||||
Verification.True(HasValueGreaterThan(candidate.U, 0.05d), "static start accelerates");
|
||||
Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
|
||||
"full segment stops at the terminal boundary");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.SolverTimeoutSeconds = 5d;
|
||||
configuration.Solver.MaximumOsqpIterations = 100000;
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(2.5d, 2.5d, 0d),
|
||||
Point(5d, 5d, 0d),
|
||||
}, true);
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.01d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "near-zero envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.01d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule schedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "near-zero schedule: " + failureReason);
|
||||
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.01d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
|
||||
LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
|
||||
CancellationToken.None);
|
||||
|
||||
Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
|
||||
"near-zero full-direction start succeeds: " + result.FailureReason);
|
||||
LongitudinalCandidate candidate = result.Candidate ??
|
||||
throw new InvalidOperationException("Near-zero full-direction candidate was missing.");
|
||||
Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1],
|
||||
"near-zero start reaches the terminal boundary");
|
||||
Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
|
||||
"near-zero start stops at the terminal boundary");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.SolverTimeoutSeconds = 5d;
|
||||
configuration.Solver.MaximumOsqpIterations = 100000;
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, 2.25d, 0.1d),
|
||||
Point(2d, 4.5d, 0d),
|
||||
}, true);
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.0001d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.0001d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule schedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero schedule: " + failureReason);
|
||||
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.0001d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
|
||||
LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
|
||||
CancellationToken.None);
|
||||
|
||||
Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
|
||||
"curved near-zero full-direction start succeeds: " + result.FailureReason);
|
||||
LongitudinalCandidate candidate = result.Candidate ??
|
||||
throw new InvalidOperationException("Curved near-zero full-direction candidate was missing.");
|
||||
Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1],
|
||||
"curved near-zero start reaches the terminal boundary");
|
||||
Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
|
||||
"curved near-zero start stops at the terminal boundary");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionNearZeroScheduleDurationIsReasonable()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, 2.25d, 0.1d),
|
||||
Point(2d, 4.5d, 0d),
|
||||
}, true);
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.0001d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "near-zero duration envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.0001d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule schedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "near-zero duration schedule: " + failureReason);
|
||||
|
||||
double duration = schedule.KnotTimes[schedule.KnotTimes.Count - 1];
|
||||
Verification.True(duration < 120d,
|
||||
"near-zero schedule duration is normal: " + duration.ToString("R", System.Globalization.CultureInfo.InvariantCulture));
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Longitudinal.ZeroSpeedHoldSeconds = 0.20d;
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, 0.0075d, 0d),
|
||||
}, true);
|
||||
var candidate = new LongitudinalCandidate(new[] { 0d, 0.10d, 0.20d, 0.40d },
|
||||
new[] { 0d, 0.005d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0d, 0d },
|
||||
new[] { 0d, -0.5d, 0d, 0d }, new[] { -5d, 5d, 0d });
|
||||
var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
|
||||
DateTimeOffset now = DateTimeOffset.UtcNow;
|
||||
var metadata = new EmTrajectoryMetadata("full-hold", now, now, 1L, "hold-path", 1L, string.Empty, 0,
|
||||
TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
|
||||
EmPlanningScope.FullDirectionSegment);
|
||||
|
||||
EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(path, result, metadata);
|
||||
|
||||
Verification.NearlyEqual(0.40d, trajectory.Points[trajectory.Points.Count - 1].TimeFromStart,
|
||||
"full-scope publication reuses its candidate hold instead of appending a second hold");
|
||||
Verification.Equal(3, CountStationaryTerminalPoints(trajectory),
|
||||
"full-scope publication emits the candidate's single nonzero terminal hold");
|
||||
}
|
||||
|
||||
private static void VerifiesExactStopIncludesAStabilizationTail()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateExactStopSeedConfiguration();
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, 0.00375d, 0d),
|
||||
Point(2d, 0.0075d, 0d),
|
||||
}, true);
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
|
||||
|
||||
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
|
||||
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(
|
||||
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
LongitudinalCandidate seed = FromPrimal(times, solver.WarmStarts[0]);
|
||||
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "exact-stop seed envelope: " + speedFailure);
|
||||
Verification.True(new LongitudinalSolutionValidator().TryValidate(input, envelope, seed, out _,
|
||||
out string validationFailure), "exact-stop seed is strictly feasible: " + validationFailure);
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"exact-stop seed remains a strict timeout fallback");
|
||||
LongitudinalCandidate candidate = result.Candidate ??
|
||||
throw new InvalidOperationException("Exact-stop timeout fallback candidate was missing.");
|
||||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
for (int index = stabilizationStart; index < candidate.S.Count; index++)
|
||||
{
|
||||
Verification.NearlyEqual(input.StopBoundaryPathS, candidate.S[index], "stop-tail S " + index);
|
||||
Verification.NearlyEqual(0d, candidate.U[index], "stop-tail U " + index);
|
||||
Verification.NearlyEqual(0d, candidate.A[index], "stop-tail A " + index);
|
||||
}
|
||||
}
|
||||
|
||||
private static int CountStationaryTerminalPoints(EmTrajectory trajectory)
|
||||
{
|
||||
double terminalPathS = trajectory.Points[trajectory.Points.Count - 1].PathS;
|
||||
int count = 0;
|
||||
for (int index = 0; index < trajectory.Points.Count; index++)
|
||||
{
|
||||
EmTrajectoryPoint point = trajectory.Points[index];
|
||||
if (Math.Abs(point.PathS - terminalPathS) <= 1e-12d &&
|
||||
Math.Abs(point.SignedLongitudinalVelocity) <= 1e-12d)
|
||||
{
|
||||
count++;
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
private static bool HasValueGreaterThan(IReadOnlyList<double> values, double threshold)
|
||||
{
|
||||
for (int index = 0; index < values.Count; index++)
|
||||
{
|
||||
if (values[index] > threshold)
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
public static void RunRealOsqp()
|
||||
{
|
||||
foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())
|
||||
{
|
||||
LongitudinalPlanningResult first = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
|
||||
CancellationToken.None);
|
||||
LongitudinalPlanningResult second = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
|
||||
CancellationToken.None);
|
||||
VerifyRealScenario(scenario, first);
|
||||
VerifyRealScenario(scenario, second);
|
||||
VerifyDeterministicResult(scenario.Name, first, second);
|
||||
}
|
||||
}
|
||||
|
||||
private static void RunRealOsqpInCleanPluginBundle()
|
||||
{
|
||||
string pluginDirectory = Path.Combine(Path.GetTempPath(), "em-planner-longitudinal-real-" + Guid.NewGuid().ToString("N"));
|
||||
try
|
||||
{
|
||||
Directory.CreateDirectory(pluginDirectory);
|
||||
foreach (string sourcePath in Directory.GetFiles(AppContext.BaseDirectory))
|
||||
File.Copy(sourcePath, Path.Combine(pluginDirectory, Path.GetFileName(sourcePath)), false);
|
||||
string nativeSource = Path.GetFullPath(Path.Combine(AppContext.BaseDirectory, "..", "..", "..", "..", "..",
|
||||
"ThirdParty", "OSQP", "win-x64", "osqp.dll"));
|
||||
Verification.True(File.Exists(nativeSource), "pinned OSQP DLL is available for the real longitudinal bundle");
|
||||
File.Copy(nativeSource, Path.Combine(pluginDirectory, "osqp.dll"), true);
|
||||
|
||||
var startInfo = new ProcessStartInfo
|
||||
{
|
||||
FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
|
||||
Arguments = "longitudinal-real-osqp-probe",
|
||||
WorkingDirectory = pluginDirectory,
|
||||
UseShellExecute = false,
|
||||
CreateNoWindow = true,
|
||||
RedirectStandardOutput = true,
|
||||
RedirectStandardError = true,
|
||||
};
|
||||
using (var process = new Process { StartInfo = startInfo })
|
||||
{
|
||||
process.Start();
|
||||
string standardOutput = process.StandardOutput.ReadToEnd();
|
||||
string standardError = process.StandardError.ReadToEnd();
|
||||
process.WaitForExit();
|
||||
if (process.ExitCode != 0 || standardOutput.IndexOf("PASS longitudinal-real-osqp", StringComparison.Ordinal) < 0)
|
||||
{
|
||||
throw new InvalidOperationException("Real longitudinal OSQP clean-plugin probe exited " + process.ExitCode + ": " +
|
||||
standardError + standardOutput);
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (Directory.Exists(pluginDirectory))
|
||||
Directory.Delete(pluginDirectory, true);
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifiesLastStrictCandidateSurvivesLaterTimeout()
|
||||
{
|
||||
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, ToPrimal(valid), 10d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
||||
});
|
||||
|
||||
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"timeout after strict candidate returns fallback success");
|
||||
LongitudinalCandidate fallback = result.Candidate ?? throw new InvalidOperationException("Fallback candidate was missing.");
|
||||
Verification.NearlyEqual(valid.S[1], fallback.S[1], "last strict candidate remains the fallback");
|
||||
}
|
||||
|
||||
private static void VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks()
|
||||
{
|
||||
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
||||
double[] invalid = ToPrimal(valid);
|
||||
var layout = new LongitudinalVariableLayout(valid.KnotTimes.Count);
|
||||
invalid[layout.U(1)] = 10d;
|
||||
var invalidSolver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, invalid, 1d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d),
|
||||
});
|
||||
LongitudinalPlanningResult invalidResult = new SequentialLongitudinalOptimizer(invalidSolver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, invalidResult.Status,
|
||||
"invalid solver vector cannot replace the strict initial fallback");
|
||||
Verification.NearlyEqual(valid.U[1], invalidResult.Candidate?.U[1] ?? double.NaN,
|
||||
"invalid solver vector does not become the fallback candidate");
|
||||
|
||||
var inaccurateSolver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.SolvedInaccurate, ToPrimal(valid), 1d, 2e-5d, 0d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d),
|
||||
});
|
||||
LongitudinalPlanningResult inaccurateResult = new SequentialLongitudinalOptimizer(inaccurateSolver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, inaccurateResult.Status,
|
||||
"inaccurate residual candidate cannot replace the strict initial fallback");
|
||||
Verification.NearlyEqual(valid.U[1], inaccurateResult.Candidate?.U[1] ?? double.NaN,
|
||||
"inaccurate residual does not become the fallback candidate");
|
||||
double[] inaccuratePrimal = ToPrimal(valid);
|
||||
for (int index = 0; index < inaccuratePrimal.Length; index++)
|
||||
{
|
||||
Verification.NearlyEqual(inaccuratePrimal[index], inaccurateSolver.WarmStarts[1][index],
|
||||
"inaccurate finite primal only warms the next ST QP " + index);
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifiesWarmStartAndFiveIterationLimit()
|
||||
{
|
||||
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
||||
var firstSolveOnly = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
|
||||
new SequentialLongitudinalOptimizer(firstSolveOnly).Optimize(input, CancellationToken.None);
|
||||
Verification.Equal(true, firstSolveOnly.LastSettings != null && firstSolveOnly.LastSettings.EnableWarmStart,
|
||||
"first ST solve enables a dynamics-consistent native warm start");
|
||||
Verification.True(FromPrimal(valid.KnotTimes, firstSolveOnly.WarmStarts[0]).SatisfiesExactDiscreteDynamics(1e-12d),
|
||||
"first ST warm start satisfies exact constant-jerk dynamics");
|
||||
|
||||
var results = new List<QpSolveResult>();
|
||||
for (int index = 0; index < 5; index++)
|
||||
results.Add(Result(QpSolveStatus.Solved, ToPrimal(valid), 100d - 10d * index));
|
||||
var solver = new FakeQpSolver(results);
|
||||
|
||||
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.Success, result.Status, "five solved iterations publish success");
|
||||
Verification.Equal(5, solver.SolveCallCount, "ST has a hard five-envelope-iteration maximum");
|
||||
Verification.Equal(new LongitudinalVariableLayout(valid.KnotTimes.Count).VariableCount, solver.WarmStarts[0].Count,
|
||||
"first ST linearization seed remains a complete primal vector");
|
||||
Verification.Equal(true, solver.LastSettings != null && solver.LastSettings.EnableWarmStart,
|
||||
"later ST solves enable native warm start");
|
||||
for (int index = 0; index < solver.WarmStarts[1].Count; index++)
|
||||
Verification.NearlyEqual(ToPrimal(valid)[index], solver.WarmStarts[1][index], "strict candidate warms the next QP " + index);
|
||||
}
|
||||
|
||||
private static void VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection()
|
||||
{
|
||||
LongitudinalPlanningInput baseline = CreateFakeInput(out LongitudinalCandidate candidate);
|
||||
var curvedPath = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, candidate.S[1], 10000d),
|
||||
Point(2d, baseline.PathUpperBoundS, 0d),
|
||||
}, true);
|
||||
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
|
||||
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
|
||||
baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
|
||||
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "curved-envelope setup: " + speedFailure);
|
||||
Verification.True(candidate.U[1] > envelope.MaximumSpeedAt(candidate.S[1]),
|
||||
"scripted candidate violates its own curvature speed envelope");
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, ToPrimal(candidate), 2d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 2d),
|
||||
});
|
||||
|
||||
new SequentialLongitudinalOptimizer(solver).Optimize(input, CancellationToken.None);
|
||||
Verification.Equal(2, solver.SolveCallCount, "rejected candidate reaches the next envelope iteration");
|
||||
var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
|
||||
FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
|
||||
Verification.True(Math.Abs(secondUpper - envelope.MaximumSpeedAt(candidate.S[1])) > 1e-12d,
|
||||
"scripted rejection advances the ST PathS envelope probe");
|
||||
}
|
||||
|
||||
private static void VerifiesExactStopTailDoesNotDistortEnvelope()
|
||||
{
|
||||
LongitudinalPlanningInput baseline = CreateFakeInput(out LongitudinalCandidate valid);
|
||||
double[] perturbedProgress = new double[valid.S.Count];
|
||||
for (int index = 0; index < perturbedProgress.Length; index++)
|
||||
perturbedProgress[index] = valid.S[index];
|
||||
perturbedProgress[perturbedProgress.Length - 1] += 0.05d;
|
||||
var toleranceCandidate = new LongitudinalCandidate(valid.KnotTimes, perturbedProgress, valid.U, valid.A, valid.J);
|
||||
var curvedPath = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, valid.S[1], 10000d),
|
||||
Point(2d, baseline.PathUpperBoundS, 0d),
|
||||
}, true);
|
||||
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
|
||||
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
|
||||
baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 2d),
|
||||
});
|
||||
|
||||
new SequentialLongitudinalOptimizer(solver).Optimize(input, CancellationToken.None);
|
||||
Verification.Equal(2, solver.SolveCallCount, "rejected terminal PathS still reaches a new envelope iteration");
|
||||
var layout = new LongitudinalVariableLayout(valid.KnotTimes.Count);
|
||||
FindSingleVariableBounds(solver.Problems[0], layout.U(1), out _, out double firstUpper);
|
||||
FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
|
||||
Verification.NearlyEqual(firstUpper, secondUpper,
|
||||
"exact stop-tail perturbations do not distort a moving-knot envelope sample");
|
||||
}
|
||||
|
||||
private static void VerifiesCancellationInfeasibilityAndPlannerDelegation()
|
||||
{
|
||||
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
||||
using (var cancellation = new CancellationTokenSource())
|
||||
{
|
||||
cancellation.Cancel();
|
||||
var cancellationSolver = new FakeQpSolver(Result(QpSolveStatus.Solved, ToPrimal(valid), 1d));
|
||||
LongitudinalPlanningResult cancelled = new SequentialLongitudinalOptimizer(cancellationSolver).Optimize(input,
|
||||
cancellation.Token);
|
||||
Verification.Equal(EmPlanningStatus.Cancelled, cancelled.Status, "cancellation before QP solve");
|
||||
Verification.Equal(0, cancellationSolver.SolveCallCount, "cancelled ST does not call the QP solver");
|
||||
}
|
||||
|
||||
using (var cancellation = new CancellationTokenSource())
|
||||
{
|
||||
var solver = new CancellingQpSolver(
|
||||
new FakeQpSolver(Result(QpSolveStatus.Solved, ToPrimal(valid), 1d)), cancellation);
|
||||
LongitudinalPlanningResult cancelledAfterStrictCandidate = new SequentialLongitudinalOptimizer(solver).Optimize(
|
||||
input, cancellation.Token);
|
||||
Verification.Equal(EmPlanningStatus.Cancelled, cancelledAfterStrictCandidate.Status,
|
||||
"cancellation after a strict ST candidate is never fallback success");
|
||||
Verification.True(cancelledAfterStrictCandidate.Candidate == null,
|
||||
"cancelled ST result exposes no candidate");
|
||||
}
|
||||
|
||||
var infeasibleSolver = new FakeQpSolver(Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>(), 1d));
|
||||
LongitudinalPlanningResult infeasible = new SequentialLongitudinalOptimizer(infeasibleSolver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, infeasible.Status,
|
||||
"QP infeasibility preserves the strict mode-specific seed");
|
||||
Verification.True(infeasible.Candidate != null, "QP infeasibility retains a safe fallback profile");
|
||||
|
||||
var plannerSolver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, ToPrimal(valid), 2d),
|
||||
Result(QpSolveStatus.Solved, ToPrimal(valid), 1d),
|
||||
Result(QpSolveStatus.Solved, ToPrimal(valid), 1d),
|
||||
});
|
||||
LongitudinalPlanningResult delegated = new LongitudinalPlanner(plannerSolver).Plan(input, CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.Success, delegated.Status, "LongitudinalPlanner delegates to the ST optimizer");
|
||||
}
|
||||
|
||||
private static void VerifiesNonzeroSpeedSeedIsStrictlyFeasible()
|
||||
{
|
||||
LongitudinalPlanningInput input = CreateFakeInput(out _);
|
||||
var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
|
||||
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(
|
||||
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
Verification.True(solver.WarmStarts.Count > 0,
|
||||
"nonzero-speed seed reaches the ST solver: " + result.Status + " " + result.FailureReason);
|
||||
LongitudinalCandidate seed = FromPrimal(times, solver.WarmStarts[0]);
|
||||
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "nonzero-speed seed envelope: " + speedFailure);
|
||||
Verification.True(new LongitudinalSolutionValidator().TryValidate(input, envelope, seed, out _,
|
||||
out string validationFailure), "nonzero-speed seed is strictly feasible: " + validationFailure);
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"strictly validated initial seed survives an immediate solver timeout");
|
||||
Verification.True(result.Candidate != null,
|
||||
"strictly validated initial seed is retained as the timeout fallback");
|
||||
}
|
||||
|
||||
private static void VerifiesValidatedEndpointsAreCanonical()
|
||||
{
|
||||
LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
|
||||
double toleranceOffset = 0.5d * input.Configuration.Validation.KinematicTolerance;
|
||||
double[] progress = new double[valid.S.Count];
|
||||
double[] speed = new double[valid.U.Count];
|
||||
for (int index = 0; index < progress.Length; index++)
|
||||
{
|
||||
progress[index] = valid.S[index];
|
||||
speed[index] = valid.U[index];
|
||||
}
|
||||
progress[progress.Length - 1] += toleranceOffset;
|
||||
speed[speed.Length - 1] += toleranceOffset;
|
||||
var toleranceCandidate = new LongitudinalCandidate(valid.KnotTimes, progress, speed, valid.A, valid.J);
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
|
||||
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 2d),
|
||||
});
|
||||
|
||||
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"canonical strict candidate remains the timeout fallback");
|
||||
LongitudinalCandidate canonical = result.Candidate ??
|
||||
throw new InvalidOperationException("Canonical fallback candidate was missing.");
|
||||
Verification.Equal(input.StopBoundaryPathS, canonical.S[canonical.S.Count - 1],
|
||||
"validated terminal PathS is canonicalized exactly");
|
||||
Verification.Equal(0d, canonical.U[canonical.U.Count - 1],
|
||||
"validated terminal speed is canonicalized exactly");
|
||||
}
|
||||
|
||||
private static IReadOnlyList<LongitudinalScenario> CreateRealOsqpScenarios()
|
||||
{
|
||||
return new[]
|
||||
{
|
||||
CreateRealScenario("forward", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
|
||||
CreateRealScenario("reverse", TravelDirection.Reverse, 0.50d, 0d, 0d, 0d),
|
||||
CreateRealScenario("curvature-limited", TravelDirection.Forward, 0.35d, 20d, 0d, 0d),
|
||||
CreateRealScenario("jerk-limited-stop", TravelDirection.Forward, 0.50d, 0d, 0.05d, 0d),
|
||||
CreateRealScenario("short-segment", TravelDirection.Forward, 0.05d, 0d, 0d, 0d),
|
||||
CreateRealScenario("zero-start-speed", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
|
||||
};
|
||||
}
|
||||
|
||||
private static LongitudinalScenario CreateRealScenario(string name, TravelDirection direction, double terminalPathS,
|
||||
double middleCurvature, double initialSpeed, double initialAcceleration)
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
||||
configuration.Validation.KinematicTolerance = 1e-5d;
|
||||
var points = new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, terminalPathS * 0.5d, middleCurvature),
|
||||
Point(2d, terminalPathS, 0d),
|
||||
};
|
||||
return new LongitudinalScenario(name, new LongitudinalPlanningInput(new LateralPath(points, true), direction,
|
||||
initialSpeed, initialAcceleration, EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary,
|
||||
configuration, Array.Empty<double>(), Array.Empty<double>()));
|
||||
}
|
||||
|
||||
private static void VerifyRealScenario(LongitudinalScenario scenario, LongitudinalPlanningResult result)
|
||||
{
|
||||
Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
|
||||
scenario.Name + " returns a strict profile: " + result.FailureReason);
|
||||
LongitudinalCandidate candidate = result.Candidate ?? throw new InvalidOperationException(scenario.Name + " candidate missing.");
|
||||
Verification.True(candidate.S[candidate.S.Count - 1] <= scenario.Input.PathUpperBoundS,
|
||||
scenario.Name + " remains inside the PathS window");
|
||||
PathSpeedLimitBuilder builder = new PathSpeedLimitBuilder();
|
||||
EmPlanningStatus speedStatus = builder.Build(scenario.Input, out PathSpeedLimit envelope, out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, scenario.Name + " envelope: " + speedFailure);
|
||||
Verification.True(new LongitudinalSolutionValidator().TryValidate(scenario.Input, envelope, candidate,
|
||||
out _, out string validationFailure), scenario.Name + " strict physical validation: " + validationFailure);
|
||||
}
|
||||
|
||||
private static void VerifyDeterministicResult(string name, LongitudinalPlanningResult first, LongitudinalPlanningResult second)
|
||||
{
|
||||
Verification.Equal(first.Status, second.Status, name + " deterministic status");
|
||||
LongitudinalCandidate left = first.Candidate ?? throw new InvalidOperationException(name + " first candidate missing.");
|
||||
LongitudinalCandidate right = second.Candidate ?? throw new InvalidOperationException(name + " second candidate missing.");
|
||||
for (int index = 0; index < left.S.Count; index++)
|
||||
{
|
||||
Verification.NearlyEqual(left.S[index], right.S[index], name + " deterministic S " + index);
|
||||
Verification.NearlyEqual(left.U[index], right.U[index], name + " deterministic U " + index);
|
||||
Verification.NearlyEqual(left.A[index], right.A[index], name + " deterministic A " + index);
|
||||
}
|
||||
for (int index = 0; index < left.J.Count; index++)
|
||||
Verification.NearlyEqual(left.J[index], right.J[index], name + " deterministic J " + index);
|
||||
}
|
||||
|
||||
private static LongitudinalPlanningInput CreateFakeInput(out LongitudinalCandidate valid)
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 1d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.25d;
|
||||
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 4d;
|
||||
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(1d, 0.25d);
|
||||
valid = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0d,
|
||||
new[] { -0.8d, 0d, 0.8d, 0d });
|
||||
double terminalPathS = valid.S[valid.S.Count - 1];
|
||||
var path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
Point(1d, terminalPathS * 0.5d, 0d),
|
||||
Point(2d, terminalPathS, 0d),
|
||||
}, true);
|
||||
return new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d, EmTerminalType.Goal,
|
||||
EmLongitudinalMode.ExactStopAtBoundary, configuration, Array.Empty<double>(), Array.Empty<double>());
|
||||
}
|
||||
|
||||
private static EmPlannerConfiguration CreateExactStopSeedConfiguration()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 0.40d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
|
||||
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static LateralPathPoint Point(double referenceS, double pathS, double curvature)
|
||||
{
|
||||
return new LateralPathPoint(referenceS, pathS, 0d, 0d, 0d, 0d, pathS, 0d, 0d, curvature, curvature, 0d);
|
||||
}
|
||||
|
||||
private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal, double objective,
|
||||
double primalResidual = 0d, double dualResidual = 0d)
|
||||
{
|
||||
return new QpSolveResult(status, primal, objective, primalResidual, dualResidual, 1, TimeSpan.Zero,
|
||||
status.ToString(), string.Empty);
|
||||
}
|
||||
|
||||
private static double[] ToPrimal(LongitudinalCandidate candidate)
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
|
||||
var primal = new double[layout.VariableCount];
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
primal[layout.S(index)] = candidate.S[index];
|
||||
primal[layout.U(index)] = candidate.U[index];
|
||||
primal[layout.A(index)] = candidate.A[index];
|
||||
}
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
primal[layout.J(index)] = candidate.J[index];
|
||||
return primal;
|
||||
}
|
||||
|
||||
private static LongitudinalCandidate FromPrimal(IReadOnlyList<double> times, IReadOnlyList<double> primal)
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(times.Count);
|
||||
var s = new double[layout.KnotCount];
|
||||
var u = new double[layout.KnotCount];
|
||||
var a = new double[layout.KnotCount];
|
||||
var j = new double[layout.KnotCount - 1];
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
s[index] = primal[layout.S(index)];
|
||||
u[index] = primal[layout.U(index)];
|
||||
a[index] = primal[layout.A(index)];
|
||||
}
|
||||
for (int index = 0; index < j.Length; index++)
|
||||
j[index] = primal[layout.J(index)];
|
||||
return new LongitudinalCandidate(times, s, u, a, j);
|
||||
}
|
||||
|
||||
private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
|
||||
{
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
int found = 0;
|
||||
double coefficient = 0d;
|
||||
for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
|
||||
{
|
||||
for (int index = problem.ConstraintMatrix.ColumnPointers[column];
|
||||
index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (problem.ConstraintMatrix.RowIndices[index] == row)
|
||||
{
|
||||
found++;
|
||||
if (column == variable)
|
||||
coefficient = problem.ConstraintMatrix.Values[index];
|
||||
}
|
||||
}
|
||||
}
|
||||
if (found == 1 && Math.Abs(coefficient - 1d) <= 1e-12d)
|
||||
{
|
||||
lower = problem.LowerBounds[row];
|
||||
upper = problem.UpperBounds[row];
|
||||
return;
|
||||
}
|
||||
}
|
||||
throw new InvalidOperationException("No single-variable bounds were found for ST variable " + variable + ".");
|
||||
}
|
||||
|
||||
private sealed class LongitudinalScenario
|
||||
{
|
||||
public LongitudinalScenario(string name, LongitudinalPlanningInput input)
|
||||
{
|
||||
Name = name;
|
||||
Input = input;
|
||||
}
|
||||
|
||||
public string Name { get; }
|
||||
|
||||
public LongitudinalPlanningInput Input { get; }
|
||||
}
|
||||
}
|
||||
@@ -1,988 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class LongitudinalModelChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesJerkLimitedStoppingProfileEndsAtRest();
|
||||
VerifiesStoppedReachabilityUsesTheSameJerkModel();
|
||||
VerifiesRollingEnvelopeDoesNotStopAtWindowEnd();
|
||||
VerifiesFinitePathSIndexedSpeedEnvelope();
|
||||
VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
|
||||
VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath();
|
||||
VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
|
||||
VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
|
||||
VerifiesFullDirectionScopeSelectsActualSegmentBoundary();
|
||||
VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints();
|
||||
VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics();
|
||||
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
|
||||
VerifiesModeSpecificSolutionValidation();
|
||||
VerifiesFullDirectionNoProgressValidation();
|
||||
VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
|
||||
}
|
||||
|
||||
private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
|
||||
{
|
||||
Verification.True(JerkLimitedStoppingMath.TryCalculate(
|
||||
0.20d, 0d, 0.30d, 0.50d,
|
||||
out JerkLimitedStoppingProfile profile, out string failure),
|
||||
"jerk-limited stop builds: " + failure);
|
||||
Verification.True(profile.DistanceMeters > 0d, "stop distance is positive");
|
||||
Verification.True(profile.DurationSeconds > 0d, "stop duration is positive");
|
||||
Verification.NearlyEqual(0d, profile.FinalSpeedMetersPerSecond,
|
||||
"stop ends at zero speed");
|
||||
Verification.NearlyEqual(0d, profile.FinalAccelerationMetersPerSecondSquared,
|
||||
"stop releases acceleration to zero");
|
||||
|
||||
Verification.True(JerkLimitedStoppingMath.TryCalculate(
|
||||
0.20d, 0.20d, 0.30d, 0.50d,
|
||||
out JerkLimitedStoppingProfile accelerating, out failure),
|
||||
"positive-acceleration stop builds: " + failure);
|
||||
Verification.True(accelerating.DistanceMeters > profile.DistanceMeters,
|
||||
"positive initial acceleration needs more stopping distance");
|
||||
Verification.NearlyEqual(0d, accelerating.FinalAccelerationMetersPerSecondSquared,
|
||||
"positive-acceleration stop also releases acceleration");
|
||||
}
|
||||
|
||||
private static void VerifiesStoppedReachabilityUsesTheSameJerkModel()
|
||||
{
|
||||
double maximumDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
|
||||
0d, 0d, 0.20d, 0.20d, 0.30d, 0.50d, 2d);
|
||||
Verification.True(maximumDistance > 0d && maximumDistance < 0.40d,
|
||||
"two-second stopped reach is finite and below unconstrained cruise distance");
|
||||
|
||||
double cap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
|
||||
maximumDistance, 0.20d, 0.30d, 0.50d, 0.20d);
|
||||
Verification.True(cap >= 0d && cap <= 0.20d,
|
||||
"distance inversion stays inside the direction speed range");
|
||||
}
|
||||
|
||||
private static void VerifiesRollingEnvelopeDoesNotStopAtWindowEnd()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
LateralPath path = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(1d, 1d, 0d, 0d),
|
||||
});
|
||||
var rolling = new LongitudinalPlanningInput(path, TravelDirection.Forward,
|
||||
0d, 0d, EmTerminalType.RollingSafetyStop,
|
||||
EmLongitudinalMode.RollingContinuation, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(
|
||||
rolling, out PathSpeedLimit envelope, out string failure);
|
||||
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "rolling envelope: " + failure);
|
||||
Verification.True(envelope.MaximumSpeedAt(rolling.PathUpperBoundS) > 0d,
|
||||
"rolling window end keeps a nonzero speed allowance");
|
||||
Verification.True(!envelope.HasStopBoundary, "rolling envelope has no stop boundary");
|
||||
Verification.NearlyEqual(rolling.PathUpperBoundS, envelope.PathUpperBoundS,
|
||||
"rolling envelope reports its PathS upper bound");
|
||||
|
||||
var approach = new LongitudinalPlanningInput(path, TravelDirection.Forward,
|
||||
0d, 0d, EmTerminalType.Goal,
|
||||
EmLongitudinalMode.ApproachStopBoundary, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
status = new PathSpeedLimitBuilder().Build(approach, out PathSpeedLimit approachEnvelope, out failure);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "approach envelope: " + failure);
|
||||
Verification.True(approachEnvelope.HasStopBoundary, "approach envelope retains its real stop boundary");
|
||||
Verification.NearlyEqual(0d, approachEnvelope.StoppingLimitAt(approach.StopBoundaryPathS),
|
||||
"approach stop boundary has an exact zero stopping limit");
|
||||
}
|
||||
|
||||
private static void VerifiesFinitePathSIndexedSpeedEnvelope()
|
||||
{
|
||||
LateralPath directionPath = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(1d, 1d, 0d, 0d),
|
||||
});
|
||||
var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, EmPlannerConfiguration.CreateDefault(),
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput,
|
||||
out PathSpeedLimit directionEnvelope, out string directionFailureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
|
||||
directionFailureReason);
|
||||
Verification.NearlyEqual(1d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
|
||||
"default direction speed limit");
|
||||
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
LateralPath path = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(10d, 0d, 0d, 0d),
|
||||
new PathFixture(20d, 2d, 2d, 4d),
|
||||
new PathFixture(20.5d, 4d, 20d, 0d),
|
||||
new PathFixture(21d, 5d, 0d, 0d),
|
||||
});
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "speed envelope status: " + failureReason);
|
||||
Verification.NearlyEqual(1d, envelope.DirectionMaximumSpeedMetersPerSecond, "overridden direction speed limit");
|
||||
Verification.NearlyEqual(Math.Sqrt(0.20d / 2d), envelope.LateralAccelerationLimitAt(2d),
|
||||
"curvature lateral-acceleration limit");
|
||||
Verification.NearlyEqual(0.50d / 4d, envelope.CurvatureRateLimitAt(2d), "curvature-rate limit");
|
||||
Verification.True(double.IsFinite(envelope.LateralAccelerationLimitAt(0d)) &&
|
||||
double.IsFinite(envelope.CurvatureRateLimitAt(0d)), "zero curvature limits stay finite");
|
||||
Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 4d), envelope.StoppingLimitAt(4d),
|
||||
"stopping speed limit uses the complete jerk-limited model");
|
||||
Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
|
||||
"combined limit chooses the finite minimum");
|
||||
double interpolationQueryPathS = 1.013d;
|
||||
int upperStation = 1;
|
||||
while (envelope.PathS[upperStation] < interpolationQueryPathS)
|
||||
upperStation++;
|
||||
double lowerPathS = envelope.PathS[upperStation - 1];
|
||||
double upperPathS = envelope.PathS[upperStation];
|
||||
double fraction = (interpolationQueryPathS - lowerPathS) / (upperPathS - lowerPathS);
|
||||
double expectedInterpolatedSpeed = envelope.MaximumSpeedMetersPerSecond[upperStation - 1] +
|
||||
(envelope.MaximumSpeedMetersPerSecond[upperStation] - envelope.MaximumSpeedMetersPerSecond[upperStation - 1]) *
|
||||
fraction;
|
||||
Verification.NearlyEqual(expectedInterpolatedSpeed, envelope.MaximumSpeedAt(interpolationQueryPathS),
|
||||
"speed envelope interpolates by PathS rather than ReferenceS");
|
||||
Verification.NearlyEqual(0d, envelope.MaximumSpeedAt(5d), "terminal speed is exactly zero");
|
||||
}
|
||||
|
||||
private static void VerifiesStoppingEnvelopeIsRefinedOnActualPathS()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
LateralPath path = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(100d, 2d, 0d, 0d),
|
||||
});
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "refined stopping envelope status: " + failureReason);
|
||||
Verification.True(envelope.PathS.Count > path.Points.Count, "stopping envelope inserts actual-PathS refinement stations");
|
||||
Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 1.5d), envelope.MaximumSpeedAt(1.5d),
|
||||
"refined stopping envelope uses the complete jerk-limited stopping cap");
|
||||
}
|
||||
|
||||
private static void VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
LateralPath path = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(100d, 2d, 0d, 0d),
|
||||
});
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "jerk-limited stopping-tail status: " + failureReason);
|
||||
double nearBoundaryPathS = input.StopBoundaryPathS - 0.005d;
|
||||
Verification.True(envelope.StoppingLimitAt(nearBoundaryPathS) > 0d &&
|
||||
envelope.StoppingLimitAt(nearBoundaryPathS) <=
|
||||
MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS) + 1e-12d,
|
||||
"near-boundary speed cap conservatively interpolates jerk-limited distance inversion");
|
||||
Verification.NearlyEqual(0d, envelope.StoppingLimitAt(input.StopBoundaryPathS),
|
||||
"real stop boundary keeps an exact zero stopping cap");
|
||||
}
|
||||
|
||||
private static void VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
LateralPath shortPath = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(100d, 0.01d, 0d, 0d),
|
||||
});
|
||||
var rolling = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(rolling, out PathSpeedLimit envelope,
|
||||
out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status,
|
||||
"rolling windows do not require a stop inside their local PathS extent: " + failureReason);
|
||||
Verification.True(envelope != null, "rolling speed envelope is created despite the short local window");
|
||||
|
||||
var approach = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
status = new PathSpeedLimitBuilder().Build(approach, out envelope, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status,
|
||||
"real stop-boundary jerk/deceleration stopping precheck status");
|
||||
Verification.True(envelope == null, "stopping-distance failure does not create a speed envelope");
|
||||
Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
|
||||
}
|
||||
|
||||
private static void VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.DistanceHorizonMeters = 1d;
|
||||
configuration.Scheduling.TimeHorizonSeconds = 2d;
|
||||
|
||||
DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal);
|
||||
EmPlanningStatus status = new PlanningHorizonSelector().Select(
|
||||
longSegment, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
|
||||
out PlanningHorizonSelection rolling, out string failure);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
|
||||
Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
|
||||
"distance horizon defines the LS window");
|
||||
Verification.Equal(EmLongitudinalMode.RollingContinuation,
|
||||
rolling.LongitudinalMode, "far boundary rolls");
|
||||
|
||||
DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
|
||||
status = new PlanningHorizonSelector().Select(
|
||||
visibleButFar, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
|
||||
out PlanningHorizonSelection approach, out failure);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
|
||||
Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
|
||||
approach.LongitudinalMode, "visible unreachable boundary approaches");
|
||||
|
||||
DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
|
||||
status = new PlanningHorizonSelector().Select(
|
||||
reachableGoal, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
|
||||
out PlanningHorizonSelection exact, out failure);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
|
||||
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
|
||||
exact.LongitudinalMode, "reachable goal stops exactly");
|
||||
Verification.Equal(EmTerminalType.Goal, exact.TerminalType,
|
||||
"exact stop preserves Goal identity");
|
||||
|
||||
IReadOnlyList<double> regularTimes = LongitudinalCandidate.CreateKnotTimes(2d, 0.1d);
|
||||
Verification.Equal(19, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
regularTimes, 0.1d), "regular exact stop reserves t=1.9..2.0");
|
||||
Verification.Equal(1, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
new[] { 0d, 0.1d, 0.2d, 0.25d }, 0.1d),
|
||||
"short final interval moves the stop anchor earlier");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionScopeSelectsActualSegmentBoundary()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.DistanceHorizonMeters = 1d;
|
||||
DirectionSegmentView tenMeterGoal = CreateSegment(10d, EmBoundaryType.Goal);
|
||||
var selector = new PlanningHorizonSelector();
|
||||
EmPlanningStatus status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
|
||||
EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection full, out string failure);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "full selection: " + failure);
|
||||
Verification.NearlyEqual(10d, full.WindowEndReferenceS, "full selection reaches actual segment end");
|
||||
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary, full.LongitudinalMode,
|
||||
"full selection stops at its real boundary");
|
||||
Verification.True(full.HasStopBoundary, "full selection retains the real stop boundary");
|
||||
|
||||
status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
|
||||
EmPlanningScope.RollingHorizon, configuration, out PlanningHorizonSelection rolling, out failure);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
|
||||
Verification.NearlyEqual(4d, rolling.WindowEndReferenceS, "rolling selection retains the one-metre window");
|
||||
|
||||
DirectionSegmentView gearSwitch = CreateSegment(10d, EmBoundaryType.GearSwitchApproach);
|
||||
status = selector.Select(gearSwitch, 3d, 0d, 0d,
|
||||
EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection gear, out failure);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "gear full selection: " + failure);
|
||||
Verification.Equal(EmTerminalType.GearSwitch, gear.TerminalType, "gear full selection preserves switch terminal");
|
||||
Verification.NearlyEqual(10d, gear.WindowEndReferenceS, "gear full selection stops before the next segment");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 10d;
|
||||
configuration.Scheduling.DistanceHorizonMeters = 0.25d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
|
||||
configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationKnotCount = 401;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 0.50d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 0.50d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
|
||||
|
||||
LateralPath shortPath = CreateStraightPath(0.50d);
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(shortPath, TravelDirection.Forward, 0.10d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit shortLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "short full-segment envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(shortPath, shortLimit, 0.10d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule shortSchedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "short full-segment schedule: " + failureReason);
|
||||
Verification.True(shortSchedule.TotalDurationSeconds < 10d, "short segment derives its own T_end");
|
||||
|
||||
LateralPath longPath = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(1.50d, 1.50d, 2d, 0d),
|
||||
new PathFixture(3d, 3d, 0d, 0d),
|
||||
});
|
||||
status = new PathSpeedLimitBuilder().Build(longPath, TravelDirection.Forward, 0.10d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit longLimit, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "long full-segment envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule longSchedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "long full-segment schedule: " + failureReason);
|
||||
Verification.True(longSchedule.TotalDurationSeconds > shortSchedule.TotalDurationSeconds,
|
||||
"duration grows from s_end and limits");
|
||||
Verification.True(longSchedule.KnotTimes.Count <= configuration.Scheduling.MaximumOptimizationKnotCount,
|
||||
"adaptive schedule respects knot cap");
|
||||
Verification.True(longSchedule.IsAdaptive, "full segment produces an adaptive knot schedule");
|
||||
Verification.True(longSchedule.ReferencePathS.Count > longPath.Points.Count,
|
||||
"curvature and stopping envelopes add schedule breakpoints");
|
||||
Verification.NearlyEqual(longPath.Points[longPath.Points.Count - 1].PathS,
|
||||
longSchedule.ReferencePathS[longSchedule.ReferencePathS.Count - 1], "schedule reaches s_end");
|
||||
Verification.NearlyEqual(0d,
|
||||
longSchedule.ReferenceSpeedMetersPerSecond[longSchedule.ReferenceSpeedMetersPerSecond.Count - 1],
|
||||
"schedule stops at s_end");
|
||||
|
||||
EmPlannerConfiguration constrained = configuration.Copy();
|
||||
constrained.Scheduling.MaximumOptimizationKnotCount = 4;
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
|
||||
constrained.Longitudinal.DesiredForwardSpeedMetersPerSecond, constrained,
|
||||
out LongitudinalKnotSchedule rejected, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, status,
|
||||
"undersized full-segment knot cap rejects rather than truncates");
|
||||
Verification.True(rejected == null, "resource rejection produces no partial schedule");
|
||||
Verification.True(failureReason.IndexOf("required", StringComparison.OrdinalIgnoreCase) >= 0 &&
|
||||
failureReason.IndexOf("configured", StringComparison.OrdinalIgnoreCase) >= 0,
|
||||
"resource rejection reports required and configured knots");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
|
||||
configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
|
||||
configuration.Scheduling.MaximumOptimizationKnotCount = 401;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
|
||||
LateralPath path = CreateStraightPath(0.0075d);
|
||||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d,
|
||||
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference envelope: " + failureReason);
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
|
||||
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
||||
out LongitudinalKnotSchedule schedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference schedule: " + failureReason);
|
||||
|
||||
Verification.True(typeof(LongitudinalKnotSchedule).GetProperty("ReferenceCandidate") == null,
|
||||
"adaptive schedule is only a knot/reference/hold contract");
|
||||
Verification.True(schedule.TerminalHoldStartIndex > 0 &&
|
||||
schedule.TerminalHoldStartIndex < schedule.KnotTimes.Count,
|
||||
"adaptive reference explicitly identifies its terminal hold boundary");
|
||||
Verification.True(schedule.TerminalHoldStartIndex >= 3,
|
||||
"adaptive exact-stop schedule reserves three independent motion jerk intervals");
|
||||
LongitudinalCandidate strictProjection = CreateStrictNonuniformExactStopCandidate();
|
||||
var projectionSchedule = new LongitudinalKnotSchedule(strictProjection.KnotTimes,
|
||||
new[] { 0d, 0.003d, 0.006d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0.01d, 0d, 0d }, true, 3);
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
EmPlanningScope.FullDirectionSegment, projectionSchedule, Array.Empty<double>(), Array.Empty<double>());
|
||||
Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit, strictProjection,
|
||||
out _, out failureReason), "nonuniform strict projection fixture is physically feasible: " + failureReason);
|
||||
|
||||
var constraintBuilder = new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder());
|
||||
Verification.True(constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit,
|
||||
out QuadraticProgram projectionProblem, out failureReason),
|
||||
"full exact-stop feasibility projection builds: " + failureReason);
|
||||
var layout = new LongitudinalVariableLayout(projectionSchedule.KnotTimes.Count);
|
||||
Verification.True(Math.Abs(projectionProblem.LinearCost[layout.S(1)]) > 1e-12d,
|
||||
"feasibility projection tracks scheduled PathS");
|
||||
Verification.True(Math.Abs(projectionProblem.LinearCost[layout.U(1)]) > 1e-12d,
|
||||
"feasibility projection tracks scheduled speed");
|
||||
Verification.Equal(9 * layout.KnotCount - 3 +
|
||||
3 * (layout.KnotCount - projectionSchedule.TerminalHoldStartIndex), projectionProblem.ConstraintCount,
|
||||
"feasibility projection carries a PathS-linearized speed-envelope row for each motion knot");
|
||||
|
||||
var initialTimeoutSolver = new FakeQpSolver(new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d,
|
||||
0d, 0, TimeSpan.Zero, "time limit", string.Empty));
|
||||
LongitudinalPlanningResult initialTimeout = new SequentialLongitudinalOptimizer(initialTimeoutSolver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SolverTimedOut, initialTimeout.Status,
|
||||
"initial feasibility timeout cannot publish a fallback");
|
||||
Verification.True(initialTimeout.Candidate == null, "initial feasibility timeout publishes no candidate");
|
||||
|
||||
var solver = new FakeQpSolver(new[]
|
||||
{
|
||||
new QpSolveResult(QpSolveStatus.Solved, ToPrimal(strictProjection), 0d, 0d, 0d, 1,
|
||||
TimeSpan.Zero, "solved", string.Empty),
|
||||
new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d, 0d, 0,
|
||||
TimeSpan.Zero, "time limit", string.Empty),
|
||||
});
|
||||
LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
|
||||
CancellationToken.None);
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"strict feasibility projection permits a later exact-stop fallback: " + result.FailureReason);
|
||||
Verification.Equal(2, solver.SolveCallCount,
|
||||
"full scope consumes strict feasibility projection before the objective timeout");
|
||||
Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit,
|
||||
result.Candidate ?? throw new InvalidOperationException("Adaptive fallback was missing."), out _,
|
||||
out failureReason), "adaptive fallback is strict-feasible: " + failureReason);
|
||||
|
||||
EmPlannerConfiguration denserPublication = configuration.Copy();
|
||||
denserPublication.Scheduling.OutputTimeStepSeconds = 0.05d;
|
||||
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
|
||||
denserPublication.Longitudinal.DesiredForwardSpeedMetersPerSecond, denserPublication,
|
||||
out LongitudinalKnotSchedule sameOptimizationSchedule, out failureReason);
|
||||
Verification.Equal(EmPlanningStatus.Success, status, "independent-cadence schedule: " + failureReason);
|
||||
Verification.Equal(schedule.KnotTimes.Count, sameOptimizationSchedule.KnotTimes.Count,
|
||||
"publication cadence does not change adaptive knot count");
|
||||
Verification.Equal(schedule.TerminalHoldStartIndex, sameOptimizationSchedule.TerminalHoldStartIndex,
|
||||
"publication cadence does not change the terminal hold boundary");
|
||||
}
|
||||
|
||||
private static LongitudinalCandidate CreateStrictNonuniformExactStopCandidate()
|
||||
{
|
||||
double[] times = { 0d, 0.09d, 0.19d, 0.30d, 0.50d };
|
||||
double[] motionTimes = { 0d, 0.09d, 0.19d, 0.30d };
|
||||
var influence = new double[3, 3];
|
||||
for (int interval = 0; interval < 3; interval++)
|
||||
{
|
||||
var basis = new double[3];
|
||||
basis[interval] = 1d;
|
||||
LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
|
||||
int last = response.S.Count - 1;
|
||||
influence[0, interval] = response.A[last];
|
||||
influence[1, interval] = response.U[last];
|
||||
influence[2, interval] = response.S[last];
|
||||
}
|
||||
double[] jerkMotion = SolveThreeByThree(influence, new[] { 0d, -0.05d, -0.0075d });
|
||||
var jerk = new[] { jerkMotion[0], jerkMotion[1], jerkMotion[2], 0d };
|
||||
LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.05d, 0d, jerk);
|
||||
var pathS = new[] { integrated.S[0], integrated.S[1], integrated.S[2], 0.0075d, 0.0075d };
|
||||
var speed = new[] { integrated.U[0], integrated.U[1], integrated.U[2], 0d, 0d };
|
||||
var acceleration = new[] { integrated.A[0], integrated.A[1], integrated.A[2], 0d, 0d };
|
||||
return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
|
||||
}
|
||||
|
||||
private static double[] ToPrimal(LongitudinalCandidate candidate)
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
|
||||
var primal = new double[layout.VariableCount];
|
||||
for (int index = 0; index < layout.KnotCount; index++)
|
||||
{
|
||||
primal[layout.S(index)] = candidate.S[index];
|
||||
primal[layout.U(index)] = candidate.U[index];
|
||||
primal[layout.A(index)] = candidate.A[index];
|
||||
}
|
||||
for (int index = 0; index < layout.KnotCount - 1; index++)
|
||||
primal[layout.J(index)] = candidate.J[index];
|
||||
return primal;
|
||||
}
|
||||
|
||||
private static double[] SolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide)
|
||||
{
|
||||
var augmented = new double[3, 4];
|
||||
for (int row = 0; row < 3; row++)
|
||||
{
|
||||
for (int column = 0; column < 3; column++)
|
||||
augmented[row, column] = matrix[row, column];
|
||||
augmented[row, 3] = rightHandSide[row];
|
||||
}
|
||||
for (int pivot = 0; pivot < 3; pivot++)
|
||||
{
|
||||
int bestRow = pivot;
|
||||
for (int row = pivot + 1; row < 3; row++)
|
||||
{
|
||||
if (Math.Abs(augmented[row, pivot]) > Math.Abs(augmented[bestRow, pivot]))
|
||||
bestRow = row;
|
||||
}
|
||||
for (int column = pivot; column < 4; column++)
|
||||
{
|
||||
double temporary = augmented[pivot, column];
|
||||
augmented[pivot, column] = augmented[bestRow, column];
|
||||
augmented[bestRow, column] = temporary;
|
||||
}
|
||||
double divisor = augmented[pivot, pivot];
|
||||
for (int column = pivot; column < 4; column++)
|
||||
augmented[pivot, column] /= divisor;
|
||||
for (int row = 0; row < 3; row++)
|
||||
{
|
||||
if (row == pivot)
|
||||
continue;
|
||||
double factor = augmented[row, pivot];
|
||||
for (int column = pivot; column < 4; column++)
|
||||
augmented[row, column] -= factor * augmented[pivot, column];
|
||||
}
|
||||
}
|
||||
return new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
|
||||
}
|
||||
|
||||
private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
|
||||
{
|
||||
var layout = new LongitudinalVariableLayout(5);
|
||||
Verification.Equal(19, layout.VariableCount, "ST variable count");
|
||||
for (int index = 0; index < 5; index++)
|
||||
{
|
||||
Verification.Equal(index, layout.S(index), "s index " + index);
|
||||
Verification.Equal(5 + index, layout.U(index), "u index " + index);
|
||||
Verification.Equal(10 + index, layout.A(index), "a index " + index);
|
||||
}
|
||||
for (int index = 0; index < 4; index++)
|
||||
Verification.Equal(15 + index, layout.J(index), "j index " + index);
|
||||
|
||||
double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d };
|
||||
double[] jerk = { 0.30d, -0.10d, 0.20d, -0.20d };
|
||||
LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0.02d, jerk);
|
||||
for (int index = 0; index < jerk.Length; index++)
|
||||
{
|
||||
double dt = times[index + 1] - times[index];
|
||||
Verification.NearlyEqual(integrated.A[index] + dt * integrated.J[index], integrated.A[index + 1],
|
||||
"exact ST acceleration dynamics " + index);
|
||||
Verification.NearlyEqual(integrated.U[index] + dt * integrated.A[index] + 0.5d * dt * dt * integrated.J[index],
|
||||
integrated.U[index + 1], "exact ST speed dynamics " + index);
|
||||
Verification.NearlyEqual(integrated.S[index] + dt * integrated.U[index] +
|
||||
0.5d * dt * dt * integrated.A[index] + dt * dt * dt * integrated.J[index] / 6d,
|
||||
integrated.S[index + 1], "exact ST progress dynamics " + index);
|
||||
}
|
||||
Verification.True(integrated.SatisfiesExactDiscreteDynamics(1e-12d), "integrated ST candidate validates dynamics");
|
||||
|
||||
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
||||
LateralPath path = CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(1d, 1d, 0d, 0d),
|
||||
new PathFixture(2d, 2d, 0d, 0d),
|
||||
});
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
|
||||
new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
|
||||
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
|
||||
out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "unit-scale speed envelope: " + speedFailure);
|
||||
|
||||
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, envelope,
|
||||
integrated, out QuadraticProgram problem, out string failureReason), "ST QP builds: " + failureReason);
|
||||
var rollingInput = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
|
||||
new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
|
||||
EmPlanningStatus rollingSpeedStatus = new PathSpeedLimitBuilder().Build(rollingInput,
|
||||
out PathSpeedLimit rollingEnvelope, out string rollingSpeedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, rollingSpeedStatus,
|
||||
"unit-scale rolling speed envelope: " + rollingSpeedFailure);
|
||||
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(
|
||||
rollingInput, rollingEnvelope, integrated, out QuadraticProgram rollingProblem, out string rollingFailure),
|
||||
"rolling ST QP builds: " + rollingFailure);
|
||||
Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.U(0), layout.U(0)),
|
||||
"normalized speed and previous-U P coefficient");
|
||||
Verification.NearlyEqual(2d, MatrixValue(problem.UpperTriangularP, layout.A(0), layout.A(0)),
|
||||
"normalized acceleration P coefficient");
|
||||
Verification.NearlyEqual(20d, MatrixValue(problem.UpperTriangularP, layout.J(0), layout.J(0)),
|
||||
"normalized jerk P coefficient");
|
||||
Verification.NearlyEqual(2.5d, MatrixValue(problem.UpperTriangularP, layout.S(0), layout.S(0)),
|
||||
"normalized previous-S P coefficient");
|
||||
Verification.NearlyEqual(0d, MatrixValue(problem.UpperTriangularP, layout.S(4), layout.S(4)),
|
||||
"fixed terminal S has no progress-reward coefficient");
|
||||
|
||||
FindSingleVariableBounds(problem, layout.S(0), out double sLower, out double sUpper);
|
||||
Verification.NearlyEqual(0d, sLower, "S lower bound");
|
||||
Verification.NearlyEqual(2d, sUpper, "S upper bound");
|
||||
FindSingleVariableBounds(problem, layout.U(1), out double uLower, out double uUpper);
|
||||
Verification.NearlyEqual(0d, uLower, "U nonnegative bound");
|
||||
Verification.NearlyEqual(input.DirectionMaximumSpeedMetersPerSecond, uUpper,
|
||||
"U retains its direction hard bound alongside the PathS envelope");
|
||||
int envelopeSegment = 0;
|
||||
while (envelopeSegment < envelope.PathS.Count - 2 && integrated.S[1] > envelope.PathS[envelopeSegment + 1])
|
||||
envelopeSegment++;
|
||||
double envelopeSlope = (envelope.MaximumSpeedMetersPerSecond[envelopeSegment + 1] -
|
||||
envelope.MaximumSpeedMetersPerSecond[envelopeSegment]) /
|
||||
(envelope.PathS[envelopeSegment + 1] - envelope.PathS[envelopeSegment]);
|
||||
double envelopeIntercept = envelope.MaximumSpeedMetersPerSecond[envelopeSegment] -
|
||||
envelopeSlope * envelope.PathS[envelopeSegment];
|
||||
var envelopeRow = new Dictionary<int, double> { { layout.U(1), 1d } };
|
||||
if (Math.Abs(envelopeSlope) > 1e-12d)
|
||||
envelopeRow.Add(layout.S(1), -envelopeSlope);
|
||||
Verification.Equal(1, CountBoundedRow(problem, envelopeRow,
|
||||
-QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
|
||||
"U upper bound linearly re-evaluates the actual PathS envelope");
|
||||
FindSingleVariableBounds(problem, layout.A(1), out double aLower, out double aUpper);
|
||||
Verification.NearlyEqual(-1d, aLower, "deceleration lower bound");
|
||||
Verification.NearlyEqual(1d, aUpper, "acceleration upper bound");
|
||||
FindSingleVariableBounds(problem, layout.J(1), out double jLower, out double jUpper);
|
||||
Verification.NearlyEqual(-1d, jLower, "jerk lower bound");
|
||||
Verification.NearlyEqual(1d, jUpper, "jerk upper bound");
|
||||
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.S(0), 1d } }, 0d),
|
||||
"exact initial S");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.U(0), 1d } }, 0.10d),
|
||||
"exact initial U");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.A(0), 1d } }, 0.02d),
|
||||
"exact initial A");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||||
new Dictionary<int, double> { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS),
|
||||
"rolling has no exact terminal S");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||||
new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
|
||||
"rolling has no exact terminal U");
|
||||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||||
new Dictionary<int, double> { { layout.A(4), 1d } }, 0d),
|
||||
"rolling has no exact terminal A");
|
||||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
integrated.KnotTimes, configuration.Scheduling.OutputTimeStepSeconds);
|
||||
for (int index = stabilizationStart; index < layout.KnotCount; index++)
|
||||
{
|
||||
Verification.Equal(1, CountExactEqualityRows(problem,
|
||||
new Dictionary<int, double> { { layout.S(index), 1d } }, input.StopBoundaryPathS),
|
||||
"stop tail exact S " + index);
|
||||
Verification.Equal(1, CountExactEqualityRows(problem,
|
||||
new Dictionary<int, double> { { layout.U(index), 1d } }, 0d),
|
||||
"stop tail exact U " + index);
|
||||
Verification.Equal(1, CountExactEqualityRows(problem,
|
||||
new Dictionary<int, double> { { layout.A(index), 1d } }, 0d),
|
||||
"stop tail exact A " + index);
|
||||
}
|
||||
Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.S(1), 1d }, { layout.S(0), -1d },
|
||||
}, 0d, QuadraticProgram.MaximumFiniteBound), "monotonic S hard constraint");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.A(1), 1d }, { layout.A(0), -1d }, { layout.J(0), -0.05d },
|
||||
}, 0d), "exact ST acceleration equation");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.U(1), 1d }, { layout.U(0), -1d }, { layout.A(0), -0.05d }, { layout.J(0), -0.00125d },
|
||||
}, 0d), "exact ST speed equation");
|
||||
Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
|
||||
{
|
||||
{ layout.S(1), 1d }, { layout.S(0), -1d }, { layout.U(0), -0.05d }, { layout.A(0), -0.00125d },
|
||||
{ layout.J(0), -0.000020833333333333333d },
|
||||
}, 0d), "exact ST progress equation");
|
||||
}
|
||||
|
||||
private static void VerifiesModeSpecificSolutionValidation()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateTaskFourConfiguration();
|
||||
IReadOnlyList<double> exactTimes = LongitudinalCandidate.CreateKnotTimes(0.30d, 0.10d);
|
||||
LongitudinalCandidate nonstationaryExact = LongitudinalCandidate.Integrate(
|
||||
exactTimes, 0d, 0.10d, 0d, new[] { -4d, 0d, 0d });
|
||||
LateralPath exactPath = CreateStraightPath(nonstationaryExact.S[nonstationaryExact.S.Count - 1]);
|
||||
var exactInput = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
var rollingForExact = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(rollingForExact,
|
||||
out PathSpeedLimit rollingEnvelope, out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "exact-validation rolling envelope: " + speedFailure);
|
||||
|
||||
var validator = new LongitudinalSolutionValidator();
|
||||
Verification.True(!validator.TryValidate(exactInput, rollingEnvelope, nonstationaryExact,
|
||||
out _, out string exactFailure), "exact stops reject a nonstationary internal tail");
|
||||
Verification.True(exactFailure.IndexOf("exact stabilized", StringComparison.Ordinal) >= 0,
|
||||
"exact-stop failure identifies the stabilized tail: " + exactFailure);
|
||||
|
||||
LateralPath rollingPath = CreateStraightPath(0.10d);
|
||||
var rollingInput = new LongitudinalPlanningInput(rollingPath, TravelDirection.Forward, 0.10d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
speedStatus = new PathSpeedLimitBuilder().Build(rollingInput, out PathSpeedLimit openEnvelope, out speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "rolling-validation envelope: " + speedFailure);
|
||||
LongitudinalCandidate rollingCandidate = LongitudinalCandidate.Integrate(
|
||||
exactTimes, 0d, 0.10d, 0d, new[] { 0d, 0d, 0d });
|
||||
Verification.True(validator.TryValidate(rollingInput, openEnvelope, rollingCandidate,
|
||||
out _, out string rollingFailure), "rolling nonzero terminal speed validates: " + rollingFailure);
|
||||
|
||||
EmPlannerConfiguration approachConfiguration = CreateTaskFourConfiguration();
|
||||
approachConfiguration.Scheduling.TimeHorizonSeconds = 0.15d;
|
||||
approachConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
||||
IReadOnlyList<double> approachTimes = LongitudinalCandidate.CreateKnotTimes(0.15d, 0.05d);
|
||||
LongitudinalCandidate unstoppablyFastApproach = LongitudinalCandidate.Integrate(
|
||||
approachTimes, 0d, 0.20d, 0d, new[] { 0d, -10d, 10d });
|
||||
LateralPath approachPath = CreateStraightPath(0.035d);
|
||||
var approachInput = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, approachConfiguration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
var rollingForApproach = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
|
||||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, approachConfiguration,
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
speedStatus = new PathSpeedLimitBuilder().Build(rollingForApproach,
|
||||
out PathSpeedLimit approachEnvelope, out speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "approach-validation rolling envelope: " + speedFailure);
|
||||
Verification.True(!validator.TryValidate(approachInput, approachEnvelope, unstoppablyFastApproach,
|
||||
out _, out string approachFailure), "approach candidates outside the stoppable set are rejected");
|
||||
Verification.True(approachFailure.IndexOf("stoppable set", StringComparison.Ordinal) >= 0,
|
||||
"approach failure identifies the jerk-limited stoppable set");
|
||||
}
|
||||
|
||||
private static void VerifiesFullDirectionNoProgressValidation()
|
||||
{
|
||||
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
|
||||
double[] times = { 0d, 0.10d, 0.20d, 0.30d, 0.50d };
|
||||
var fullSchedule = new LongitudinalKnotSchedule(times,
|
||||
new[] { 0d, 0.04d, 0.08d, 0.10d, 0.10d }, new[] { 0d, 0.20d, 0.10d, 0d, 0d }, true, 3);
|
||||
LateralPath path = CreateStraightPath(0.10d);
|
||||
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
|
||||
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
EmPlanningScope.FullDirectionSegment, fullSchedule, Array.Empty<double>(), Array.Empty<double>());
|
||||
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit speedLimit,
|
||||
out string speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "no-progress envelope: " + speedFailure);
|
||||
LongitudinalCandidate stationary = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d,
|
||||
new[] { 0d, 0d, 0d, 0d });
|
||||
|
||||
var validator = new LongitudinalSolutionValidator();
|
||||
Verification.True(!validator.TryValidate(input, speedLimit, stationary, out _, out EmPlanningStatus failureStatus,
|
||||
out string failureReason), "nonterminal stationary full segment is rejected");
|
||||
Verification.Equal(EmPlanningStatus.NoProgress, failureStatus, "stationary full segment has exact status");
|
||||
Verification.True(failureReason.IndexOf("NoProgress", StringComparison.Ordinal) >= 0,
|
||||
"stationary full segment has exact diagnostic");
|
||||
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input,
|
||||
speedLimit, stationary, out QuadraticProgram fullProblem, out string buildFailure),
|
||||
"full no-progress objective builds: " + buildFailure);
|
||||
var fullLayout = new LongitudinalVariableLayout(times.Length);
|
||||
double expectedReferenceLinearCost = -2d * configuration.Longitudinal.Weights.ReferenceSpeed *
|
||||
fullSchedule.ReferenceSpeedMetersPerSecond[1] /
|
||||
(input.DirectionMaximumSpeedMetersPerSecond * input.DirectionMaximumSpeedMetersPerSecond);
|
||||
Verification.NearlyEqual(expectedReferenceLinearCost, fullProblem.LinearCost[fullLayout.U(1)],
|
||||
"full direction objective tracks adaptive speed reference");
|
||||
|
||||
LateralPath nearTerminalPath = CreateStraightPath(0.02d);
|
||||
var nearTerminalSchedule = new LongitudinalKnotSchedule(times,
|
||||
new[] { 0d, 0.01d, 0.02d, 0.02d, 0.02d }, new[] { 0d, 0d, 0d, 0d, 0d }, true, 3);
|
||||
var nearTerminalInput = new LongitudinalPlanningInput(nearTerminalPath, TravelDirection.Forward, 0d, 0d,
|
||||
EmTerminalType.GearSwitch, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
||||
EmPlanningScope.FullDirectionSegment, nearTerminalSchedule, Array.Empty<double>(), Array.Empty<double>());
|
||||
speedStatus = new PathSpeedLimitBuilder().Build(nearTerminalInput, out PathSpeedLimit nearTerminalLimit,
|
||||
out speedFailure);
|
||||
Verification.Equal(EmPlanningStatus.Success, speedStatus, "near-terminal no-progress envelope: " + speedFailure);
|
||||
Verification.True(!validator.TryValidate(nearTerminalInput, nearTerminalLimit, stationary, out _,
|
||||
out failureStatus, out _), "near-terminal stationary full segment is rejected");
|
||||
Verification.Equal(EmPlanningStatus.NoProgress, failureStatus,
|
||||
"near-terminal gear-switch stop is classified as no progress");
|
||||
}
|
||||
|
||||
private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
|
||||
{
|
||||
LateralPath path = CreateStraightPath(1d);
|
||||
DateTimeOffset previousEffectiveAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
|
||||
EmTrajectory previous = CreatePreviousTrajectory(previousEffectiveAtUtc, TravelDirection.Forward, 3);
|
||||
var builder = new LongitudinalPreviousTrajectorySeedBuilder();
|
||||
LongitudinalPreviousTrajectorySeed seed = builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
|
||||
new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Forward);
|
||||
|
||||
Verification.Equal(3, seed.PathS.Count, "previous seed path-S count");
|
||||
Verification.Equal(3, seed.ProgressSpeedMetersPerSecond.Count, "previous seed speed count");
|
||||
Verification.NearlyEqual(0.20d, seed.PathS[0], "previous seed begins at new absolute effective time");
|
||||
Verification.True(seed.PathS[1] >= seed.PathS[0] && seed.PathS[2] >= seed.PathS[1],
|
||||
"previous seed progress is monotone");
|
||||
Verification.NearlyEqual(0.10d, seed.ProgressSpeedMetersPerSecond[0],
|
||||
"previous seed uses absolute progress speed");
|
||||
|
||||
Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
|
||||
new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Reverse).PathS.Count,
|
||||
"different direction returns an empty seed");
|
||||
Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
|
||||
new[] { 0d, 0.10d, 0.20d }, 4, TravelDirection.Forward).PathS.Count,
|
||||
"different segment returns an empty seed");
|
||||
Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.40d),
|
||||
new[] { 0d, 0.10d }, 3, TravelDirection.Forward).PathS.Count,
|
||||
"out-of-range absolute sampling returns an empty seed");
|
||||
}
|
||||
|
||||
private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
|
||||
{
|
||||
var points = new List<LateralPathPoint>(fixtures.Count);
|
||||
for (int index = 0; index < fixtures.Count; index++)
|
||||
{
|
||||
PathFixture fixture = fixtures[index];
|
||||
points.Add(new LateralPathPoint(fixture.ReferenceS, fixture.PathS, 0d, 0d, 0d, 0d, fixture.PathS, 0d,
|
||||
0d, fixture.Curvature, fixture.Curvature, fixture.CurvatureDerivative));
|
||||
}
|
||||
return new LateralPath(points, true);
|
||||
}
|
||||
|
||||
private static EmTrajectory CreatePreviousTrajectory(DateTimeOffset effectiveAtUtc, TravelDirection direction,
|
||||
int segmentIndex)
|
||||
{
|
||||
var metadata = new EmTrajectoryMetadata("previous-seed", effectiveAtUtc, effectiveAtUtc, 1L,
|
||||
"previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
|
||||
EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
|
||||
double sign = direction == TravelDirection.Forward ? 1d : -1d;
|
||||
return new EmTrajectory(metadata, new[]
|
||||
{
|
||||
new EmTrajectoryPoint(0d, 0d, 0d, sign * 0.10d, 0d, 0d, segmentIndex, 0d, 0d,
|
||||
direction, EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(sign * 0.10d, 0d, 0d, sign * 0.10d, 0.10d, 0d, segmentIndex, 0.10d, 0.10d,
|
||||
direction, EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(sign * 0.20d, 0d, 0d, sign * 0.10d, 0.20d, 0d, segmentIndex, 0.20d, 0.20d,
|
||||
direction, EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(sign * 0.30d, 0d, 0d, sign * 0.10d, 0.30d, 0d, segmentIndex, 0.30d, 0.30d,
|
||||
direction, EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(sign * 0.40d, 0d, 0d, sign * 0.10d, 0.40d, 0d, segmentIndex, 0.40d, 0.40d,
|
||||
direction, EmBoundaryType.None, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d),
|
||||
Point(length, length),
|
||||
};
|
||||
return new DirectionSegmentView(0, TravelDirection.Forward, points,
|
||||
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
||||
new ReferenceBoundary(0, length, endBoundaryType, length), 0d);
|
||||
}
|
||||
|
||||
private static SmoothedPathPoint Point(double x, double s)
|
||||
{
|
||||
return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d, false,
|
||||
SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
|
||||
private static EmPlannerConfiguration CreateUnitScaleConfiguration()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 0.20d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.05d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
|
||||
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static EmPlannerConfiguration CreateTaskFourConfiguration()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.TimeHorizonSeconds = 0.30d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 10d;
|
||||
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static LateralPath CreateStraightPath(double pathUpperBoundS)
|
||||
{
|
||||
return CreatePath(new[]
|
||||
{
|
||||
new PathFixture(0d, 0d, 0d, 0d),
|
||||
new PathFixture(pathUpperBoundS, pathUpperBoundS, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS)
|
||||
{
|
||||
LongitudinalConfiguration limits = input.Configuration.Longitudinal;
|
||||
return JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
|
||||
Math.Max(0d, input.StopBoundaryPathS - pathS),
|
||||
Math.Max(0d, input.InitialAccelerationMetersPerSecondSquared),
|
||||
limits.MaximumDecelerationMetersPerSecondSquared,
|
||||
limits.MaximumJerkMetersPerSecondCubed,
|
||||
input.DirectionMaximumSpeedMetersPerSecond);
|
||||
}
|
||||
|
||||
private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
|
||||
{
|
||||
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (matrix.RowIndices[index] == row)
|
||||
return matrix.Values[index];
|
||||
}
|
||||
return 0d;
|
||||
}
|
||||
|
||||
private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
|
||||
{
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
if (RowMatches(problem.ConstraintMatrix, row, new Dictionary<int, double> { { variable, 1d } }))
|
||||
{
|
||||
lower = problem.LowerBounds[row];
|
||||
upper = problem.UpperBounds[row];
|
||||
return;
|
||||
}
|
||||
}
|
||||
throw new InvalidOperationException("No single-variable bounds were found for variable " + variable + ".");
|
||||
}
|
||||
|
||||
private static int CountExactEqualityRows(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
|
||||
double bound)
|
||||
{
|
||||
return CountBoundedRow(problem, expected, bound, bound);
|
||||
}
|
||||
|
||||
private static int CountBoundedRow(QuadraticProgram problem, IReadOnlyDictionary<int, double> expected,
|
||||
double lower, double upper)
|
||||
{
|
||||
int count = 0;
|
||||
for (int row = 0; row < problem.ConstraintCount; row++)
|
||||
{
|
||||
if (Math.Abs(problem.LowerBounds[row] - lower) <= 1e-12d &&
|
||||
Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d && RowMatches(problem.ConstraintMatrix, row, expected))
|
||||
{
|
||||
count++;
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary<int, double> expected)
|
||||
{
|
||||
var actual = new Dictionary<int, double>();
|
||||
for (int column = 0; column < matrix.ColumnCount; column++)
|
||||
{
|
||||
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
||||
{
|
||||
if (matrix.RowIndices[index] == targetRow)
|
||||
actual[column] = matrix.Values[index];
|
||||
}
|
||||
}
|
||||
if (actual.Count != expected.Count)
|
||||
return false;
|
||||
foreach (KeyValuePair<int, double> pair in expected)
|
||||
{
|
||||
if (!actual.TryGetValue(pair.Key, out double actualValue) || Math.Abs(actualValue - pair.Value) > 1e-12d)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private sealed class PathFixture
|
||||
{
|
||||
public PathFixture(double referenceS, double pathS, double curvature, double curvatureDerivative)
|
||||
{
|
||||
ReferenceS = referenceS;
|
||||
PathS = pathS;
|
||||
Curvature = curvature;
|
||||
CurvatureDerivative = curvatureDerivative;
|
||||
}
|
||||
|
||||
public double ReferenceS { get; }
|
||||
public double PathS { get; }
|
||||
public double Curvature { get; }
|
||||
public double CurvatureDerivative { get; }
|
||||
}
|
||||
}
|
||||
@@ -1,142 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class OptimizationChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifyCanonicalCscAssembly();
|
||||
VerifyInvalidTripletsAreRejected();
|
||||
VerifyUpperTriangularHessianStorage();
|
||||
VerifyQuadraticProgramValidation();
|
||||
VerifyQuadraticProgramDefensivelyCopiesInputs();
|
||||
}
|
||||
|
||||
private static void VerifyCanonicalCscAssembly()
|
||||
{
|
||||
var builder = new SparseTripletBuilder(3, 2);
|
||||
builder.Add(2, 1, 1.5d);
|
||||
builder.Add(0, 0, 2d);
|
||||
builder.Add(1, 1, 3d);
|
||||
builder.Add(2, 1, 0.5d);
|
||||
builder.Add(1, 0, -2d);
|
||||
builder.Add(0, 1, 7d);
|
||||
builder.Add(0, 1, -7d);
|
||||
|
||||
SparseCscMatrix matrix = builder.Build();
|
||||
|
||||
EMPlannerVerificationHost.Verification.Equal(3, matrix.ColumnPointers.Count, "CSC column-pointer length");
|
||||
EMPlannerVerificationHost.Verification.Equal(0, matrix.ColumnPointers[0], "CSC first pointer");
|
||||
EMPlannerVerificationHost.Verification.Equal(2, matrix.ColumnPointers[1], "CSC second pointer");
|
||||
EMPlannerVerificationHost.Verification.Equal(4, matrix.ColumnPointers[2], "CSC final pointer");
|
||||
EMPlannerVerificationHost.Verification.Equal(4, matrix.Values.Count, "CSC nonzero count");
|
||||
EMPlannerVerificationHost.Verification.Equal(0, matrix.RowIndices[0], "CSC first row");
|
||||
EMPlannerVerificationHost.Verification.Equal(1, matrix.RowIndices[1], "CSC second row");
|
||||
EMPlannerVerificationHost.Verification.Equal(1, matrix.RowIndices[2], "CSC third row");
|
||||
EMPlannerVerificationHost.Verification.Equal(2, matrix.RowIndices[3], "CSC fourth row");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(2d, matrix.Values[0], "CSC first value");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(-2d, matrix.Values[1], "CSC second value");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(3d, matrix.Values[2], "CSC third value");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(2d, matrix.Values[3], "CSC duplicate sum");
|
||||
}
|
||||
|
||||
private static void VerifyInvalidTripletsAreRejected()
|
||||
{
|
||||
var builder = new SparseTripletBuilder(2, 2);
|
||||
|
||||
AssertArgumentOutOfRange(() => builder.Add(-1, 0, 1d), "negative row");
|
||||
AssertArgumentOutOfRange(() => builder.Add(0, -1, 1d), "negative column");
|
||||
AssertArgumentOutOfRange(() => builder.Add(2, 0, 1d), "row outside matrix");
|
||||
AssertArgumentOutOfRange(() => builder.Add(0, 2, 1d), "column outside matrix");
|
||||
AssertArgumentOutOfRange(() => builder.Add(0, 0, double.NaN), "NaN triplet");
|
||||
AssertArgumentOutOfRange(() => builder.Add(0, 0, double.PositiveInfinity), "infinite triplet");
|
||||
}
|
||||
|
||||
private static void VerifyUpperTriangularHessianStorage()
|
||||
{
|
||||
var upperBuilder = new SparseTripletBuilder(2, 2, true);
|
||||
upperBuilder.Add(0, 0, 1d);
|
||||
upperBuilder.Add(0, 1, 2d);
|
||||
AssertArgumentOutOfRange(() => upperBuilder.Add(1, 0, 3d), "lower-triangular Hessian entry");
|
||||
|
||||
SparseCscMatrix upperHessian = upperBuilder.Build();
|
||||
var constraints = new SparseTripletBuilder(1, 2);
|
||||
constraints.Add(0, 0, 1d);
|
||||
constraints.Add(0, 1, 1d);
|
||||
|
||||
var lowerTriangleBuilder = new SparseTripletBuilder(2, 2);
|
||||
lowerTriangleBuilder.Add(0, 0, 1d);
|
||||
lowerTriangleBuilder.Add(1, 0, 3d);
|
||||
AssertArgumentOutOfRange(
|
||||
() => new QuadraticProgram(lowerTriangleBuilder.Build(), new[] { 0d, 0d }, constraints.Build(), new[] { 0d }, new[] { 1d }),
|
||||
"lower-triangular quadratic-program Hessian");
|
||||
|
||||
EMPlannerVerificationHost.Verification.Equal(2, upperHessian.ColumnCount, "upper Hessian column count");
|
||||
}
|
||||
|
||||
private static void VerifyQuadraticProgramValidation()
|
||||
{
|
||||
var nonSquareHessian = new SparseCscMatrix(1, 2, new double[0], new int[0], new[] { 0, 0, 0 });
|
||||
var oneVariableHessian = new SparseTripletBuilder(1, 1, true);
|
||||
oneVariableHessian.Add(0, 0, 1d);
|
||||
var oneConstraint = new SparseTripletBuilder(1, 1);
|
||||
oneConstraint.Add(0, 0, 1d);
|
||||
|
||||
AssertArgumentOutOfRange(
|
||||
() => new QuadraticProgram(nonSquareHessian, new[] { 0d, 0d }, oneConstraint.Build(), new[] { 0d }, new[] { 1d }),
|
||||
"non-square Hessian");
|
||||
AssertArgumentOutOfRange(
|
||||
() => new QuadraticProgram(oneVariableHessian.Build(), new[] { 0d }, oneConstraint.Build(), new[] { 2d }, new[] { 1d }),
|
||||
"inverted constraint bounds");
|
||||
AssertArgumentOutOfRange(
|
||||
() => new QuadraticProgram(oneVariableHessian.Build(), new[] { double.NaN }, oneConstraint.Build(), new[] { 0d }, new[] { 1d }),
|
||||
"non-finite linear cost");
|
||||
AssertArgumentOutOfRange(
|
||||
() => new QuadraticProgram(oneVariableHessian.Build(), new[] { 0d }, oneConstraint.Build(), new[] { -1e30d }, new[] { 2e30d }),
|
||||
"out-of-range finite bound");
|
||||
}
|
||||
|
||||
private static void VerifyQuadraticProgramDefensivelyCopiesInputs()
|
||||
{
|
||||
var hessianBuilder = new SparseTripletBuilder(1, 1, true);
|
||||
hessianBuilder.Add(0, 0, 1d);
|
||||
var constraintBuilder = new SparseTripletBuilder(1, 1);
|
||||
constraintBuilder.Add(0, 0, 1d);
|
||||
|
||||
var linearCost = new List<double> { -2d };
|
||||
var lowerBounds = new List<double> { 0d };
|
||||
var upperBounds = new List<double> { 1d };
|
||||
QuadraticProgram problem = new QuadraticProgram(
|
||||
hessianBuilder.Build(),
|
||||
linearCost,
|
||||
constraintBuilder.Build(),
|
||||
lowerBounds,
|
||||
upperBounds);
|
||||
|
||||
linearCost[0] = 100d;
|
||||
lowerBounds[0] = -100d;
|
||||
upperBounds[0] = 100d;
|
||||
|
||||
EMPlannerVerificationHost.Verification.Equal(1, problem.VariableCount, "micro problem variable count");
|
||||
EMPlannerVerificationHost.Verification.Equal(1, problem.ConstraintCount, "micro problem constraint count");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(-2d, problem.LinearCost[0], "copied linear cost");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0d, problem.LowerBounds[0], "copied lower bound");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, problem.UpperBounds[0], "copied upper bound");
|
||||
}
|
||||
|
||||
private static void AssertArgumentOutOfRange(Action action, string name)
|
||||
{
|
||||
try
|
||||
{
|
||||
action();
|
||||
}
|
||||
catch (ArgumentOutOfRangeException)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(name + " did not throw ArgumentOutOfRangeException.");
|
||||
}
|
||||
}
|
||||
@@ -1,305 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.IO;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class OsqpChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
var pluginDirectories = new List<string>();
|
||||
try
|
||||
{
|
||||
string missingNativeDirectory = CreatePluginDirectory();
|
||||
pluginDirectories.Add(missingNativeDirectory);
|
||||
ProbeResult missingNative = RunProbe(missingNativeDirectory);
|
||||
EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.SolverUnavailable, missingNative.Status, "missing native status");
|
||||
EMPlannerVerificationHost.Verification.True(!string.IsNullOrEmpty(missingNative.Diagnostic), "missing native diagnostic");
|
||||
|
||||
string corruptNativeDirectory = CreatePluginDirectory();
|
||||
pluginDirectories.Add(corruptNativeDirectory);
|
||||
File.WriteAllText(Path.Combine(corruptNativeDirectory, "osqp.dll"), "not a native library");
|
||||
ProbeResult corruptNative = RunProbe(corruptNativeDirectory);
|
||||
EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.SolverUnavailable, corruptNative.Status, "corrupt native status");
|
||||
EMPlannerVerificationHost.Verification.True(!string.IsNullOrEmpty(corruptNative.Diagnostic), "corrupt native diagnostic");
|
||||
EMPlannerVerificationHost.Verification.True(corruptNative.RawOutput.IndexOf("BadImageFormatException", StringComparison.Ordinal) < 0,
|
||||
"corrupt native does not escape BadImageFormatException");
|
||||
|
||||
string realNativeDirectory = CreatePluginDirectory();
|
||||
pluginDirectories.Add(realNativeDirectory);
|
||||
string realNativePath = Path.GetFullPath(Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ThirdParty", "OSQP", "win-x64", "osqp.dll"));
|
||||
EMPlannerVerificationHost.Verification.True(File.Exists(realNativePath), "pinned native package exists");
|
||||
File.Copy(realNativePath, Path.Combine(realNativeDirectory, "osqp.dll"), false);
|
||||
ProbeResult realNative = RunProbe(realNativeDirectory);
|
||||
EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, realNative.Status, "real native status");
|
||||
EMPlannerVerificationHost.Verification.Equal("1.0.0", realNative.Version, "real native version");
|
||||
EMPlannerVerificationHost.Verification.True(realNative.ModuleHandle != IntPtr.Zero, "real native module handle");
|
||||
EMPlannerVerificationHost.Verification.Equal(true, realNative.ConcurrentHandleStable, "concurrent native module handle");
|
||||
EMPlannerVerificationHost.Verification.Equal(true, realNative.SolveChecksPassed, "native solve checks");
|
||||
Console.WriteLine("PASS osqp-solve");
|
||||
}
|
||||
finally
|
||||
{
|
||||
for (int index = 0; index < pluginDirectories.Count; index++)
|
||||
{
|
||||
if (Directory.Exists(pluginDirectories[index]))
|
||||
Directory.Delete(pluginDirectories[index], true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static void RunProbe()
|
||||
{
|
||||
var results = new OsqpNativeLoadResult[16];
|
||||
Parallel.For(0, results.Length, index => results[index] = OsqpNativeLoader.Load());
|
||||
|
||||
OsqpNativeLoadResult first = results[0];
|
||||
bool sameHandle = true;
|
||||
for (int index = 1; index < results.Length; index++)
|
||||
{
|
||||
sameHandle = sameHandle && results[index].Status == first.Status && results[index].ModuleHandle == first.ModuleHandle;
|
||||
}
|
||||
|
||||
Console.WriteLine("STATUS=" + first.Status);
|
||||
Console.WriteLine("VERSION=" + first.Version);
|
||||
Console.WriteLine("HANDLE=" + first.ModuleHandle.ToInt64());
|
||||
Console.WriteLine("DIAGNOSTIC=" + first.Diagnostic.Replace('\r', ' ').Replace('\n', ' '));
|
||||
Console.WriteLine("CONCURRENT_HANDLE_STABLE=" + sameHandle);
|
||||
|
||||
if (first.Status == QpSolveStatus.Solved)
|
||||
{
|
||||
VerifyNativeSolveLifecycle();
|
||||
Console.WriteLine("SOLVE_CHECKS=PASS");
|
||||
}
|
||||
}
|
||||
|
||||
private static string CreatePluginDirectory()
|
||||
{
|
||||
string sourceDirectory = AppContext.BaseDirectory;
|
||||
string destinationDirectory = Path.Combine(Path.GetTempPath(), "em-planner-osqp-loader-" + Guid.NewGuid().ToString("N"));
|
||||
Directory.CreateDirectory(destinationDirectory);
|
||||
|
||||
string[] hostFiles = Directory.GetFiles(sourceDirectory);
|
||||
for (int index = 0; index < hostFiles.Length; index++)
|
||||
{
|
||||
if (string.Equals(Path.GetFileName(hostFiles[index]), "osqp.dll", StringComparison.OrdinalIgnoreCase))
|
||||
continue;
|
||||
string destinationPath = Path.Combine(destinationDirectory, Path.GetFileName(hostFiles[index]));
|
||||
File.Copy(hostFiles[index], destinationPath, false);
|
||||
}
|
||||
|
||||
EMPlannerVerificationHost.Verification.True(File.Exists(Path.Combine(destinationDirectory, "ClumsyPilot.dll")), "copied plugin ClumsyPilot.dll");
|
||||
return destinationDirectory;
|
||||
}
|
||||
|
||||
private static void VerifyNativeSolveLifecycle()
|
||||
{
|
||||
var solver = new OsqpNativeSolver();
|
||||
|
||||
QpSolveResult bounded = solver.Solve(
|
||||
CreateBoundedOptimumProblem(),
|
||||
CreateSettings(TimeSpan.FromMilliseconds(100)),
|
||||
new[] { 0d },
|
||||
CancellationToken.None);
|
||||
EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, bounded.Status, "bounded optimum status");
|
||||
AssertPopulatedResult(bounded, "bounded optimum");
|
||||
EMPlannerVerificationHost.Verification.Equal(1, bounded.Primal.Count, "bounded optimum primal count");
|
||||
AssertClose(1d, bounded.Primal[0], 1e-5d, "bounded optimum primal");
|
||||
AssertClose(-1.5d, bounded.Objective, 1e-5d, "bounded optimum objective");
|
||||
EMPlannerVerificationHost.Verification.True(bounded.PrimalResidual <= 1e-5d, "bounded optimum primal residual");
|
||||
EMPlannerVerificationHost.Verification.True(bounded.DualResidual <= 1e-5d, "bounded optimum dual residual");
|
||||
|
||||
QpSolveResult equality = solver.Solve(
|
||||
CreateEqualityOptimumProblem(),
|
||||
CreateSettings(TimeSpan.FromMilliseconds(100)),
|
||||
new[] { 0.5d, 0.5d },
|
||||
CancellationToken.None);
|
||||
EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, equality.Status, "equality optimum status");
|
||||
AssertPopulatedResult(equality, "equality optimum");
|
||||
EMPlannerVerificationHost.Verification.Equal(2, equality.Primal.Count, "equality optimum primal count");
|
||||
AssertClose(0.5d, equality.Primal[0], 1e-5d, "equality optimum first primal");
|
||||
AssertClose(0.5d, equality.Primal[1], 1e-5d, "equality optimum second primal");
|
||||
AssertClose(0.5d, equality.Objective, 1e-5d, "equality optimum objective");
|
||||
EMPlannerVerificationHost.Verification.True(equality.PrimalResidual <= 1e-5d, "equality optimum primal residual");
|
||||
EMPlannerVerificationHost.Verification.True(equality.DualResidual <= 1e-5d, "equality optimum dual residual");
|
||||
|
||||
QpSolveResult infeasible = solver.Solve(
|
||||
CreateInfeasibleProblem(),
|
||||
CreateSettings(TimeSpan.FromMilliseconds(100)),
|
||||
null,
|
||||
CancellationToken.None);
|
||||
EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.PrimalInfeasible, infeasible.Status, "infeasible status");
|
||||
AssertPopulatedResult(infeasible, "infeasible");
|
||||
|
||||
QpSolveResult tinyTimeLimit = solver.Solve(
|
||||
CreateEqualityOptimumProblem(),
|
||||
CreateSettings(TimeSpan.FromTicks(1)),
|
||||
null,
|
||||
CancellationToken.None);
|
||||
EMPlannerVerificationHost.Verification.True(
|
||||
tinyTimeLimit.Status == QpSolveStatus.TimeLimit ||
|
||||
tinyTimeLimit.Status == QpSolveStatus.Solved ||
|
||||
tinyTimeLimit.Status == QpSolveStatus.SolvedInaccurate,
|
||||
"tiny time-limit status maps to a time limit or solved state");
|
||||
AssertPopulatedResult(tinyTimeLimit, "tiny time-limit");
|
||||
}
|
||||
|
||||
private static QuadraticProgram CreateBoundedOptimumProblem()
|
||||
{
|
||||
var hessian = new SparseTripletBuilder(1, 1, true);
|
||||
hessian.Add(0, 0, 1d);
|
||||
var constraints = new SparseTripletBuilder(1, 1);
|
||||
constraints.Add(0, 0, 1d);
|
||||
return new QuadraticProgram(hessian.Build(), new[] { -2d }, constraints.Build(), new[] { 0d }, new[] { 1d });
|
||||
}
|
||||
|
||||
private static QuadraticProgram CreateEqualityOptimumProblem()
|
||||
{
|
||||
var hessian = new SparseTripletBuilder(2, 2, true);
|
||||
hessian.Add(0, 0, 2d);
|
||||
hessian.Add(1, 1, 2d);
|
||||
var constraints = new SparseTripletBuilder(1, 2);
|
||||
constraints.Add(0, 0, 1d);
|
||||
constraints.Add(0, 1, 1d);
|
||||
return new QuadraticProgram(hessian.Build(), new[] { 0d, 0d }, constraints.Build(), new[] { 1d }, new[] { 1d });
|
||||
}
|
||||
|
||||
private static QuadraticProgram CreateInfeasibleProblem()
|
||||
{
|
||||
var hessian = new SparseTripletBuilder(1, 1, true);
|
||||
hessian.Add(0, 0, 1d);
|
||||
var constraints = new SparseTripletBuilder(2, 1);
|
||||
constraints.Add(0, 0, 1d);
|
||||
constraints.Add(1, 0, 1d);
|
||||
return new QuadraticProgram(
|
||||
hessian.Build(),
|
||||
new[] { 0d },
|
||||
constraints.Build(),
|
||||
new[] { 1d, -QuadraticProgram.MaximumFiniteBound },
|
||||
new[] { QuadraticProgram.MaximumFiniteBound, 0d });
|
||||
}
|
||||
|
||||
private static QpSolverSettings CreateSettings(TimeSpan timeLimit)
|
||||
{
|
||||
return new QpSolverSettings(4000, 1e-6d, 1e-6d, timeLimit, true, true, false);
|
||||
}
|
||||
|
||||
private static void AssertPopulatedResult(QpSolveResult result, string name)
|
||||
{
|
||||
EMPlannerVerificationHost.Verification.True(result.Iterations >= 0, name + " iterations");
|
||||
EMPlannerVerificationHost.Verification.True(result.SolveTime >= TimeSpan.Zero, name + " solve time");
|
||||
EMPlannerVerificationHost.Verification.True(!double.IsNaN(result.Objective) && !double.IsInfinity(result.Objective), name + " objective");
|
||||
EMPlannerVerificationHost.Verification.True(!double.IsNaN(result.PrimalResidual) && !double.IsInfinity(result.PrimalResidual), name + " primal residual");
|
||||
EMPlannerVerificationHost.Verification.True(!double.IsNaN(result.DualResidual) && !double.IsInfinity(result.DualResidual), name + " dual residual");
|
||||
EMPlannerVerificationHost.Verification.True(!string.IsNullOrWhiteSpace(result.NativeStatus), name + " native status");
|
||||
}
|
||||
|
||||
private static void AssertClose(double expected, double actual, double tolerance, string name)
|
||||
{
|
||||
EMPlannerVerificationHost.Verification.True(Math.Abs(expected - actual) <= tolerance, name + " expected " + expected + " but was " + actual);
|
||||
}
|
||||
|
||||
private static ProbeResult RunProbe(string pluginDirectory)
|
||||
{
|
||||
var startInfo = new ProcessStartInfo
|
||||
{
|
||||
FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
|
||||
Arguments = "osqp-probe",
|
||||
WorkingDirectory = pluginDirectory,
|
||||
UseShellExecute = false,
|
||||
CreateNoWindow = true,
|
||||
RedirectStandardOutput = true,
|
||||
RedirectStandardError = true,
|
||||
};
|
||||
|
||||
using (var process = new Process { StartInfo = startInfo })
|
||||
{
|
||||
process.Start();
|
||||
string standardOutput = process.StandardOutput.ReadToEnd();
|
||||
string standardError = process.StandardError.ReadToEnd();
|
||||
process.WaitForExit();
|
||||
if (process.ExitCode != 0)
|
||||
throw new InvalidOperationException("OSQP loader probe exited " + process.ExitCode + ": " + standardError + standardOutput);
|
||||
return ProbeResult.Parse(standardOutput + standardError);
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class ProbeResult
|
||||
{
|
||||
private ProbeResult(QpSolveStatus status, string version, IntPtr moduleHandle, string diagnostic, bool concurrentHandleStable, bool solveChecksPassed, string rawOutput)
|
||||
{
|
||||
Status = status;
|
||||
Version = version;
|
||||
ModuleHandle = moduleHandle;
|
||||
Diagnostic = diagnostic;
|
||||
ConcurrentHandleStable = concurrentHandleStable;
|
||||
SolveChecksPassed = solveChecksPassed;
|
||||
RawOutput = rawOutput;
|
||||
}
|
||||
|
||||
public QpSolveStatus Status { get; }
|
||||
|
||||
public string Version { get; }
|
||||
|
||||
public IntPtr ModuleHandle { get; }
|
||||
|
||||
public string Diagnostic { get; }
|
||||
|
||||
public bool ConcurrentHandleStable { get; }
|
||||
|
||||
public bool SolveChecksPassed { get; }
|
||||
|
||||
public string RawOutput { get; }
|
||||
|
||||
public static ProbeResult Parse(string output)
|
||||
{
|
||||
string statusText = ReadValue(output, "STATUS=");
|
||||
string version = ReadValue(output, "VERSION=");
|
||||
string handleText = ReadValue(output, "HANDLE=");
|
||||
string diagnostic = ReadValue(output, "DIAGNOSTIC=");
|
||||
string stableText = ReadValue(output, "CONCURRENT_HANDLE_STABLE=");
|
||||
string solveChecksText = ReadOptionalValue(output, "SOLVE_CHECKS=");
|
||||
|
||||
QpSolveStatus status;
|
||||
if (!Enum.TryParse(statusText, out status))
|
||||
throw new InvalidOperationException("Loader probe reported an invalid status: " + statusText);
|
||||
long handleValue;
|
||||
if (!long.TryParse(handleText, out handleValue))
|
||||
throw new InvalidOperationException("Loader probe reported an invalid handle: " + handleText);
|
||||
bool concurrentHandleStable;
|
||||
if (!bool.TryParse(stableText, out concurrentHandleStable))
|
||||
throw new InvalidOperationException("Loader probe reported an invalid concurrency flag: " + stableText);
|
||||
|
||||
return new ProbeResult(status, version, new IntPtr(handleValue), diagnostic, concurrentHandleStable,
|
||||
string.Equals(solveChecksText, "PASS", StringComparison.Ordinal), output);
|
||||
}
|
||||
|
||||
private static string ReadValue(string output, string prefix)
|
||||
{
|
||||
string[] lines = output.Replace("\r", string.Empty).Split('\n');
|
||||
for (int index = 0; index < lines.Length; index++)
|
||||
{
|
||||
if (lines[index].StartsWith(prefix, StringComparison.Ordinal))
|
||||
return lines[index].Substring(prefix.Length);
|
||||
}
|
||||
|
||||
throw new InvalidOperationException("Loader probe did not report " + prefix + ". Output: " + output);
|
||||
}
|
||||
|
||||
private static string ReadOptionalValue(string output, string prefix)
|
||||
{
|
||||
string[] lines = output.Replace("\r", string.Empty).Split('\n');
|
||||
for (int index = 0; index < lines.Length; index++)
|
||||
{
|
||||
if (lines[index].StartsWith(prefix, StringComparison.Ordinal))
|
||||
return lines[index].Substring(prefix.Length);
|
||||
}
|
||||
|
||||
return string.Empty;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,152 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.IO;
|
||||
using System.Reflection;
|
||||
using System.Security.Cryptography;
|
||||
|
||||
namespace EMPlannerVerificationHost;
|
||||
|
||||
internal static class PluginPackagingChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
string repositoryRoot = Directory.GetCurrentDirectory();
|
||||
string scriptPath = Path.Combine(repositoryRoot, "ClumsyPilot", "scripts", "Publish-ClumsyPilotPlugin.ps1");
|
||||
Verification.True(File.Exists(scriptPath), "plugin publish script exists");
|
||||
|
||||
string managedDll = Path.Combine(AppContext.BaseDirectory, "ClumsyPilot.dll");
|
||||
Verification.True(File.Exists(managedDll), "verification host has managed ClumsyPilot.dll");
|
||||
VerifyBuildOutputMetadata();
|
||||
|
||||
string packageRoot = Path.Combine(Path.GetTempPath(), "em-planner-plugin-package-" + Guid.NewGuid().ToString("N"));
|
||||
Directory.CreateDirectory(packageRoot);
|
||||
try
|
||||
{
|
||||
Publish(scriptPath, managedDll, packageRoot);
|
||||
VerifyPackage(repositoryRoot, packageRoot);
|
||||
|
||||
Publish(scriptPath, managedDll, packageRoot);
|
||||
VerifyPackage(repositoryRoot, packageRoot);
|
||||
AssertNoStaleSiblingDirectories(packageRoot);
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (Directory.Exists(packageRoot))
|
||||
Directory.Delete(packageRoot, true);
|
||||
}
|
||||
}
|
||||
|
||||
private static void Publish(string scriptPath, string managedDll, string packageRoot)
|
||||
{
|
||||
var startInfo = new ProcessStartInfo
|
||||
{
|
||||
FileName = "powershell.exe",
|
||||
WorkingDirectory = Directory.GetCurrentDirectory(),
|
||||
UseShellExecute = false,
|
||||
CreateNoWindow = true,
|
||||
RedirectStandardOutput = true,
|
||||
RedirectStandardError = true,
|
||||
};
|
||||
startInfo.ArgumentList.Add("-NoProfile");
|
||||
startInfo.ArgumentList.Add("-ExecutionPolicy");
|
||||
startInfo.ArgumentList.Add("Bypass");
|
||||
startInfo.ArgumentList.Add("-File");
|
||||
startInfo.ArgumentList.Add(scriptPath);
|
||||
startInfo.ArgumentList.Add("-ManagedDll");
|
||||
startInfo.ArgumentList.Add(managedDll);
|
||||
startInfo.ArgumentList.Add("-OutputDirectory");
|
||||
startInfo.ArgumentList.Add(packageRoot);
|
||||
|
||||
using (var process = new Process { StartInfo = startInfo })
|
||||
{
|
||||
process.Start();
|
||||
string standardOutput = process.StandardOutput.ReadToEnd();
|
||||
string standardError = process.StandardError.ReadToEnd();
|
||||
process.WaitForExit();
|
||||
if (process.ExitCode != 0)
|
||||
{
|
||||
throw new InvalidOperationException("plugin publish exited " + process.ExitCode + ": " +
|
||||
standardOutput + standardError);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyPackage(string repositoryRoot, string packageRoot)
|
||||
{
|
||||
string pluginsDirectory = Path.Combine(packageRoot, "plugins");
|
||||
Verification.True(Directory.Exists(pluginsDirectory), "plugins directory exists");
|
||||
|
||||
var relativeFiles = new List<string>();
|
||||
string[] files = Directory.GetFiles(pluginsDirectory, "*", SearchOption.AllDirectories);
|
||||
for (int index = 0; index < files.Length; index++)
|
||||
relativeFiles.Add(Path.GetRelativePath(packageRoot, files[index]).Replace('\\', '/'));
|
||||
relativeFiles.Sort(StringComparer.Ordinal);
|
||||
|
||||
Verification.Equal("plugins/ClumsyPilot.dll|plugins/TrajectoryPlanningVisualization.dll|" +
|
||||
"plugins/licenses/OSQP-LICENSE.txt|plugins/licenses/OSQP-NOTICE.txt|plugins/licenses/OSQP-VERSION.txt|" +
|
||||
"plugins/osqp.dll", string.Join("|", relativeFiles),
|
||||
"plugin tree contains exactly the managed DLLs, native DLL, and OSQP license files");
|
||||
|
||||
string managedPlugin = Path.Combine(pluginsDirectory, "ClumsyPilot.dll");
|
||||
AssemblyName managedAssembly = AssemblyName.GetAssemblyName(managedPlugin);
|
||||
Verification.True(!string.IsNullOrWhiteSpace(managedAssembly.Name), "packaged ClumsyPilot.dll is managed");
|
||||
|
||||
string visualizationPlugin = Path.Combine(pluginsDirectory, "TrajectoryPlanningVisualization.dll");
|
||||
AssemblyName visualizationAssembly = AssemblyName.GetAssemblyName(visualizationPlugin);
|
||||
Verification.Equal("TrajectoryPlanningVisualization", visualizationAssembly.Name,
|
||||
"packaged visualization DLL has the expected managed assembly name");
|
||||
|
||||
string nativePlugin = Path.Combine(pluginsDirectory, "osqp.dll");
|
||||
Verification.Equal((ushort)0x8664, ReadPeMachine(nativePlugin), "packaged OSQP binary is x64");
|
||||
Verification.Equal(ReadExpectedOsqpHash(repositoryRoot), GetSha256(nativePlugin),
|
||||
"packaged OSQP hash matches SHA256SUMS");
|
||||
}
|
||||
|
||||
private static void AssertNoStaleSiblingDirectories(string packageRoot)
|
||||
{
|
||||
string parentDirectory = Path.GetDirectoryName(packageRoot) ??
|
||||
throw new InvalidOperationException("temporary package root has no parent directory");
|
||||
string prefix = Path.GetFileName(packageRoot);
|
||||
string[] matchingDirectories = Directory.GetDirectories(parentDirectory, prefix + "*");
|
||||
Verification.Equal(1, matchingDirectories.Length, "publish leaves no stale sibling staging directory");
|
||||
Verification.Equal(packageRoot, matchingDirectories[0], "publish keeps only the requested temporary test root");
|
||||
}
|
||||
|
||||
private static ushort ReadPeMachine(string path)
|
||||
{
|
||||
byte[] bytes = File.ReadAllBytes(path);
|
||||
Verification.True(bytes.Length > 0x40 && bytes[0] == 'M' && bytes[1] == 'Z', "OSQP has DOS header");
|
||||
int peOffset = BitConverter.ToInt32(bytes, 0x3c);
|
||||
Verification.True(peOffset >= 0 && peOffset + 6 <= bytes.Length && bytes[peOffset] == 'P' &&
|
||||
bytes[peOffset + 1] == 'E', "OSQP has PE header");
|
||||
return BitConverter.ToUInt16(bytes, peOffset + 4);
|
||||
}
|
||||
|
||||
private static string ReadExpectedOsqpHash(string repositoryRoot)
|
||||
{
|
||||
string manifestPath = Path.Combine(repositoryRoot, "ClumsyPilot", "ThirdParty", "OSQP", "SHA256SUMS");
|
||||
string[] tokens = File.ReadAllText(manifestPath).Split((char[]?)null, StringSplitOptions.RemoveEmptyEntries);
|
||||
Verification.True(tokens.Length >= 2 && string.Equals(tokens[1], "win-x64/osqp.dll", StringComparison.Ordinal),
|
||||
"OSQP hash manifest describes the pinned x64 runtime");
|
||||
return tokens[0].ToLowerInvariant();
|
||||
}
|
||||
|
||||
private static string GetSha256(string path)
|
||||
{
|
||||
return Convert.ToHexString(SHA256.HashData(File.ReadAllBytes(path))).ToLowerInvariant();
|
||||
}
|
||||
|
||||
private static void VerifyBuildOutputMetadata()
|
||||
{
|
||||
string outputDirectory = AppContext.BaseDirectory;
|
||||
Verification.True(File.Exists(Path.Combine(outputDirectory, "osqp.dll")),
|
||||
"build output contains the pinned OSQP runtime");
|
||||
Verification.True(File.Exists(Path.Combine(outputDirectory, "licenses", "OSQP-LICENSE.txt")),
|
||||
"build output contains the OSQP license");
|
||||
Verification.True(File.Exists(Path.Combine(outputDirectory, "licenses", "OSQP-NOTICE.txt")),
|
||||
"build output contains the OSQP notice");
|
||||
Verification.True(File.Exists(Path.Combine(outputDirectory, "licenses", "OSQP-VERSION.txt")),
|
||||
"build output contains the OSQP version");
|
||||
}
|
||||
}
|
||||
@@ -1,147 +0,0 @@
|
||||
using System;
|
||||
|
||||
namespace EMPlannerVerificationHost;
|
||||
|
||||
internal static class Program
|
||||
{
|
||||
private static int Main(string[] args)
|
||||
{
|
||||
if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
|
||||
args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp" && args[0] != "osqp-loader" && args[0] != "osqp-probe" &&
|
||||
args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration" &&
|
||||
args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" &&
|
||||
args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" &&
|
||||
args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory" &&
|
||||
args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator" &&
|
||||
args[0] != "executor" && args[0] != "plugin-package" && args[0] != "trajectory-observation" && args[0] != "em-all"))
|
||||
{
|
||||
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration|em-core-all|coordinator|executor|plugin-package|trajectory-observation|em-all");
|
||||
return 2;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (args[0] == "foundation" || args[0] == "all-foundation" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run();
|
||||
Console.WriteLine("PASS foundation");
|
||||
}
|
||||
if (args[0] == "segmentation" || args[0] == "all-foundation" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run();
|
||||
Console.WriteLine("PASS segmentation");
|
||||
}
|
||||
if (args[0] == "frenet" || args[0] == "all-foundation" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.FrenetChecks.Run();
|
||||
Console.WriteLine("PASS frenet");
|
||||
}
|
||||
if (args[0] == "corridor" || args[0] == "all-foundation" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.CorridorChecks.Run();
|
||||
Console.WriteLine("PASS corridor");
|
||||
}
|
||||
if (args[0] == "optimization" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.OptimizationChecks.Run();
|
||||
Console.WriteLine("PASS optimization");
|
||||
}
|
||||
if (args[0] == "osqp" || args[0] == "osqp-loader" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.Run();
|
||||
Console.WriteLine("PASS osqp-loader");
|
||||
}
|
||||
if (args[0] == "osqp-probe")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.RunProbe();
|
||||
}
|
||||
if (args[0] == "lateral-model" || args[0] == "lateral-all" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralModelChecks.Run();
|
||||
Console.WriteLine("PASS lateral-model");
|
||||
}
|
||||
if (args[0] == "lateral-integration" || args[0] == "lateral-all" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.Run();
|
||||
Console.WriteLine("PASS lateral-integration");
|
||||
}
|
||||
if (args[0] == "lateral-real-osqp-probe")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqp();
|
||||
Console.WriteLine("PASS lateral-real-osqp");
|
||||
}
|
||||
if (args[0] == "lateral-real-osqp" || args[0] == "lateral-all" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqpInCleanPluginBundle();
|
||||
Console.WriteLine("PASS lateral-real-osqp");
|
||||
}
|
||||
if (args[0] == "longitudinal-model")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
|
||||
Console.WriteLine("PASS longitudinal-model");
|
||||
}
|
||||
if (args[0] == "longitudinal-integration")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.Run();
|
||||
Console.WriteLine("PASS longitudinal-integration");
|
||||
}
|
||||
if (args[0] == "longitudinal-real-osqp-probe")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.RunRealOsqp();
|
||||
Console.WriteLine("PASS longitudinal-real-osqp");
|
||||
}
|
||||
if (args[0] == "trajectory")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run();
|
||||
Console.WriteLine("PASS trajectory");
|
||||
}
|
||||
if (args[0] == "em-planning-service" || args[0] == "em-core-all" || args[0] == "em-all")
|
||||
{
|
||||
if (args[0] == "em-core-all" || args[0] == "em-all")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
|
||||
Console.WriteLine("PASS longitudinal-model");
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.Run();
|
||||
Console.WriteLine("PASS longitudinal-integration");
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run();
|
||||
Console.WriteLine("PASS trajectory");
|
||||
}
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.EmPlanningServiceChecks.Run();
|
||||
Console.WriteLine("PASS em-planning-service");
|
||||
}
|
||||
if (args[0] == "coordinator" || args[0] == "em-all")
|
||||
{
|
||||
CoordinatorChecks.Run();
|
||||
Console.WriteLine("PASS coordinator");
|
||||
}
|
||||
if (args[0] == "executor" || args[0] == "em-all")
|
||||
{
|
||||
ExecutorChecks.Run();
|
||||
Console.WriteLine("PASS executor");
|
||||
}
|
||||
if (args[0] == "plugin-package" || args[0] == "em-all")
|
||||
{
|
||||
PluginPackagingChecks.Run();
|
||||
Console.WriteLine("PASS plugin-package");
|
||||
}
|
||||
if (args[0] == "trajectory-observation" || args[0] == "em-all")
|
||||
{
|
||||
TrajectoryObservationChecks.Run();
|
||||
Console.WriteLine("PASS trajectory-observation");
|
||||
}
|
||||
if (args[0] == "em-all")
|
||||
{
|
||||
CoordinatorChecks.RunRollingEndToEnd();
|
||||
Console.WriteLine("PASS rolling-end-to-end");
|
||||
ExecutorChecks.RunRollingEndToEnd();
|
||||
Console.WriteLine("PASS rolling-execution-tail");
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
Console.Error.WriteLine(exception);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,46 +0,0 @@
|
||||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class SegmentationChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
var referencePath = EMPlannerVerificationHost.EmFixtureFactory.CreateGearPairReferencePath();
|
||||
var segments = ReferencePathSegmenter.Create(referencePath);
|
||||
|
||||
EMPlannerVerificationHost.Verification.Equal(2, segments.Count, "segment count");
|
||||
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
|
||||
segments[0].EndBoundary.BoundaryType, "forward end boundary");
|
||||
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchDeparture,
|
||||
segments[1].StartBoundary.BoundaryType, "reverse start boundary");
|
||||
EMPlannerVerificationHost.Verification.True(!segments[0].EndBoundary.Equals(segments[1].StartBoundary),
|
||||
"gear-pair boundary identities differ");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[2].ArcLength, "forward source arc length");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[3].ArcLength, "reverse source arc length");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0d, segments[1].Points[0].ArcLength, "rebased reverse start");
|
||||
|
||||
ReferenceHorizonSlice rolling = ReferenceHorizonSlicer.Slice(segments[0], 1.95d);
|
||||
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.RollingSafetyStop,
|
||||
rolling.TerminalBoundary.BoundaryType, "rolling terminal type");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.TerminalBoundary.SegmentLocalS,
|
||||
"rolling terminal local s");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].ArcLength,
|
||||
"rolling anchor path s");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].X,
|
||||
"rolling anchor x");
|
||||
|
||||
ReferenceHorizonSlice gearSwitch = ReferenceHorizonSlicer.Slice(segments[0], 2.05d);
|
||||
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
|
||||
gearSwitch.TerminalBoundary.BoundaryType, "gear terminal type");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.TerminalBoundary.SegmentLocalS,
|
||||
"gear terminal local s");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.Points[gearSwitch.Points.Count - 1].ArcLength,
|
||||
"gear anchor path s");
|
||||
for (int index = 0; index < gearSwitch.Points.Count; index++)
|
||||
{
|
||||
EMPlannerVerificationHost.Verification.Equal(segments[0].Direction, gearSwitch.Points[index].Direction,
|
||||
"horizon point direction");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,551 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Reflection;
|
||||
using EMPlannerVerificationHost;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class TrajectoryChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesForwardFieldsExactTerminalAndHold();
|
||||
VerifiesSegmentLocalReferenceStationIsPublished();
|
||||
VerifiesReverseTravelVelocityAndUnwrappedYaw();
|
||||
VerifiesPublishedListsAreImmutable();
|
||||
VerifiesRollingTrajectoryHasNoSyntheticStopTail();
|
||||
VerifiesExactStopHoldHasNoJerkDiscontinuity();
|
||||
VerifiesWorldSpacePublicationMutationsAreRejected();
|
||||
VerifiesTerminalPoseAndSampleLimitPublicationGates();
|
||||
VerifiesReverseSpeedLimitUsesReverseConfiguration();
|
||||
VerifiesFullScopeNormalizesToleranceSizedPathSResiduals();
|
||||
VerifiesFullScopeRejectsMaterialPathSRegressionWithDiagnostics();
|
||||
}
|
||||
|
||||
private static void VerifiesForwardFieldsExactTerminalAndHold()
|
||||
{
|
||||
EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
|
||||
CreatePath(TravelDirection.Forward, 0d, Math.PI / 2d),
|
||||
CreateLongitudinalResult(),
|
||||
CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal));
|
||||
|
||||
VerifyKinematicFields(trajectory, TravelDirection.Forward, "forward");
|
||||
VerifyTerminalAndHold(trajectory, EmBoundaryType.Goal, "forward");
|
||||
}
|
||||
|
||||
private static void VerifiesReverseTravelVelocityAndUnwrappedYaw()
|
||||
{
|
||||
EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
|
||||
CreatePath(TravelDirection.Reverse, 3.10d, -3.10d),
|
||||
CreateLongitudinalResult(),
|
||||
CreateMetadata(TravelDirection.Reverse, EmTerminalType.GearSwitch));
|
||||
|
||||
VerifyKinematicFields(trajectory, TravelDirection.Reverse, "reverse");
|
||||
VerifyTerminalAndHold(trajectory, EmBoundaryType.GearSwitchApproach, "reverse");
|
||||
EmTrajectoryPoint moving = trajectory.Points[1];
|
||||
Verification.True(moving.SignedLongitudinalVelocity < 0d, "reverse signed velocity is negative");
|
||||
Verification.True(moving.VelocityX * Math.Cos(moving.Yaw) + moving.VelocityY * Math.Sin(moving.Yaw) < 0d,
|
||||
"reverse world velocity points opposite the vehicle yaw");
|
||||
Verification.True(Math.Abs(Math.Abs(moving.Yaw) - Math.PI) < 0.1d,
|
||||
"reverse yaw interpolation unwraps across the pi boundary");
|
||||
}
|
||||
|
||||
private static void VerifiesSegmentLocalReferenceStationIsPublished()
|
||||
{
|
||||
EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
|
||||
CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
|
||||
CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal));
|
||||
|
||||
EmTrajectoryPoint terminal = trajectory.Points[trajectory.Points.Count - 1];
|
||||
Verification.NearlyEqual(1d, terminal.SegmentLocalS,
|
||||
"published SegmentLocalS retains the lateral path reference station");
|
||||
Verification.NearlyEqual(0.12d, terminal.PathS,
|
||||
"published PathS remains the longitudinal actual path distance");
|
||||
|
||||
ValidationContext context = CreateValidationContext();
|
||||
EmTrajectory validationTrajectory = CreateValidationTrajectory(TravelDirection.Forward);
|
||||
EmTrajectoryValidationResult accepted = new EmTrajectoryValidator().Validate(validationTrajectory, context.EmptyMap,
|
||||
context.Vehicle, context.Configuration, 2, 1d, 0.0055d, EmBoundaryType.Goal);
|
||||
Verification.True(accepted.IsValid,
|
||||
"publication validates reference and actual path bounds independently: " + accepted.Message);
|
||||
EmTrajectoryValidationResult segmentExceeded = new EmTrajectoryValidator().Validate(validationTrajectory,
|
||||
context.EmptyMap, context.Vehicle, context.Configuration, 2, 0.5d, 0.0055d, EmBoundaryType.Goal);
|
||||
Verification.Equal(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, segmentExceeded.Failure,
|
||||
"segment-local reference bound is checked independently of PathS");
|
||||
}
|
||||
|
||||
private static void VerifiesPublishedListsAreImmutable()
|
||||
{
|
||||
EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
|
||||
CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
|
||||
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
|
||||
EmLongitudinalMode.RollingContinuation));
|
||||
Verification.True(!(trajectory.Points is IList<EmTrajectoryPoint> mutable) || mutable.IsReadOnly,
|
||||
"trajectory public point list is immutable");
|
||||
}
|
||||
|
||||
private static void VerifiesRollingTrajectoryHasNoSyntheticStopTail()
|
||||
{
|
||||
EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
|
||||
CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),
|
||||
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
|
||||
EmLongitudinalMode.RollingContinuation));
|
||||
|
||||
Verification.Equal(21, trajectory.Points.Count, "rolling trajectory keeps only ST knots");
|
||||
EmTrajectoryPoint terminal = trajectory.Points[trajectory.Points.Count - 1];
|
||||
Verification.Equal(EmBoundaryType.None, terminal.BoundaryType,
|
||||
"rolling horizon end is not a boundary anchor");
|
||||
Verification.True(terminal.SignedLongitudinalVelocity > 0d,
|
||||
"rolling terminal speed stays nonzero");
|
||||
Verification.Equal(EmLongitudinalMode.RollingContinuation, trajectory.Metadata.LongitudinalMode,
|
||||
"rolling longitudinal mode is published");
|
||||
}
|
||||
|
||||
private static void VerifiesExactStopHoldHasNoJerkDiscontinuity()
|
||||
{
|
||||
ValidationContext context = CreateValidationContext();
|
||||
EmTrajectory trajectory = CreateValidationTrajectory(TravelDirection.Forward);
|
||||
EmTrajectoryValidationResult result = new EmTrajectoryValidator().Validate(trajectory, context.EmptyMap,
|
||||
context.Vehicle, context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
|
||||
Verification.True(result.IsValid, "exact stop with internal tail and external hold is publishable: " + result.Message);
|
||||
|
||||
int anchorIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
EmTrajectoryPoint previousMoving = trajectory.Points[anchorIndex - 1];
|
||||
EmTrajectoryPoint anchor = trajectory.Points[anchorIndex];
|
||||
EmTrajectoryPoint stabilization = trajectory.Points[anchorIndex + 1];
|
||||
EmTrajectoryPoint firstExternalHold = trajectory.Points[anchorIndex + 2];
|
||||
Verification.Equal(EmBoundaryType.Goal, anchor.BoundaryType, "exact stop marks only the real boundary anchor");
|
||||
Verification.Equal(EmBoundaryType.None, stabilization.BoundaryType,
|
||||
"QP stabilization point is not a duplicate boundary anchor");
|
||||
Verification.Equal(EmBoundaryType.None, firstExternalHold.BoundaryType,
|
||||
"external hold is not a duplicate boundary anchor");
|
||||
|
||||
double terminalFiniteDifferenceAcceleration =
|
||||
(anchor.SignedLongitudinalVelocity - previousMoving.SignedLongitudinalVelocity) /
|
||||
(anchor.TimeFromStart - previousMoving.TimeFromStart);
|
||||
double stabilizationAcceleration =
|
||||
(stabilization.SignedLongitudinalVelocity - anchor.SignedLongitudinalVelocity) /
|
||||
(stabilization.TimeFromStart - anchor.TimeFromStart);
|
||||
double externalHoldAcceleration =
|
||||
(firstExternalHold.SignedLongitudinalVelocity - stabilization.SignedLongitudinalVelocity) /
|
||||
(firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
|
||||
double jerkIntoStabilization = (stabilizationAcceleration - terminalFiniteDifferenceAcceleration) /
|
||||
(stabilization.TimeFromStart - anchor.TimeFromStart);
|
||||
double jerkIntoExternalHold = (externalHoldAcceleration - stabilizationAcceleration) /
|
||||
(firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
|
||||
Verification.True(Math.Abs(jerkIntoStabilization) <= 0.5d,
|
||||
"jerk into QP stabilization stays within the limit");
|
||||
Verification.NearlyEqual(0d, jerkIntoExternalHold, "jerk into the external hold is zero");
|
||||
}
|
||||
|
||||
private static void VerifiesWorldSpacePublicationMutationsAreRejected()
|
||||
{
|
||||
ValidationContext context = CreateValidationContext();
|
||||
EmTrajectory valid = CreateValidationTrajectory(TravelDirection.Forward);
|
||||
EmTrajectoryValidationResult accepted = new EmTrajectoryValidator().Validate(valid, context.EmptyMap, context.Vehicle,
|
||||
context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
|
||||
Verification.True(accepted.IsValid, "valid world-space trajectory is publishable: " + accepted.Message);
|
||||
|
||||
AssertRejected(context, CorruptDouble(valid, 1, "X", double.NaN), EmTrajectoryValidationFailure.NonFinite, 1,
|
||||
"non-finite point");
|
||||
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], timeFromStart: valid.Points[0].TimeFromStart)),
|
||||
EmTrajectoryValidationFailure.TimeNotStrictlyIncreasing, 1, "non-increasing time");
|
||||
AssertRejected(context, Replace(valid, 2, Clone(valid.Points[2], pathS: 0.0005d)),
|
||||
EmTrajectoryValidationFailure.PathSDecreased, 2, "decreasing PathS");
|
||||
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], signedSpeed: -valid.Points[1].Speed)),
|
||||
EmTrajectoryValidationFailure.DirectionSignMismatch, 1, "direction sign");
|
||||
AssertRejected(context, CorruptDouble(valid, 1, "Speed", valid.Points[1].Speed + 0.01d),
|
||||
EmTrajectoryValidationFailure.RedundantSpeedMismatch, 1, "redundant speed");
|
||||
AssertRejected(context, CorruptDouble(valid, 1, "VelocityX", valid.Points[1].VelocityX + 0.01d),
|
||||
EmTrajectoryValidationFailure.WorldVelocityMismatch, 1, "world velocity");
|
||||
AssertRejected(context, CorruptDouble(valid, 1, "YawRate", valid.Points[1].YawRate + 0.01d),
|
||||
EmTrajectoryValidationFailure.YawRateMismatch, 1, "yaw rate");
|
||||
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], signedSpeed: 1.10d)),
|
||||
EmTrajectoryValidationFailure.SpeedLimitExceeded, 1, "speed limit");
|
||||
AssertRejected(context, Replace(valid, 2, Clone(valid.Points[2], signedSpeed: 0.19d)),
|
||||
EmTrajectoryValidationFailure.AccelerationLimitExceeded, 2, "acceleration limit");
|
||||
|
||||
EmTrajectory jerkMutated = Replace(valid, 1, Clone(valid.Points[1], signedSpeed: 0.035d));
|
||||
jerkMutated = Replace(jerkMutated, 2, Clone(jerkMutated.Points[2], signedSpeed: 0.03d));
|
||||
AssertRejected(context, jerkMutated, EmTrajectoryValidationFailure.JerkLimitExceeded, 2, "jerk limit");
|
||||
EmTrajectoryValidationResult jerkResult = new EmTrajectoryValidator().Validate(
|
||||
jerkMutated, context.EmptyMap, context.Vehicle, context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
|
||||
Verification.Equal(EmTrajectoryValidationFailure.JerkLimitExceeded, jerkResult.Failure,
|
||||
"jerk diagnostic failure code");
|
||||
Verification.Equal(2, jerkResult.PointIndex, "jerk diagnostic point index");
|
||||
foreach (string field in new[]
|
||||
{
|
||||
"time=", "dt=", "previousAcceleration=", "acceleration=", "jerk=", "limit=", "excess=",
|
||||
"storedPreviousJerk=", "storedCurrentJerk=",
|
||||
})
|
||||
{
|
||||
Verification.True(jerkResult.Message.Contains(field), "jerk diagnostic includes " + field);
|
||||
}
|
||||
|
||||
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], vehicleCurvature: 2d)),
|
||||
EmTrajectoryValidationFailure.CurvatureLimitExceeded, 1, "curvature limit");
|
||||
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], vehicleCurvature: 0.75d)),
|
||||
EmTrajectoryValidationFailure.CurvatureRateLimitExceeded, 1, "curvature-rate limit");
|
||||
|
||||
int terminalIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
AssertRejected(context, Replace(valid, terminalIndex, Clone(valid.Points[terminalIndex], boundaryType: EmBoundaryType.None)),
|
||||
EmTrajectoryValidationFailure.MissingTerminalAnchor, terminalIndex - 1, "missing exact terminal anchor");
|
||||
AssertRejected(context, Replace(valid, terminalIndex, Clone(valid.Points[terminalIndex], signedSpeed: 0.01d)),
|
||||
EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex, "terminal speed");
|
||||
AssertRejected(context, CorruptDouble(valid, terminalIndex, "LongitudinalAcceleration", 0.01d),
|
||||
EmTrajectoryValidationFailure.TerminalAccelerationNotZero, terminalIndex, "terminal acceleration");
|
||||
AssertRejected(context, CorruptDouble(valid, terminalIndex, "YawRate", 0.01d),
|
||||
EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex, "terminal yaw rate");
|
||||
|
||||
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], x: 1d)), context.PoseCollisionMap,
|
||||
EmTrajectoryValidationFailure.PoseCollision, 1, "pose collision");
|
||||
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], x: 1d)), context.SweptCollisionMap,
|
||||
EmTrajectoryValidationFailure.SweptCollision, 1, "swept collision");
|
||||
EmTrajectoryValidationResult beyondSegment = new EmTrajectoryValidator().Validate(valid, context.EmptyMap, context.Vehicle,
|
||||
context.Configuration, 2, 0.004d, EmBoundaryType.Goal);
|
||||
Verification.Equal(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, beyondSegment.Failure,
|
||||
"segment-boundary failure code");
|
||||
Verification.Equal(4, beyondSegment.PointIndex, "segment-boundary first point");
|
||||
}
|
||||
|
||||
private static void VerifiesTerminalPoseAndSampleLimitPublicationGates()
|
||||
{
|
||||
ValidationContext context = CreateValidationContext();
|
||||
EmTrajectory valid = CreateValidationTrajectory(TravelDirection.Forward);
|
||||
int terminalIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||||
CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
|
||||
EmTrajectoryPoint terminal = valid.Points[terminalIndex];
|
||||
var validator = new EmTrajectoryValidator();
|
||||
|
||||
EmTrajectoryValidationResult exactPosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
|
||||
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, terminal.Yaw),
|
||||
EmBoundaryType.Goal);
|
||||
Verification.True(exactPosition.IsValid, "exact terminal position is accepted");
|
||||
EmTrajectoryValidationResult outsidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
|
||||
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.001d, terminal.Y, terminal.Yaw),
|
||||
EmBoundaryType.Goal);
|
||||
Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsidePosition.Failure,
|
||||
"1-millimetre terminal position error is rejected");
|
||||
|
||||
double degrees = Math.PI / 180d;
|
||||
EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: Math.PI));
|
||||
EmTrajectoryValidationResult wrapped = validator.Validate(wrappedYaw, context.EmptyMap, context.Vehicle,
|
||||
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -Math.PI),
|
||||
EmBoundaryType.Goal);
|
||||
Verification.True(wrapped.IsValid, "PI and -PI use normalized yaw error");
|
||||
EmTrajectoryValidationResult outsideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
|
||||
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -0.1d * degrees),
|
||||
EmBoundaryType.Goal);
|
||||
Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsideYaw.Failure,
|
||||
"0.1-degree terminal yaw error is rejected");
|
||||
|
||||
EmTrajectory rolling = new EmTrajectoryAssembler().Assemble(
|
||||
CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),
|
||||
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
|
||||
EmLongitudinalMode.RollingContinuation));
|
||||
EmTrajectoryValidationResult rollingResult = validator.Validate(rolling, context.EmptyMap, context.Vehicle,
|
||||
context.Configuration, 2, 0.08d, new Pose2D(99d, 99d, Math.PI), EmBoundaryType.RollingSafetyStop);
|
||||
Verification.True(rollingResult.IsValid, "rolling-safety window end does not use the terminal-pose gate");
|
||||
|
||||
EmPlannerConfiguration limitedConfiguration = EmPlannerConfiguration.CreateDefault();
|
||||
limitedConfiguration.Scheduling.MaximumPublishedSampleCount = 8;
|
||||
EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(limitedConfiguration).TryAssemble(
|
||||
CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
|
||||
CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal), out EmTrajectory limitedTrajectory,
|
||||
out string assemblyFailure);
|
||||
Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, assemblyStatus,
|
||||
"sample schedule one above the limit is rejected");
|
||||
Verification.True(limitedTrajectory == null, "sample-limit rejection does not assemble a partial trajectory");
|
||||
Verification.True(assemblyFailure.IndexOf("MaximumPublishedSampleCount", StringComparison.Ordinal) >= 0,
|
||||
"sample-limit rejection preserves a diagnostic");
|
||||
Verification.True(assemblyFailure.IndexOf("required-at-least=9", StringComparison.Ordinal) >= 0,
|
||||
"sample-limit fixture exceeds the cap by exactly one sample");
|
||||
}
|
||||
|
||||
private static void VerifiesReverseSpeedLimitUsesReverseConfiguration()
|
||||
{
|
||||
ValidationContext context = CreateValidationContext();
|
||||
context.Configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
|
||||
context.Configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 0.02d;
|
||||
EmTrajectoryValidationResult result = new EmTrajectoryValidator().Validate(
|
||||
CreateValidationTrajectory(TravelDirection.Reverse), context.EmptyMap, context.Vehicle, context.Configuration,
|
||||
2, 0.0055d, EmBoundaryType.Goal);
|
||||
Verification.Equal(EmTrajectoryValidationFailure.SpeedLimitExceeded, result.Failure,
|
||||
"reverse speed uses the configured reverse limit");
|
||||
Verification.Equal(0, result.PointIndex, "reverse speed first over-limit point");
|
||||
}
|
||||
|
||||
private static void AssertRejected(ValidationContext context, EmTrajectory trajectory,
|
||||
EmTrajectoryValidationFailure expectedFailure, int expectedIndex, string name)
|
||||
{
|
||||
AssertRejected(context, trajectory, context.EmptyMap, expectedFailure, expectedIndex, name);
|
||||
}
|
||||
|
||||
private static void AssertRejected(ValidationContext context, EmTrajectory trajectory, PlanningGridMap map,
|
||||
EmTrajectoryValidationFailure expectedFailure, int expectedIndex, string name)
|
||||
{
|
||||
EmTrajectoryValidationResult result = new EmTrajectoryValidator().Validate(trajectory, map, context.Vehicle,
|
||||
context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
|
||||
Verification.True(!result.IsValid, name + " is rejected");
|
||||
Verification.Equal(expectedFailure, result.Failure, name + " failure code");
|
||||
Verification.Equal(expectedIndex, result.PointIndex, name + " failure index");
|
||||
}
|
||||
|
||||
private static ValidationContext CreateValidationContext()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
return new ValidationContext(configuration, new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.01d,
|
||||
WidthMeters = 0.01d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 1d,
|
||||
},
|
||||
CreateValidationMap(Array.Empty<IMapObstacle>()),
|
||||
CreateValidationMap(new IMapObstacle[] { new AxisAlignedRectangleObstacle(990f, 1010f, -10f, 10f) }),
|
||||
CreateValidationMap(new IMapObstacle[] { new AxisAlignedRectangleObstacle(490f, 510f, -10f, 10f) }));
|
||||
}
|
||||
|
||||
private static PlanningGridMap CreateValidationMap(IReadOnlyList<IMapObstacle> obstacles)
|
||||
{
|
||||
IMapObstacleSource[] sources = obstacles.Count == 0
|
||||
? Array.Empty<IMapObstacleSource>()
|
||||
: new IMapObstacleSource[] { new ManualObstacleSource("trajectory-validator", 1L, true, obstacles) };
|
||||
PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f),
|
||||
ResolutionMm = 20f,
|
||||
ObstacleSources = sources,
|
||||
AllowExplicitEmptyMap = obstacles.Count == 0,
|
||||
});
|
||||
Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady,
|
||||
"trajectory-validator map builds: " + result.FailureReason);
|
||||
return result.Map!;
|
||||
}
|
||||
|
||||
private static EmTrajectory CreateValidationTrajectory(TravelDirection direction)
|
||||
{
|
||||
LongitudinalCandidate candidate = CreateValidationCandidate();
|
||||
var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
|
||||
var path = new LateralPath(new[]
|
||||
{
|
||||
new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d),
|
||||
new LateralPathPoint(1d, 0.0055d, 0d, 0d, 0d, 0d, 0.0055d, 0d, 0d, 0d, 0d, 0d),
|
||||
}, true);
|
||||
return new EmTrajectoryAssembler().Assemble(path, result, CreateMetadata(direction, EmTerminalType.Goal));
|
||||
}
|
||||
|
||||
private static LongitudinalCandidate CreateValidationCandidate()
|
||||
{
|
||||
double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d, 0.25d, 0.30d, 0.35d, 0.40d, 0.45d, 0.50d };
|
||||
double[] pathS = { 0d, 0.0014375d, 0.00271875d, 0.00378125d, 0.0045625d, 0.0050625d, 0.00534375d,
|
||||
0.00546875d, 0.0055d, 0.0055d, 0.0055d };
|
||||
double[] speed = { 0.03d, 0.0275d, 0.02375d, 0.01875d, 0.0125d, 0.0075d, 0.00375d, 0.00125d, 0d, 0d, 0d };
|
||||
double[] acceleration = { -0.05d, -0.075d, -0.10d, -0.125d, -0.10d, -0.075d, -0.05d, -0.025d, 0d, 0d, 0d };
|
||||
double[] jerk = { -0.5d, -0.5d, -0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0d, 0d };
|
||||
return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
|
||||
}
|
||||
|
||||
private static EmTrajectory Replace(EmTrajectory trajectory, int index, EmTrajectoryPoint replacement)
|
||||
{
|
||||
var points = new List<EmTrajectoryPoint>(trajectory.Points);
|
||||
points[index] = replacement;
|
||||
return new EmTrajectory(trajectory.Metadata, points);
|
||||
}
|
||||
|
||||
private static EmTrajectory CorruptDouble(EmTrajectory trajectory, int index, string propertyName, double value)
|
||||
{
|
||||
EmTrajectoryPoint replacement = Clone(trajectory.Points[index]);
|
||||
FieldInfo field = typeof(EmTrajectoryPoint).GetField("<" + propertyName + ">k__BackingField",
|
||||
BindingFlags.Instance | BindingFlags.NonPublic) ?? throw new InvalidOperationException("Missing backing field " + propertyName);
|
||||
field.SetValue(replacement, value);
|
||||
return Replace(trajectory, index, replacement);
|
||||
}
|
||||
|
||||
private static EmTrajectoryPoint Clone(EmTrajectoryPoint point, double? x = null, double? y = null, double? yaw = null,
|
||||
double? timeFromStart = null, double? signedSpeed = null, double? pathS = null, double? vehicleCurvature = null,
|
||||
EmBoundaryType? boundaryType = null)
|
||||
{
|
||||
return new EmTrajectoryPoint(x ?? point.X, y ?? point.Y, yaw ?? point.Yaw, signedSpeed ?? point.SignedLongitudinalVelocity,
|
||||
timeFromStart ?? point.TimeFromStart, vehicleCurvature ?? point.VehicleCurvature, point.SegmentIndex,
|
||||
pathS ?? point.SegmentLocalS, pathS ?? point.PathS, point.Direction, boundaryType ?? point.BoundaryType,
|
||||
0d, 0d);
|
||||
}
|
||||
|
||||
private sealed class ValidationContext
|
||||
{
|
||||
public ValidationContext(EmPlannerConfiguration configuration, VehicleParameters vehicle, PlanningGridMap emptyMap,
|
||||
PlanningGridMap poseCollisionMap, PlanningGridMap sweptCollisionMap)
|
||||
{
|
||||
Configuration = configuration;
|
||||
Vehicle = vehicle;
|
||||
EmptyMap = emptyMap;
|
||||
PoseCollisionMap = poseCollisionMap;
|
||||
SweptCollisionMap = sweptCollisionMap;
|
||||
}
|
||||
|
||||
public EmPlannerConfiguration Configuration { get; }
|
||||
public VehicleParameters Vehicle { get; }
|
||||
public PlanningGridMap EmptyMap { get; }
|
||||
public PlanningGridMap PoseCollisionMap { get; }
|
||||
public PlanningGridMap SweptCollisionMap { get; }
|
||||
}
|
||||
|
||||
private static void VerifyKinematicFields(EmTrajectory trajectory, TravelDirection direction, string name)
|
||||
{
|
||||
double directionSign = direction == TravelDirection.Forward ? 1d : -1d;
|
||||
double previousTime = double.NegativeInfinity;
|
||||
double previousPathS = double.NegativeInfinity;
|
||||
for (int index = 0; index < trajectory.Points.Count; index++)
|
||||
{
|
||||
EmTrajectoryPoint point = trajectory.Points[index];
|
||||
Verification.NearlyEqual(Math.Abs(point.SignedLongitudinalVelocity), point.Speed,
|
||||
name + " speed field " + index);
|
||||
Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Cos(point.Yaw), point.VelocityX,
|
||||
name + " velocity X field " + index);
|
||||
Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Sin(point.Yaw), point.VelocityY,
|
||||
name + " velocity Y field " + index);
|
||||
Verification.NearlyEqual(point.SignedLongitudinalVelocity * point.VehicleCurvature, point.YawRate,
|
||||
name + " yaw-rate field " + index);
|
||||
if (index < 4)
|
||||
Verification.NearlyEqual(directionSign * CreateLongitudinalResult().Candidate.U[index],
|
||||
point.SignedLongitudinalVelocity, name + " signed speed field " + index);
|
||||
Verification.True(point.TimeFromStart > previousTime, name + " time strictly increases " + index);
|
||||
Verification.True(point.PathS >= previousPathS, name + " PathS never decreases " + index);
|
||||
previousTime = point.TimeFromStart;
|
||||
previousPathS = point.PathS;
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyTerminalAndHold(EmTrajectory trajectory, EmBoundaryType terminalBoundary, string name)
|
||||
{
|
||||
const int terminalIndex = 3;
|
||||
EmTrajectoryPoint terminal = trajectory.Points[terminalIndex];
|
||||
Verification.Equal(terminalBoundary, terminal.BoundaryType, name + " exact terminal boundary type");
|
||||
Verification.NearlyEqual(0d, terminal.SignedLongitudinalVelocity, name + " exact terminal signed speed");
|
||||
Verification.NearlyEqual(0d, terminal.YawRate, name + " exact terminal yaw rate");
|
||||
Verification.NearlyEqual(0.15d, terminal.TimeFromStart, name + " exact terminal stabilization time");
|
||||
Verification.Equal(terminalIndex + 6, trajectory.Points.Count, name + " terminal plus 0.20-second hold samples");
|
||||
|
||||
for (int index = terminalIndex + 1; index < trajectory.Points.Count; index++)
|
||||
{
|
||||
EmTrajectoryPoint hold = trajectory.Points[index];
|
||||
Verification.NearlyEqual(terminal.TimeFromStart + (index - terminalIndex) * 0.05d,
|
||||
hold.TimeFromStart, name + " hold timing " + index);
|
||||
Verification.NearlyEqual(terminal.X, hold.X, name + " hold X " + index);
|
||||
Verification.NearlyEqual(terminal.Y, hold.Y, name + " hold Y " + index);
|
||||
Verification.NearlyEqual(terminal.Yaw, hold.Yaw, name + " hold yaw " + index);
|
||||
Verification.NearlyEqual(0d, hold.SignedLongitudinalVelocity, name + " hold signed speed " + index);
|
||||
Verification.NearlyEqual(0d, hold.YawRate, name + " hold yaw rate " + index);
|
||||
}
|
||||
}
|
||||
|
||||
private static LateralPath CreatePath(TravelDirection direction, double firstYaw, double lastYaw)
|
||||
{
|
||||
return new LateralPath(new[]
|
||||
{
|
||||
new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, firstYaw, 0d, 0.5d, 0d),
|
||||
new LateralPathPoint(1d, 0.12d, 0d, 0d, 0d, 0d, 0.12d, 0d, lastYaw, 0d, 0.5d, 0d),
|
||||
}, true);
|
||||
}
|
||||
|
||||
private static LongitudinalPlanningResult CreateLongitudinalResult()
|
||||
{
|
||||
var candidate = new LongitudinalCandidate(
|
||||
new[] { 0d, 0.05d, 0.10d, 0.15d, 0.20d },
|
||||
new[] { 0d, 0.04d, 0.08d, 0.12d, 0.12d },
|
||||
new[] { 0.8d, 0.8d, 0.2d, 0d, 0d },
|
||||
new[] { 0d, 0d, 0d, 0d, 0d },
|
||||
new[] { 0d, 0d, 0d, 0d });
|
||||
return new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
|
||||
}
|
||||
|
||||
private static LongitudinalPlanningResult CreateRollingLongitudinalResult()
|
||||
{
|
||||
var times = new double[21];
|
||||
var pathS = new double[21];
|
||||
var speed = new double[21];
|
||||
var acceleration = new double[21];
|
||||
var jerk = new double[20];
|
||||
for (int index = 0; index < times.Length; index++)
|
||||
{
|
||||
times[index] = index * 0.1d;
|
||||
pathS[index] = index * 0.004d;
|
||||
speed[index] = 0.04d;
|
||||
}
|
||||
return new LongitudinalPlanningResult(EmPlanningStatus.Success,
|
||||
new LongitudinalCandidate(times, pathS, speed, acceleration, jerk), string.Empty);
|
||||
}
|
||||
|
||||
private static void VerifiesFullScopeNormalizesToleranceSizedPathSResiduals()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
|
||||
var candidate = new LongitudinalCandidate(
|
||||
new[] { 0d, 0.1d, 0.2d, 0.3d },
|
||||
new[] { 0d, 0.05d, 0.05d - 0.5d * configuration.Validation.KinematicTolerance, 0.10d },
|
||||
new[] { 0.5d, 0d, 0d, 0d },
|
||||
new[] { 0d, 0d, 0d, 0d },
|
||||
new[] { 0d, 0d, 0d });
|
||||
|
||||
EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(
|
||||
CreatePath(TravelDirection.Forward, 0d, 0d),
|
||||
new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty),
|
||||
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
|
||||
EmLongitudinalMode.RollingContinuation, EmPlanningScope.FullDirectionSegment));
|
||||
|
||||
for (int index = 1; index < trajectory.Points.Count; index++)
|
||||
{
|
||||
Verification.True(trajectory.Points[index].PathS >= trajectory.Points[index - 1].PathS,
|
||||
"full-scope numerical residual is normalized at sample " + index);
|
||||
}
|
||||
Verification.True(trajectory.Points[trajectory.Points.Count - 1].PathS <= candidate.S[candidate.S.Count - 1],
|
||||
"full-scope publication remains within terminal PathS");
|
||||
}
|
||||
|
||||
private static void VerifiesFullScopeRejectsMaterialPathSRegressionWithDiagnostics()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
|
||||
var candidate = new LongitudinalCandidate(
|
||||
new[] { 0d, 0.1d, 0.2d, 0.3d },
|
||||
new[] { 0d, 0.05d, 0.04d, 0.10d },
|
||||
new[] { 0.5d, 0d, 0d, 0d },
|
||||
new[] { 0d, 0d, 0d, 0d },
|
||||
new[] { 0d, 0d, 0d });
|
||||
|
||||
try
|
||||
{
|
||||
new EmTrajectoryAssembler(configuration).Assemble(
|
||||
CreatePath(TravelDirection.Forward, 0d, 0d),
|
||||
new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty),
|
||||
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
|
||||
EmLongitudinalMode.RollingContinuation, EmPlanningScope.FullDirectionSegment));
|
||||
throw new InvalidOperationException("Material PathS regression was accepted.");
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
Verification.True(exception.Message.Contains("sampleIndex=2"),
|
||||
"PathS diagnostic contains sample index");
|
||||
Verification.True(exception.Message.Contains("previousPathS="),
|
||||
"PathS diagnostic contains previous value");
|
||||
Verification.True(exception.Message.Contains("candidatePathS="),
|
||||
"PathS diagnostic contains candidate value");
|
||||
Verification.True(exception.Message.Contains("difference="),
|
||||
"PathS diagnostic contains difference");
|
||||
}
|
||||
}
|
||||
|
||||
private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType,
|
||||
EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary,
|
||||
EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
|
||||
{
|
||||
return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
|
||||
"reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode,
|
||||
planningScope);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user