chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,457 @@
|
||||
# EM Observation MovementTest Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (- [ ]) syntax for tracking.
|
||||
|
||||
**Goal:** Build a real-localization, observe-only MovementTest that creates a configurable start/goal map, plans Hybrid A* → Local G2 → EM trajectories, and shows world, LS, and ST diagnostics without sending a chassis command.
|
||||
|
||||
**Architecture:** Put map construction, planning bootstrap, rolling EM requests, trajectory observation, and LS/ST derivation in pure, testable classes. Keep MDCS reads, prompts, painters, background timing, and cancellation in one thin MovementTest host. The host may only read DetourInterface and BasicPilotBase.Chassis; its only control output is a displayed TrajectoryControlCommand.
|
||||
|
||||
**Tech Stack:** C# 10, netstandard2.0, existing Clumsy MovementTest/Painter UI, MDCS localization and chassis read APIs, Hybrid A*, Local G2, EM planner, OSQP, and EMPlannerVerificationHost.
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- All map geometry and UI world coordinates are mm; Pose2D, velocities, and EM geometry are m, m/s, and rad.
|
||||
- Bounds are exactly the start/goal axis-aligned rectangle expanded by MapPaddingMeters on all sides. Obstacles must fit these bounds; they must not enlarge them.
|
||||
- Default settings are: padding 2.0 m, resolution 50 mm, replan 0.20 s, observer period 0.05 s.
|
||||
- Capture world pose through DetourInterface.getCartLocation() and signed body-longitudinal velocity from BasicPilotBase.Chassis.GetCarSpeed(true).Vx. Create a monotonically increasing state sequence id.
|
||||
- Output is TrajectoryControlCommand for display only. Do not invoke SendXYThSpeed, SendMotion, SendTh, AccumulateSpeed, ComputeWheelsGeometrically, brake/wheel adapter methods, or a geometric controller.
|
||||
- Keep runtime source under ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest. Do not change the existing coarse-path factory, whose unrelated manual demo uses an 8 m expansion.
|
||||
- Stop/cancel must cancel worker activity and clear the World, LS, and ST painter layers.
|
||||
|
||||
---
|
||||
|
||||
## File structure
|
||||
|
||||
| File | Responsibility |
|
||||
| --- | --- |
|
||||
| ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs | Settings, manual-obstacle DTOs, validation, exact map-job construction. |
|
||||
| ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs | Hybrid A* + Local G2 bootstrap, rolling EM requests, time observation, LS/ST models. |
|
||||
| ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs | Three painter layers and presentation text; no MDCS/hardware use. |
|
||||
| ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs | Discoverable test, MDCS state reader, prompts, background session, console, cancellation. |
|
||||
| ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md | Operator configuration, layer interpretation, unit and safety guidance. |
|
||||
| ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs | Deterministic regression checks and an actuator-call source audit. |
|
||||
| ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs | Adds the trajectory-observation command. |
|
||||
|
||||
### Task 1: Configuration and exact rectangle-map inputs
|
||||
|
||||
**Files:**
|
||||
|
||||
- Create: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs
|
||||
- Create: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
|
||||
|
||||
**Interfaces:**
|
||||
|
||||
- Consumes: Pose2D, VehicleParameters, PlanningMapRequest, MapBoundsMm, ManualObstacleSource, CircleObstacle, AxisAlignedRectangleObstacle.
|
||||
- Produces: TrajectoryObservationSettings.Validate(), TrajectoryObservationObstacle.Circle(double, double, double), TrajectoryObservationObstacle.Rectangle(double, double, double, double), and TrajectoryObservationSetupFactory.CreateBootstrapJob(Pose2D, Pose2D, TrajectoryObservationSettings, IReadOnlyList<TrajectoryObservationObstacle>, long).
|
||||
|
||||
- [ ] **Step 1: Write failing map-bounds and obstacle checks**
|
||||
|
||||
Create the verification host class and invoke it with a new trajectory-observation argument:
|
||||
|
||||
~~~csharp
|
||||
internal static class TrajectoryObservationChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
|
||||
RejectsObstacleOutsideConfiguredBounds();
|
||||
}
|
||||
|
||||
private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
|
||||
{
|
||||
var settings = new TrajectoryObservationSettings
|
||||
{
|
||||
MapPaddingMeters = 2d,
|
||||
MapResolutionMillimeters = 50f,
|
||||
};
|
||||
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
||||
new Pose2D(10d, -5d, 0d), new Pose2D(13d, -1d, 0d), settings,
|
||||
Array.Empty<TrajectoryObservationObstacle>(), 17L);
|
||||
|
||||
Verification.NearlyEqual(8000d, job.MapRequest.Bounds.XMin, "observer map x min");
|
||||
Verification.NearlyEqual(15000d, job.MapRequest.Bounds.XMax, "observer map x max");
|
||||
Verification.NearlyEqual(-7000d, job.MapRequest.Bounds.YMin, "observer map y min");
|
||||
Verification.NearlyEqual(1000d, job.MapRequest.Bounds.YMax, "observer map y max");
|
||||
Verification.NearlyEqual(50d, job.MapRequest.ResolutionMm, "observer map resolution");
|
||||
}
|
||||
}
|
||||
~~~
|
||||
|
||||
Modify Program.Main to accept trajectory-observation, call TrajectoryObservationChecks.Run(), then write PASS trajectory-observation. Add the same call to em-all.
|
||||
|
||||
- [ ] **Step 2: Run the new check to prove it fails**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
~~~
|
||||
|
||||
Expected: compilation fails because TrajectoryObservationSettings and TrajectoryObservationSetupFactory do not exist.
|
||||
|
||||
- [ ] **Step 3: Implement the contracts and factory**
|
||||
|
||||
Create the editable configuration contract:
|
||||
|
||||
~~~csharp
|
||||
public sealed class TrajectoryObservationSettings
|
||||
{
|
||||
public double MapPaddingMeters { get; set; } = 2d;
|
||||
public float MapResolutionMillimeters { get; set; } = 50f;
|
||||
public double ReplanPeriodSeconds { get; set; } = 0.20d;
|
||||
public double ObserverPeriodSeconds { get; set; } = 0.05d;
|
||||
public double VehicleLengthMeters { get; set; } = 0.80d;
|
||||
public double VehicleWidthMeters { get; set; } = 0.60d;
|
||||
public double SafetyMarginMeters { get; set; } = 0.05d;
|
||||
public double MaximumCurvaturePerMeter { get; set; } = 1d / 1.20d;
|
||||
|
||||
public void Validate();
|
||||
public VehicleParameters CreateVehicle();
|
||||
}
|
||||
~~~
|
||||
|
||||
Implement finite/positive validation. Implement the obstacle as world-mm circle or axis-aligned rectangle with GetBounds() and ToMapObstacle(). Build bounds with the following exact calculation, rounded outward to the configured grid:
|
||||
|
||||
~~~csharp
|
||||
double padMm = settings.MapPaddingMeters * 1000d;
|
||||
var bounds = new MapBoundsMm(
|
||||
ToGridLower(Math.Min(start.X, goal.X) * 1000d - padMm, settings.MapResolutionMillimeters),
|
||||
ToGridUpper(Math.Max(start.X, goal.X) * 1000d + padMm, settings.MapResolutionMillimeters),
|
||||
ToGridLower(Math.Min(start.Y, goal.Y) * 1000d - padMm, settings.MapResolutionMillimeters),
|
||||
ToGridUpper(Math.Max(start.Y, goal.Y) * 1000d + padMm, settings.MapResolutionMillimeters));
|
||||
~~~
|
||||
|
||||
Reject an obstacle unless its full envelope is contained in bounds. With zero obstacles set AllowExplicitEmptyMap true. Otherwise construct exactly one required ManualObstacleSource named trajectory-observer-manual with the supplied positive snapshot version. Return a CoarsePathPlanningJob with new HybridAStarConfiguration, StartDirection = null, and GoalDirection = GoalDirectionConstraint.Any.
|
||||
|
||||
- [ ] **Step 4: Run focused and existing checks**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- foundation
|
||||
~~~
|
||||
|
||||
Expected: both exit 0 and print PASS trajectory-observation and PASS foundation.
|
||||
|
||||
- [ ] **Step 5: Commit the input layer**
|
||||
|
||||
~~~powershell
|
||||
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
|
||||
git commit -m "feat: add observation test map inputs"
|
||||
~~~
|
||||
|
||||
### Task 2: Pure planning bootstrap, time observation, and LS/ST derivation
|
||||
|
||||
**Files:**
|
||||
|
||||
- Create: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs
|
||||
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
|
||||
**Interfaces:**
|
||||
|
||||
- Consumes: CoarsePathPlanningService, PathSmoothingService, EmPlanningCoordinator, TrajectoryExecutor, FrenetProjector, and caller-supplied VehicleMotionState.
|
||||
- Produces: TrajectoryObservationBootstrapper.Bootstrap(CoarsePathPlanningJob, CancellationToken), TrajectoryObservationController.StartCycle(DateTimeOffset, VehicleMotionState, CancellationToken), TrajectoryObservationController.Observe(DateTimeOffset, VehicleMotionState), and TrajectoryObservationCharts.Build(EmTrajectory, DirectionSegmentView, double).
|
||||
|
||||
- [ ] **Step 1: Add failing chart and time-sampling checks**
|
||||
|
||||
Extend TrajectoryObservationChecks.Run() by adding VerifiesLsAndStUsePublishedTrajectoryData(). Use a fixed two-point EmTrajectory whose EffectiveAtUtc is 2026-08-04T00:00:00Z, with TimeFromStart values 0 and 1, PathS values 4 and 5, and known signed speeds. Assert that Build returns two ST samples (0,4) and (1,5), two speed samples, and the expected LS projection count. Call Observe at 00:00:00.500Z and assert that TrajectoryExecutor selected an interpolated point with TimeFromStart == 0.5d.
|
||||
|
||||
- [ ] **Step 2: Run the new check to prove it fails**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
~~~
|
||||
|
||||
Expected: compilation fails because TrajectoryObservationCharts and TrajectoryObservationController do not exist.
|
||||
|
||||
- [ ] **Step 3: Implement the pipeline**
|
||||
|
||||
Bootstrap must use exactly this success gate:
|
||||
|
||||
~~~csharp
|
||||
CoarsePathPlanningJobResult coarse = coarseService.Plan(job, cancellationToken);
|
||||
if (coarse.PlanningResult.Status != PlanningStatus.Success)
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(
|
||||
job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);
|
||||
|
||||
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);
|
||||
if (!IsPublishedSmoothingStatus(smooth.Status))
|
||||
return TrajectoryObservationBootstrapResult.FromFailure(
|
||||
job, coarse, smooth, smooth.Diagnostics.TerminationReason);
|
||||
return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
|
||||
~~~
|
||||
|
||||
IsPublishedSmoothingStatus accepts only Complete, PartialImprovement, NotNeeded, and Unchanged. CopyFiniteClearance replaces a positive-infinite clearance with the finite map diagonal before copying each CoarsePathPoint.
|
||||
|
||||
TrajectoryObservationController owns EmPlanningCoordinator and TrajectoryExecutor. For a replan it creates:
|
||||
|
||||
~~~csharp
|
||||
var request = new EmPlanningRequest(
|
||||
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Job.Vehicle, state, configuration,
|
||||
segmentIndex, coordinator.PublishedTrajectory, now, now,
|
||||
sessionId + "-trajectory-" + cycleId, sessionId + "-reference",
|
||||
coordinator.PublishedTrajectory?.Metadata.TrajectoryId ?? string.Empty,
|
||||
EmMotionModel.NonholonomicForwardReverse);
|
||||
return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken);
|
||||
~~~
|
||||
|
||||
Set configuration.Scheduling.ReplanPeriodSeconds from settings. Initial observation mode always uses segmentIndex 0. Observe must use PublishedTrajectory only; when non-null call UpdateCommand(now, state, trajectory, trajectory.Metadata.Direction, trajectory.Metadata.Direction, true) and return the selected point, command, and executor state for display only.
|
||||
|
||||
Build LS/ST from published data alone:
|
||||
|
||||
~~~csharp
|
||||
ls.Add(new TrajectoryObservationLsSample(
|
||||
segment.SourceStartArcLength + projection.ReferenceS, projection.LateralOffset));
|
||||
st.Add(new TrajectoryObservationStSample(point.TimeFromStart, point.PathS));
|
||||
speed.Add(new TrajectoryObservationSpeedSample(point.TimeFromStart, point.SignedLongitudinalVelocity));
|
||||
~~~
|
||||
|
||||
Use seeded FrenetProjector calls and count failed projections. No pipeline class may reference UI, DetourInterface, BasicPilotBase, or a hardware class.
|
||||
|
||||
- [ ] **Step 4: Run diagnostics and regression checks**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- executor
|
||||
~~~
|
||||
|
||||
Expected: every command exits 0.
|
||||
|
||||
- [ ] **Step 5: Commit the pure pipeline**
|
||||
|
||||
~~~powershell
|
||||
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
git commit -m "feat: add EM observation planning pipeline"
|
||||
~~~
|
||||
|
||||
### Task 3: Presentation layers and observation text
|
||||
|
||||
**Files:**
|
||||
|
||||
- Create: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs
|
||||
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
|
||||
**Interfaces:**
|
||||
|
||||
- Consumes: bootstrap result, observation result, and chart data.
|
||||
- Produces: TrajectoryObservationPresentation.DrawWorld(TrajectoryObservationBootstrapResult, TrajectoryObservationObservation), DrawLs(TrajectoryObservationCharts), DrawSt(TrajectoryObservationCharts), ClearAll(), and TrajectoryObservationPresentationText.Create(TrajectoryObservationObservation, TrajectoryObservationCharts).
|
||||
|
||||
- [ ] **Step 1: Add a failing presentation-text check**
|
||||
|
||||
Assert that TrajectoryObservationPresentationText.Create(observation, charts) contains the literal OBSERVE_ONLY: no chassis command is sent., selected point time/path-S, signed speed, yaw rate, and LS projection failure count. The check must not instantiate a Painter.
|
||||
|
||||
- [ ] **Step 2: Run the check to verify it fails**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
~~~
|
||||
|
||||
Expected: compilation fails because TrajectoryObservationPresentationText does not exist.
|
||||
|
||||
- [ ] **Step 3: Implement the three painters**
|
||||
|
||||
Create exactly these named layers:
|
||||
|
||||
~~~csharp
|
||||
worldPainter = UI.GetPainter("TrajectoryObserver.World", true);
|
||||
lsPainter = UI.GetPainter("TrajectoryObserver.LS", true);
|
||||
stPainter = UI.GetPainter("TrajectoryObserver.ST", true);
|
||||
~~~
|
||||
|
||||
DrawWorld clears only worldPainter then draws map bounds/grid/occupied cells, start, goal, coarse path, Local G2 path, real pose, and latest EM path. Convert every planner position from m to mm before calling DrawLine, DrawCircle, or DrawText.
|
||||
|
||||
DrawLs draws axes plus s-l samples. DrawSt draws t-s and a vertically separated t-v series with a legend. A missing trajectory draws a status string instead of throwing. ClearAll invokes Clear on all three painters and performs no other action.
|
||||
|
||||
- [ ] **Step 4: Run visual-model and compile verification**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
|
||||
~~~
|
||||
|
||||
Expected: trajectory-observation passes and the project has zero compile errors.
|
||||
|
||||
- [ ] **Step 5: Commit presentation**
|
||||
|
||||
~~~powershell
|
||||
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
git commit -m "feat: visualize EM observation diagnostics"
|
||||
~~~
|
||||
|
||||
### Task 4: MDCS read-only MovementTest host
|
||||
|
||||
**Files:**
|
||||
|
||||
- Create: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs
|
||||
- Create: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md
|
||||
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
|
||||
**Interfaces:**
|
||||
|
||||
- Consumes: DetourInterface.getCartLocation(), BasicPilotBase.Chassis.GetCarSpeed(true), setup/controller/presentation APIs.
|
||||
- Produces: a [MovementTest(name = "EM轨迹规划观察闭环测试")] entry with Test() and TestStop().
|
||||
|
||||
- [ ] **Step 1: Write a failing actuator-free source audit**
|
||||
|
||||
Add VerifiesObservationSourceHasNoActuatorCalls() to TrajectoryObservationChecks.Run() and implement it in the verification host. It reads the observation runtime source files and fails on any of these tokens:
|
||||
|
||||
~~~csharp
|
||||
new[]
|
||||
{
|
||||
".SendXYThSpeed(", ".SendMotion(", ".SendTh(", ".AccumulateSpeed(",
|
||||
".ComputeWheelsGeometrically(", ".DriveStop(", ".PredefinedDriveStop("
|
||||
}
|
||||
~~~
|
||||
|
||||
The audit strings live only in the test host; none may appear in the new runtime observation files.
|
||||
|
||||
- [ ] **Step 2: Run the audit before the host exists**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
~~~
|
||||
|
||||
Expected: the check fails because MovementTest.TrajectoryObservationTest.cs is missing.
|
||||
|
||||
- [ ] **Step 3: Implement the host and lifecycle**
|
||||
|
||||
Use this discoverable configuration:
|
||||
|
||||
~~~csharp
|
||||
[MovementTest(name = "EM轨迹规划观察闭环测试")]
|
||||
public sealed class TrajectoryObservationMovementTest : MovementTest
|
||||
{
|
||||
public double GoalXmm = double.NaN;
|
||||
public double GoalYmm = double.NaN;
|
||||
public double GoalYawDeg = 0d;
|
||||
public double MapPaddingMeters = 2d;
|
||||
public float MapResolutionMm = 50f;
|
||||
public double ReplanPeriodSeconds = 0.20d;
|
||||
public double ObserverPeriodSeconds = 0.05d;
|
||||
|
||||
public override void Test();
|
||||
public override void TestStop();
|
||||
}
|
||||
~~~
|
||||
|
||||
When GoalXmm or GoalYmm is non-finite, prompt for all goal values with the same finite parser/UI.GetInput pattern as CoarsePathPlanningTest. Prompt for 0–20 manual obstacles (circle or rectangle) and freeze all inputs before Task.Run begins.
|
||||
|
||||
The MDCS reader must use only this read path:
|
||||
|
||||
~~~csharp
|
||||
var location = DetourInterface.getCartLocation();
|
||||
if (location == null) throw new InvalidOperationException("Live localization is unavailable.");
|
||||
if (BasicPilotBase.Chassis == null) throw new InvalidOperationException("Live chassis read interface is unavailable.");
|
||||
var speed = BasicPilotBase.Chassis.GetCarSpeed(true);
|
||||
return new VehicleMotionState(
|
||||
new Pose2D(location.x / 1000d, location.y / 1000d, location.th * Math.PI / 180d),
|
||||
speed.Vx, null, DateTimeOffset.UtcNow, Interlocked.Increment(ref stateSequence));
|
||||
~~~
|
||||
|
||||
Bootstrap once in a cancellable Task.Run. After success, run Task.Delay(TimeSpan.FromSeconds(ObserverPeriodSeconds), token) between ticks. At each tick capture exactly one state, start a cycle only when controller.ShouldStartCycle(now), observe the latest published trajectory, draw all layers, and print a throttled status. Construct the service as new EmPlanningService(new OsqpNativeSolver()).
|
||||
|
||||
Every status includes:
|
||||
|
||||
~~~text
|
||||
OBSERVE_ONLY: no chassis command is sent.
|
||||
~~~
|
||||
|
||||
When a GearSwitch trajectory reaches its final time, draw and print 等待真实档位/方向确认；观察模式不会推进下一方向段, leave segment index 0, and do not create a direction-change action. Goal and rolling-stop commands may only be logged.
|
||||
|
||||
Use a lock/session id pattern matching CoarsePathPlanningTestRunner: replace the active CancellationTokenSource, cancel the old source without waiting, and allow only the current session to draw or log. TestStop cancels, disposes after task completion, and calls presentation.ClearAll.
|
||||
|
||||
Write README.md with configuration fields/units, obstacle examples, default values, chart interpretations, the VelocityX/VelocityY world-frame warning, and the explicit no-driving limitation.
|
||||
|
||||
- [ ] **Step 4: Verify runner safety and integration build**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
|
||||
rg -n 'SendXYThSpeed\(|SendMotion\(|SendTh\(|AccumulateSpeed\(|ComputeWheelsGeometrically\(|DriveStop\(|PredefinedDriveStop\(' ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest -g '*.cs'
|
||||
~~~
|
||||
|
||||
Expected: host and build exit 0. The rg command exits 1 because no runtime observation file calls a forbidden actuator method.
|
||||
|
||||
- [ ] **Step 5: Commit the MovementTest**
|
||||
|
||||
~~~powershell
|
||||
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
git commit -m "feat: add read-only EM observation movement test"
|
||||
~~~
|
||||
|
||||
### Task 5: End-to-end regression and operator handoff
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md
|
||||
|
||||
**Interfaces:**
|
||||
|
||||
- Consumes: completed observation-test components and existing EMPlannerVerificationHost checks.
|
||||
- Produces: a reproducible all-up verification command and a launch/stop checklist.
|
||||
|
||||
- [ ] **Step 1: Add a failing bootstrap regression**
|
||||
|
||||
Use an empty-map setup with start (0.5, 0.5, 0) m and goal (3.5, 0.5, 0) m. Assert that bootstrap returns a successful map, PlanningStatus.Success, a publishable smoothing result, and at least one DirectionSegmentView. This check does not run native OSQP.
|
||||
|
||||
- [ ] **Step 2: Run the check to confirm its failure**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
~~~
|
||||
|
||||
Expected: the assertion identifies a missing or incorrect bootstrap result.
|
||||
|
||||
- [ ] **Step 3: Make the smallest corrective change**
|
||||
|
||||
Correct only TrajectoryObservationSetupFactory or TrajectoryObservationBootstrapper so the empty-map request produces a planning-ready map and publishable Local G2 path. Preserve the exact bounds rule and do not add UI, MDCS, or hardware dependencies to pure classes.
|
||||
|
||||
- [ ] **Step 4: Run all required evidence checks**
|
||||
|
||||
Run:
|
||||
|
||||
~~~powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all
|
||||
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
|
||||
git diff --check
|
||||
~~~
|
||||
|
||||
Expected: every command exits 0. In the vehicle UI, the entry appears as EM轨迹规划观察闭环测试 and starting/running/stopping it does not issue any chassis, motor, steering, or brake output.
|
||||
|
||||
- [ ] **Step 5: Commit final verification/documentation**
|
||||
|
||||
~~~powershell
|
||||
git add -- ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md
|
||||
git commit -m "test: verify EM observation movement test"
|
||||
~~~
|
||||
|
||||
## Plan self-review
|
||||
|
||||
**Spec coverage:** Task 1 provides configurable start/goal map bounds, vehicle settings, and manual obstacles. Task 2 covers Hybrid A*, Local G2, rolling EM, time sampling, and derived LS/ST. Task 3 creates World/LS/ST painters. Task 4 reads live MDCS state, prints observation diagnostics, handles gear-switch observation, and ensures cancellation/no-write behavior. Task 5 supplies an end-to-end fixture and final evidence.
|
||||
|
||||
**Placeholder scan:** Every task names concrete files, commands, interface names, inputs, expected behavior, and commit content; no deferred implementation markers remain.
|
||||
|
||||
**Type consistency:** Map code produces PlanningMapRequest and CoarsePathPlanningJob; bootstrap produces PathSmoothingResult and DirectionSegmentView; rolling planning consumes VehicleMotionState and EmPlanningRequest; UI consumes EmTrajectory, TrajectoryControlCommand, and chart samples without changing EM contracts.
|
||||
Reference in New Issue
Block a user