First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: correct EM observation painter geometry

Browse Source
This commit is contained in:
梁薄云
2026-08-07 10:58:18 +08:00
parent d019b08074
commit 37c27bdb1b
3 changed files with 149 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs
+86 -12
View File
@@ -58,9 +58,9 @@ public sealed class TrajectoryObservationLsPresentationModel
new List<TrajectoryObservationLsPresentationPoint>(samples));
}
public string HorizontalAxisLabel => "path-S (m)";
public string HorizontalAxisLabel => "ReferenceS (m)";
public string VerticalAxisLabel => "lateral offset (m)";
public string VerticalAxisLabel => "l (m)";
public IReadOnlyList<TrajectoryObservationLsPresentationPoint> Samples { get; }
@@ -98,13 +98,14 @@ public sealed class TrajectoryObservationPresentation
return;
}
VehicleParameters vehicle = bootstrap.Vehicle;
DrawMap(bootstrap.Map);
DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Start, Color.LimeGreen, "start");
DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Goal, Color.Orange, "goal");
DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Start, vehicle, Color.LimeGreen, "计划起点");
DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Goal, vehicle, Color.Orange, "终点");
DrawCoarsePath(bootstrap.CoarseResult == null ? null : bootstrap.CoarseResult.PlanningResult.Path);
DrawLocalG2Path(bootstrap.SmoothedPath == null ? null : bootstrap.SmoothedPath.Path);
DrawPose(observation == null || observation.VehicleState == null ? null : observation.VehicleState.Pose,
Color.DeepSkyBlue, "real pose");
vehicle, Color.DeepSkyBlue, "车辆");
if (runtimeState != null && runtimeState.WorldNotice.Length > 0)
{
float noticeX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
@@ -117,11 +118,13 @@ public sealed class TrajectoryObservationPresentation
{
float diagnosticX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
float diagnosticY = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.YMin + 300f;
DrawBoundaryMarkers(bootstrap.Segments);
worldPainter.DrawText(Color.LightYellow,
EmptyChartMessage("No published trajectory available.\n", diagnosticText), diagnosticX, diagnosticY);
return;
}
DrawEmPath(trajectory.Points);
DrawBoundaryMarkers(bootstrap.Segments);
}
public void DrawLs(TrajectoryObservationCharts charts, string diagnosticText)
@@ -167,6 +170,8 @@ public sealed class TrajectoryObservationPresentation
stPainter.DrawLine(Color.Gainsboro, 0f, 0f, maxTime, 0f, width: 2);
stPainter.DrawLine(Color.Gainsboro, 0f, 0f, 0f, maxS, width: 2);
stPainter.DrawText(Color.White, "T-S", 0f, maxS + 150f);
stPainter.DrawText(Color.White, "t (s)", maxTime + 100f, -200f);
stPainter.DrawText(Color.White, "PathS (m)", 100f, maxS + 300f);
DrawStSeries(charts.StSamples);
stPainter.DrawLine(Color.Gainsboro, 0f, VelocitySeriesBaseMillimeters,
@@ -225,14 +230,83 @@ public sealed class TrajectoryObservationPresentation
}
}
private void DrawPose(Pose2D pose, Color color, string label)
private void DrawPose(Pose2D pose, VehicleParameters vehicle, Color color, string label)
{
if (pose == null) return;
double lengthMeters = vehicle == null || vehicle.LengthMeters <= 0d ? 0.8d : vehicle.LengthMeters;
double widthMeters = vehicle == null || vehicle.WidthMeters <= 0d ? 0.6d : vehicle.WidthMeters;
double heading = pose.Heading;
double cosHeading = Math.Cos(heading);
double sinHeading = Math.Sin(heading);
float x = ToMillimeters(pose.X);
float y = ToMillimeters(pose.Y);
worldPainter.DrawCircle(color, x, y, 80f);
worldPainter.DrawLine(color, x, y, x + (float)Math.Cos(pose.Heading) * 250f,
y + (float)Math.Sin(pose.Heading) * 250f, endArrow: true, width: 3);
float halfLengthMillimeters = (float)(lengthMeters * MillimetersPerMeter / 2d);
float halfWidthMillimeters = (float)(widthMeters * MillimetersPerMeter / 2d);
float frontLeftX = x + (float)(cosHeading * halfLengthMillimeters - sinHeading * halfWidthMillimeters);
float frontLeftY = y + (float)(sinHeading * halfLengthMillimeters + cosHeading * halfWidthMillimeters);
float frontRightX = x + (float)(cosHeading * halfLengthMillimeters + sinHeading * halfWidthMillimeters);
float frontRightY = y + (float)(sinHeading * halfLengthMillimeters - cosHeading * halfWidthMillimeters);
float rearRightX = x + (float)(-cosHeading * halfLengthMillimeters + sinHeading * halfWidthMillimeters);
float rearRightY = y + (float)(-sinHeading * halfLengthMillimeters - cosHeading * halfWidthMillimeters);
float rearLeftX = x + (float)(-cosHeading * halfLengthMillimeters - sinHeading * halfWidthMillimeters);
float rearLeftY = y + (float)(-sinHeading * halfLengthMillimeters + cosHeading * halfWidthMillimeters);
DrawVehicleOutline(frontLeftX, frontLeftY, frontRightX, frontRightY,
rearRightX, rearRightY, rearLeftX, rearLeftY, color);
float headingLengthMillimeters = (float)(lengthMeters * MillimetersPerMeter * 0.25d);
worldPainter.DrawLine(color, x, y,
x + (float)cosHeading * headingLengthMillimeters,
y + (float)sinHeading * headingLengthMillimeters, endArrow: false, width: 1);
worldPainter.DrawText(color, label, x + 100f, y + 100f);
}
private void DrawVehicleOutline(float frontLeftX, float frontLeftY, float frontRightX, float frontRightY,
float rearRightX, float rearRightY, float rearLeftX, float rearLeftY, Color color)
{
worldPainter.DrawLine(color, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 1);
worldPainter.DrawLine(color, frontRightX, frontRightY, rearRightX, rearRightY, width: 1);
worldPainter.DrawLine(color, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 1);
worldPainter.DrawLine(color, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 1);
}
private void DrawBoundaryMarkers(IReadOnlyList<DirectionSegmentView> segments)
{
if (segments == null) return;
for (int index = 0; index < segments.Count; index++)
{
DirectionSegmentView segment = segments[index];
if (segment.Points.Count == 0) continue;
var point = segment.Points[segment.Points.Count - 1];
if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach)
{
DrawDiamondMarker(point, Color.Orange,
"换向点 " + (segment.SegmentIndex + 1).ToString(CultureInfo.InvariantCulture) + " / s_end");
}
else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal)
{
DrawCrossMarker(point, Color.OrangeRed, "终点 / s_end");
}
}
}
private void DrawDiamondMarker(SmoothedPathPoint point, Color color, string label)
{
float x = ToMillimeters(point.X);
float y = ToMillimeters(point.Y);
const float radius = 50f;
worldPainter.DrawLine(color, x, y + radius, x + radius, y, width: 1);
worldPainter.DrawLine(color, x + radius, y, x, y - radius, width: 1);
worldPainter.DrawLine(color, x, y - radius, x - radius, y, width: 1);
worldPainter.DrawLine(color, x - radius, y, x, y + radius, width: 1);
worldPainter.DrawText(color, label, x + 100f, y + 100f);
}
private void DrawCrossMarker(SmoothedPathPoint point, Color color, string label)
{
float x = ToMillimeters(point.X);
float y = ToMillimeters(point.Y);
const float radius = 50f;
worldPainter.DrawLine(color, x - radius, y - radius, x + radius, y + radius, width: 1);
worldPainter.DrawLine(color, x - radius, y + radius, x + radius, y - radius, width: 1);
worldPainter.DrawText(color, label, x + 100f, y + 100f);
}
@@ -241,7 +315,7 @@ public sealed class TrajectoryObservationPresentation
if (path == null || path.Count == 0) return;
for (int index = 1; index < path.Count; index++)
worldPainter.DrawLine(Color.LimeGreen, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 4);
ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 2);
}
private void DrawLocalG2Path(IReadOnlyList<SmoothedPathPoint> path)
@@ -249,14 +323,14 @@ public sealed class TrajectoryObservationPresentation
if (path == null || path.Count == 0) return;
for (int index = 1; index < path.Count; index++)
worldPainter.DrawLine(Color.Gold, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 3);
ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 1);
}
private void DrawEmPath(IReadOnlyList<EmTrajectoryPoint> path)
{
for (int index = 1; index < path.Count; index++)
worldPainter.DrawLine(Color.DeepPink, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 4);
ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 2);
}
private void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)