完善Detour状态估计与轨迹跟踪验证

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -125,7 +125,7 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 从当前位姿按速度符号生成“3m直线、沿行进方向平滑左弯180°、3m直线”的轨迹。
+        /// 从当前位姿沿指定车体运动方向生成“3m直线、平滑左弯180°、3m直线”的轨迹。
         /// </summary>
         public static Trajectory2D CreateStraightLeftSemicircleStraight(
             Pose2D startPoseInWorld,
@@ -136,7 +136,8 @@ namespace MultiWheelC
             double semicircleMaximumSpeedMetersPerSecond = 0.25,
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.12,
-            double pointSpacingMeters = 0.02)
+            double pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
         {
             return CreateStraightSmoothLeftTurnStraight(
                 startPoseInWorld,
@@ -148,11 +149,12 @@ namespace MultiWheelC
                 semicircleMaximumSpeedMetersPerSecond,
                 accelerationMetersPerSecondSquared,
                 decelerationMetersPerSecondSquared,
-                pointSpacingMeters);
+                pointSpacingMeters,
+                motionDirectionInBodyRadians);
         }
 
         /// <summary>
-        /// 按共同速度符号生成“直线、沿行进方向平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
+        /// 沿指定车体运动方向生成“直线、平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
         /// </summary>
         public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
             Pose2D startPoseInWorld,
@@ -164,7 +166,8 @@ namespace MultiWheelC
             double turnMaximumSpeedMetersPerSecond,
             double accelerationMetersPerSecondSquared,
             double decelerationMetersPerSecondSquared,
-            double pointSpacingMeters)
+            double pointSpacingMeters,
+            double motionDirectionInBodyRadians = 0.0)
         {
             NumericGuard.EnsureFinite(
                 startPoseInWorld,
@@ -195,6 +198,9 @@ namespace MultiWheelC
             NumericGuard.EnsureFinitePositive(
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             if (turnAngleRadians > 2.0 * Math.PI)
             {
@@ -276,10 +282,13 @@ namespace MultiWheelC
 
             var points = new List<TrajectoryPoint>(
                 sampleArcLengths.Count);
+            var worldMotionStartYawRadians =
+                startPoseInWorld.YawRadians +
+                motionDirectionInBodyRadians;
             var startCos = Math.Cos(
-                startPoseInWorld.YawRadians);
+                worldMotionStartYawRadians);
             var startSin = Math.Sin(
-                startPoseInWorld.YawRadians);
+                worldMotionStartYawRadians);
             var localX = 0.0;
             var localY = 0.0;
             var localYawRadians = 0.0;