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

feat: summarize published EM observation trajectories

Browse Source
This commit is contained in:
梁薄云
2026-08-05 15:34:31 +08:00
parent 9b59ce4a71
commit e11397aa03
2 changed files with 71 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs
+48 -2
View File
@@ -1,5 +1,7 @@
using System;
using System.Collections.Generic;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
@@ -51,8 +53,8 @@ public static class TrajectoryObservationDiagnostics
"ms";
if (planningInFlight)
text += "\nreplan=pending";
if (publishedTrajectory != null)
text += "\ntrajectory=" + publishedTrajectory.Metadata.TrajectoryId;
if (latestCycle.Published && latestCycle.Result.Trajectory != null)
text += "\n" + CreateTrajectorySummary(latestCycle.Result.Trajectory);
if (!string.IsNullOrWhiteSpace(latestCycle.Result.FailureReason))
text += "\nreason=" + latestCycle.Result.FailureReason;
return new TrajectoryObservationDiagnostic(text);
@@ -62,4 +64,48 @@ public static class TrajectoryObservationDiagnostics
{
return value.ToString(format, CultureInfo.InvariantCulture);
}
private static string CreateTrajectorySummary(EmTrajectory trajectory)
{
IReadOnlyList<EmTrajectoryPoint> points = trajectory.Points;
double maximumSpeed = 0d;
double maximumAcceleration = 0d;
double maximumJerk = 0d;
double previousAcceleration = 0d;
bool hasPreviousAcceleration = false;
double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d;
for (int index = 0; index < points.Count; index++)
{
EmTrajectoryPoint point = points[index];
maximumSpeed = Math.Max(maximumSpeed, Math.Abs(point.SignedLongitudinalVelocity));
if (index == 0) continue;
EmTrajectoryPoint previous = points[index - 1];
double dt = point.TimeFromStart - previous.TimeFromStart;
if (!IsPositiveFinite(dt)) continue;
double acceleration = (directionSign * point.SignedLongitudinalVelocity -
directionSign * previous.SignedLongitudinalVelocity) / dt;
maximumAcceleration = Math.Max(maximumAcceleration, Math.Abs(acceleration));
if (hasPreviousAcceleration)
maximumJerk = Math.Max(maximumJerk, Math.Abs((acceleration - previousAcceleration) / dt));
previousAcceleration = acceleration;
hasPreviousAcceleration = true;
}
EmTrajectoryPoint first = points[0];
EmTrajectoryPoint last = points[points.Count - 1];
return "trajectory summary:\n" +
"trajectoryId=" + trajectory.Metadata.TrajectoryId + ", points=" +
points.Count.ToString(CultureInfo.InvariantCulture) + ", duration=" +
Format(last.TimeFromStart, "F3") + "s, pathLength=" +
Format(last.PathS - first.PathS, "F3") + "m\n" +
"maxSpeed=" + Format(maximumSpeed, "F3") + "m/s, maxAcceleration=" +
Format(maximumAcceleration, "F3") + "m/s2, maxJerk=" +
Format(maximumJerk, "F3") + "m/s3";
}
private static bool IsPositiveFinite(double value)
{
return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
}
}