diff --git a/ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/README.md b/ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/README.md index d69e127..fbde6b7 100644 --- a/ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/README.md +++ b/ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/README.md @@ -29,7 +29,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 1:定义固定世界起点和终点 -[对应完整示例:步骤 1](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 1](EmPlannerFullPipelineDemo.cs#L40) **本步输入**:固定场景常量(世界坐标 m、航向 rad)。 **调用**:构造 `Pose2D` 起点和目标。 @@ -38,7 +38,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 2:定义车辆、曲率和搜索预算 -[对应完整示例:步骤 2](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 2](EmPlannerFullPipelineDemo.cs#L50) **本步输入**:车辆几何中心尺寸、`SafetyMarginMeters` 与可验证曲率限制。 **调用**:构造 `VehicleParameters` 和 `HybridAStarConfiguration`。 @@ -47,7 +47,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 3:建立静态地图 -[对应完整示例:步骤 3](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 3](EmPlannerFullPipelineDemo.cs#L84) **本步输入**:固定障碍物、地图边界与栅格分辨率。 **调用**:建立并冻结 `PlanningGridMap`。 @@ -56,7 +56,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 4:建立粗路径请求 -[对应完整示例:步骤 4](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 4](EmPlannerFullPipelineDemo.cs#L120) **本步输入**:步骤 1–3 的位姿、地图、车辆和 Hybrid A* 配置。 **调用**:构造 `PlanningRequest`。 @@ -65,7 +65,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 5:搜索粗路径 -[对应完整示例:步骤 5](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 5](EmPlannerFullPipelineDemo.cs#L133) **本步输入**:`PlanningRequest`。 **调用**:`HybridAStarPlanner.Plan(request, cancellationToken)`。 @@ -74,7 +74,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 6:请求并执行 Local G2 平滑 -[对应完整示例:步骤 6](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 6](EmPlannerFullPipelineDemo.cs#L144) **本步输入**:成功粗路径的 `Path`、`Segments`、同一地图/车辆和 `PathSmoothingConfiguration`。 **调用**:`new PathSmoothingRequest(...)`,再调用 `PathSmoothingService.Smooth`。 @@ -83,7 +83,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 7:选择一个方向段 -[对应完整示例:步骤 7](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 7](EmPlannerFullPipelineDemo.cs#L184) **本步输入**:平滑结果的单个 `SmoothedPathSegment`。 **调用**:建立/取得 `DirectionSegmentView`。 @@ -92,7 +92,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 8:建立初始运动状态 -[对应完整示例:步骤 8](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 8](EmPlannerFullPipelineDemo.cs#L198) **本步输入**:固定场景的世界位姿、车体纵向有符号速度、状态时间与序列号。 **调用**:构造 `VehicleMotionState`。 @@ -101,7 +101,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 9:冻结 EM 配置 -[对应完整示例:步骤 9](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 9](EmPlannerFullPipelineDemo.cs#L207) **本步输入**:低速泊车配置树。 **调用**:`EmPlannerConfiguration.CreateDefault()` 后在请求前完成本场景修改。 @@ -110,7 +110,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 10:建立 EM 请求 -[对应完整示例:步骤 10](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 10](EmPlannerFullPipelineDemo.cs#L225) **本步输入**:平滑成功结果、冻结地图、车辆、状态、配置和活动 `segmentIndex`。 **调用**:构造 `EmPlanningRequest`,给出请求/生效 UTC、输出 ID、参考 ID、`EmMotionModel` 与 `FullDirectionSegment`。 @@ -119,7 +119,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 11:调用 EM 服务 -[对应完整示例:步骤 11](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 11](EmPlannerFullPipelineDemo.cs#L245) **本步输入**:`EmPlanningRequest` 和调用方 `CancellationToken`。 **调用**:`new EmPlanningService(new OsqpNativeSolver()).Plan(request, cancellationToken)`。 @@ -128,7 +128,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 12:判断是否可发布 -[对应完整示例:步骤 12](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 12](EmPlannerFullPipelineDemo.cs#L252) **本步输入**:`Status`、`FailureReason` 与 `Trajectory`。 **调用**:只接受 `Success` 或 `SuccessWithFallback`,再检查轨迹非空及控制点数。 @@ -137,7 +137,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 13:读取输出契约 -[对应完整示例:步骤 13](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 13](EmPlannerFullPipelineDemo.cs#L264) **本步输入**:成功 `EmTrajectory`。 **调用**:读取只读 `Metadata` 与 `Points`,用于身份、方向、边界和诊断。 @@ -146,7 +146,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 14:适配为控制轨迹 -[对应完整示例:步骤 14](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 14](EmPlannerFullPipelineDemo.cs#L304) **本步输入**:步骤 12 的同一方向完整 `EmTrajectory`。 **调用**:`new EmControlTrajectoryAdapter().Create(emTrajectory)`。 @@ -155,7 +155,7 @@ PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan ### 步骤 15:交给闭环动作 -[对应完整示例:步骤 15](EmPlannerFullPipelineDemo.cs) +[对应完整示例:步骤 15](EmPlannerFullPipelineDemo.cs#L308) **本步输入**:`Trajectory2D`、控制器维护者拥有的 `StateProvider` 与调参。 **调用**:赋给 `TrajectoryTrackingMovement.Trajectory`(或由宿主包装为 `TrackMotionPlanSegment`)。