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

泛化轨迹工厂参数并新增组合运动计划执行器与复合测试

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-07 13:25:47 +08:00
parent 14ca1150e4
commit bc37e71ad0
26 changed files with 874 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Experiments/ClampTests.cs
+78 -78
View File
@@ -1,90 +1,90 @@
using System;
using ClumsyCore;
using ClumsyCore.Pilot;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
// using System;
// using ClumsyCore;
// using ClumsyCore.Pilot;
// using FundamentalLib;
// using MDCSToolBox.Clumsy.Movements;
// using MDCSToolBox.Clumsy.Pilot;
namespace MultiWheelC
{
public abstract class ClampMovementTestBase : MovementTest
{
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
// namespace MultiWheelC
// {
// public abstract class ClampMovementTestBase : MovementTest
// {
// public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
private DriveTask _task;
protected abstract bool Close { get; }
// private DriveTask _task;
// protected abstract bool Close { get; }
// 根据派生测试类型驱动左右夹臂同步夹紧或打开。
public override void Test()
{
var leftTarget = Close
? PilotDefinition.Self.LeftArmUpperPos
: PilotDefinition.Self.LeftArmLowerPos;
var rightTarget = Close
? PilotDefinition.Self.RightArmUpperPos
: PilotDefinition.Self.RightArmLowerPos;
// // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
// public override void Test()
// {
// var leftTarget = Close
// ? PilotDefinition.Self.LeftArmUpperPos
// : PilotDefinition.Self.LeftArmLowerPos;
// var rightTarget = Close
// ? PilotDefinition.Self.RightArmUpperPos
// : PilotDefinition.Self.RightArmLowerPos;
if (float.IsNaN(leftTarget) ||
float.IsInfinity(leftTarget) ||
float.IsNaN(rightTarget) ||
float.IsInfinity(rightTarget))
{
Console.WriteLine(
"夹臂目标位置无效，取消夹臂运动测试。");
return;
}
// if (float.IsNaN(leftTarget) ||
// float.IsInfinity(leftTarget) ||
// float.IsNaN(rightTarget) ||
// float.IsInfinity(rightTarget))
// {
// Console.WriteLine(
// "夹臂目标位置无效，取消夹臂运动测试。");
// return;
// }
// 防止重复点击时上一项夹臂任务仍在运行。
TestStop();
Console.WriteLine(
$"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
$"左目标={leftTarget}，右目标={rightTarget}");
// // 防止重复点击时上一项夹臂任务仍在运行。
// TestStop();
// Console.WriteLine(
// $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
// $"左目标={leftTarget}，右目标={rightTarget}");
var task = new DriveTask(
new ClampToTarget
{
LeftClampTarget = leftTarget,
RightClampTarget = rightTarget,
TimeoutSeconds = TimeoutSeconds
}.Get());
_task = task;
// var task = new DriveTask(
// new ClampToTarget
// {
// LeftClampTarget = leftTarget,
// RightClampTarget = rightTarget,
// TimeoutSeconds = TimeoutSeconds
// }.Get());
// _task = task;
try
{
task.Wait();
}
finally
{
PilotDefinition.Self.SpeedLeftArm = 0f;
PilotDefinition.Self.SpeedRightArm = 0f;
if (ReferenceEquals(_task, task))
_task = null;
}
}
// try
// {
// task.Wait();
// }
// finally
// {
// PilotDefinition.Self.SpeedLeftArm = 0f;
// PilotDefinition.Self.SpeedRightArm = 0f;
// if (ReferenceEquals(_task, task))
// _task = null;
// }
// }
// 停止夹臂任务并立即清零左右夹臂下发速度。
public override void TestStop()
{
_task?.Stop();
_task = null;
PilotDefinition.Self.SpeedLeftArm = 0f;
PilotDefinition.Self.SpeedRightArm = 0f;
}
}
// // 停止夹臂任务并立即清零左右夹臂下发速度。
// public override void TestStop()
// {
// _task?.Stop();
// _task = null;
// PilotDefinition.Self.SpeedLeftArm = 0f;
// PilotDefinition.Self.SpeedRightArm = 0f;
// }
// }
[MovementTest(name = "夹臂关闭测试")]
public sealed class TestClampCloseMovement
: ClampMovementTestBase
{
protected override bool Close => false;
}
// [MovementTest(name = "夹臂关闭测试")]
// public sealed class TestClampCloseMovement
// : ClampMovementTestBase
// {
// protected override bool Close => false;
// }
[MovementTest(name = "夹臂启动测试")]
public sealed class TestClampOpenMovement
: ClampMovementTestBase
{
protected override bool Close => true;
}
}
// [MovementTest(name = "夹臂启动测试")]
// public sealed class TestClampOpenMovement
// : ClampMovementTestBase
// {
// protected override bool Close => true;
// }
// }
MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+332
View File
@@ -0,0 +1,332 @@
using System;
using System.Drawing;
using System.Numerics;
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MultiWheelC.Control.Execution;
using MultiWheelC.StateEstimation;
using MultiWheelC.Trajectory;
using MyParking.Shared;
namespace MultiWheelC
{
/// <summary>
/// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
/// </summary>
[MovementTest(name = "新版控制器：曲线-停车自转-直线组合测试")]
public sealed class CompositeStopTurnGoTest : MovementTest
{
private const float MillimetersPerMeter = 1000f;
private readonly Painter _painter =
UI.GetPainter("CompositeStopTurnGoTest");
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
public int TrialNumber = 1; // 重复实验编号。
public double StraightLengthMeters = 2.0; // 圆弧前后直线长度，单位m。
public double TurnRadiusMeters = 2.0; // 平滑左转名义半径，单位m。
public double TurnAngleDegrees = 90.0; // 含过渡段在内的总左转角度。
public double CurvatureTransitionLengthMeters = 0.80; // 单侧过渡长度，单位m。
public double InPlaceLeftTurnDegrees = 90.0; // 停车后的原地左转角度。
public double FinalStraightLengthMeters = 4.5; // 自转后的直线长度，单位m。
public double StraightMaximumSpeedMetersPerSecond = 0.40; // 直线限速。
public double CurveMaximumSpeedMetersPerSecond = 0.30; // 转弯和过渡段限速。
public double AccelerationMetersPerSecondSquared = 0.20; // 参考加速度。
public double DecelerationMetersPerSecondSquared = 0.12; // 参考减速度。
public double PointSpacingMeters = 0.02; // 离散轨迹点间距。
/// <summary>
/// 从当前Detour位姿构造完整计划并依次执行连续跟踪、原地自转和最终直线。
/// </summary>
public override void Test()
{
if (_task != null)
{
Console.WriteLine(
"曲线-停车自转-直线组合测试已经在运行。");
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
}
var chassis = PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
Console.WriteLine(
"当前底盘不是MultiWheelChassis，无法执行组合运动测试。");
return;
}
var stateProvider = new DetourVehicleStateProvider();
if (!stateProvider.TryGetState(out var initialState))
{
Console.WriteLine(
"无法读取组合运动起点位姿：" +
stateProvider.LastFailureReason);
return;
}
var firstTrajectory =
TestTrajectoryFactory
.CreateStraightSmoothLeftTurnStraight(
initialState.PoseInWorld,
StraightLengthMeters,
TurnRadiusMeters,
AngleMath.DegreesToRadians(
TurnAngleDegrees),
CurvatureTransitionLengthMeters,
StraightMaximumSpeedMetersPerSecond,
CurveMaximumSpeedMetersPerSecond,
AccelerationMetersPerSecondSquared,
DecelerationMetersPerSecondSquared,
PointSpacingMeters);
var firstStopPose =
firstTrajectory.EndPoint.PoseInWorld;
var finalStraightYawRadians =
AngleMath.NormalizeRadians(
firstStopPose.YawRadians +
AngleMath.DegreesToRadians(
InPlaceLeftTurnDegrees));
var finalStraightStartPose =
new Pose2D(
firstStopPose.XMeters,
firstStopPose.YMeters,
finalStraightYawRadians);
var finalTrajectory =
TestTrajectoryFactory.CreateStraight(
finalStraightStartPose,
FinalStraightLengthMeters,
StraightMaximumSpeedMetersPerSecond,
AccelerationMetersPerSecondSquared,
DecelerationMetersPerSecondSquared,
PointSpacingMeters);
DrawPlan(
firstTrajectory,
finalTrajectory,
firstStopPose);
var plan = new MotionPlanSegment[]
{
new TrackMotionPlanSegment(firstTrajectory)
{
// 中间停车点允许后续原地转向和末段跟踪继续收敛位置误差。
FinishDistanceMeters = 0.05,
FinishSpeedMetersPerSecond = 0.03,
FinishHeadingToleranceRadians =
AngleMath.DegreesToRadians(3.0)
},
new RotateInPlaceMotionPlanSegment(
finalStraightYawRadians),
new TrackMotionPlanSegment(finalTrajectory)
};
_recorder = new TrackingExperimentRecorder(
controllerName: "NewStanleyPidComposite",
trajectoryName:
"SmoothTurnStopRotateStraight",
trialNumber: TrialNumber,
referenceStart: ToMillimeterVector(
firstTrajectory.StartPoint.PoseInWorld),
referenceEnd: ToMillimeterVector(
finalTrajectory.EndPoint.PoseInWorld),
referenceSpeed:
(float)StraightMaximumSpeedMetersPerSecond,
sampleIntervalMs: 50,
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared);
_recorder.Start();
var controlPointRadiusMeters =
chassis.ControlPointRadius /
MillimetersPerMeter;
var movement = new MotionPlanExecutor
{
Segments = plan,
StateProvider = stateProvider,
ConfigureTrackingMovement = tracking =>
{
tracking.MaximumCommandSpeedMetersPerSecond =
StraightMaximumSpeedMetersPerSecond;
tracking
.LongitudinalSpeedErrorDeadbandMetersPerSecond =
0.025;
},
SegmentStarted = (index, segment) =>
{
_recorder?.ClearControlReference();
_recorder?.UpdateCommand(0f, 0f);
Console.WriteLine(
$"组合运动开始第{index + 1}段：" +
segment.GetType().Name);
},
TrackingCycleObserver = (index, controller) =>
RecordTrackingCycle(
controller,
controlPointRadiusMeters),
RotationCommandObserver = (index, omega) =>
_recorder?.UpdateCommand(
0f,
(float)omega)
};
try
{
_task = new DriveTask(movement.Get());
_task.Wait();
}
finally
{
_task?.Stop();
_recorder?.UpdateCommand(0f, 0f);
_recorder?.StopAndSave();
_task = null;
_recorder = null;
_painter?.Clear();
}
}
/// <summary>
/// 停止组合运动、保存已有实验数据并清除计划轨迹。
/// </summary>
public override void TestStop()
{
_task?.Stop();
_recorder?.UpdateCommand(0f, 0f);
_recorder?.StopAndSave();
_task = null;
_recorder = null;
_painter?.Clear();
}
/// <summary>
/// 将两段轨迹和中间原地自转位置绘制到Clumsy界面。
/// </summary>
private void DrawPlan(
Trajectory2D firstTrajectory,
Trajectory2D finalTrajectory,
Pose2D rotationPoseInWorld)
{
_painter.Clear();
DrawTrajectory(
firstTrajectory,
Color.DeepSkyBlue);
DrawTrajectory(
finalTrajectory,
Color.Gold);
var rotationPoint =
ToMillimeterVector(rotationPoseInWorld);
_painter.DrawDot(
Color.Magenta,
rotationPoint.X,
rotationPoint.Y,
10f);
}
/// <summary>
/// 绘制一段离散世界坐标系轨迹。
/// </summary>
private void DrawTrajectory(
Trajectory2D trajectory,
Color color)
{
for (var index = 0;
index < trajectory.Count;
index++)
{
var point = ToMillimeterVector(
trajectory[index].PoseInWorld);
_painter.DrawDot(
color,
point.X,
point.Y,
3f);
if (index == 0)
{
continue;
}
var previousPoint = ToMillimeterVector(
trajectory[index - 1].PoseInWorld);
_painter.DrawLine(
color,
previousPoint.X,
previousPoint.Y,
point.X,
point.Y,
width: 2);
}
}
/// <summary>
/// 将轨迹控制周期使用的状态、参考误差和最终GCP命令写入记录器。
/// </summary>
private void RecordTrackingCycle(
ParkingGeometricController controller,
double controlPointRadiusMeters)
{
if (controller.LastVehicleState.HasValue)
{
_recorder?.UpdateProcessedState(
controller.LastVehicleState.Value);
}
if (!controller.LastCommand.HasValue)
{
return;
}
if (controller.LastProjection.HasValue &&
controller.LastReferenceSpeedMetersPerSecond.HasValue)
{
var projection =
controller.LastProjection.Value;
_recorder?.UpdateControlReference(
projection.ArcLengthMeters,
controller
.LastReferenceSpeedMetersPerSecond.Value,
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
projection.RemainingDistanceMeters);
}
var command = controller.LastCommand.Value;
var curvaturePerMeter = Math.Tan(
command.FrontAngleRadians) /
controlPointRadiusMeters;
var angularSpeedRadiansPerSecond =
command.SpeedMetersPerSecond *
curvaturePerMeter;
_recorder?.UpdateCommand(
(float)command.SpeedMetersPerSecond,
(float)angularSpeedRadiansPerSecond);
}
/// <summary>
/// 将米制世界位姿转换为Clumsy绘图和记录使用的毫米坐标。
/// </summary>
private static Vector2 ToMillimeterVector(
Pose2D poseInWorld)
{
return new Vector2(
(float)(poseInWorld.XMeters *
MillimetersPerMeter),
(float)(poseInWorld.YMeters *
MillimetersPerMeter));
}
}
}
MultiWheelC/Experiments/TestTrajectoryFactory.cs
+75 -14
View File
@@ -21,10 +21,33 @@ namespace MultiWheelC
double accelerationMetersPerSecondSquared = 0.20,
double decelerationMetersPerSecondSquared = 0.20,
double pointSpacingMeters = 0.02)
{
return CreateStraight(
startPoseInWorld,
StraightLengthMeters,
cruiseSpeedMetersPerSecond,
accelerationMetersPerSecondSquared,
decelerationMetersPerSecondSquared,
pointSpacingMeters);
}
/// <summary>
/// 从给定车体中心位姿沿当前航向生成指定长度并在终点停车的直线轨迹。
/// </summary>
public static Trajectory2D CreateStraight(
Pose2D startPoseInWorld,
double lengthMeters,
double cruiseSpeedMetersPerSecond = 0.30,
double accelerationMetersPerSecondSquared = 0.20,
double decelerationMetersPerSecondSquared = 0.20,
double pointSpacingMeters = 0.02)
{
EnsureFinitePose(
startPoseInWorld,
nameof(startPoseInWorld));
EnsureFinitePositive(
lengthMeters,
nameof(lengthMeters));
EnsureFinitePositive(
cruiseSpeedMetersPerSecond,
nameof(cruiseSpeedMetersPerSecond));
@@ -38,7 +61,7 @@ namespace MultiWheelC
pointSpacingMeters,
nameof(pointSpacingMeters));
if (pointSpacingMeters > StraightLengthMeters)
if (pointSpacingMeters > lengthMeters)
{
throw new ArgumentOutOfRangeException(
nameof(pointSpacingMeters),
@@ -46,7 +69,7 @@ namespace MultiWheelC
}
var segmentCount = (int)Math.Ceiling(
StraightLengthMeters /
lengthMeters /
pointSpacingMeters);
var points = new List<TrajectoryPoint>(
segmentCount + 1);
@@ -59,13 +82,13 @@ namespace MultiWheelC
index <= segmentCount;
index++)
{
// 均分后最后一个点严格落在4m终点，避免浮点累加越界。
// 均分后最后一个点严格落在指定终点，避免浮点累加越界。
var arcLengthMeters =
StraightLengthMeters *
lengthMeters *
index /
segmentCount;
var remainingDistanceMeters =
StraightLengthMeters -
lengthMeters -
arcLengthMeters;
var referenceSpeedMetersPerSecond =
CalculateReferenceSpeed(
@@ -105,6 +128,34 @@ namespace MultiWheelC
double accelerationMetersPerSecondSquared = 0.20,
double decelerationMetersPerSecondSquared = 0.12,
double pointSpacingMeters = 0.02)
{
return CreateStraightSmoothLeftTurnStraight(
startPoseInWorld,
straightLengthMeters,
turnRadiusMeters,
Math.PI,
curvatureTransitionLengthMeters,
straightMaximumSpeedMetersPerSecond,
semicircleMaximumSpeedMetersPerSecond,
accelerationMetersPerSecondSquared,
decelerationMetersPerSecondSquared,
pointSpacingMeters);
}
/// <summary>
/// 生成“直线、平滑左转、直线”轨迹，并使总转向角严格等于指定角度。
/// </summary>
public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
Pose2D startPoseInWorld,
double straightLengthMeters,
double turnRadiusMeters,
double turnAngleRadians,
double curvatureTransitionLengthMeters,
double straightMaximumSpeedMetersPerSecond,
double turnMaximumSpeedMetersPerSecond,
double accelerationMetersPerSecondSquared,
double decelerationMetersPerSecondSquared,
double pointSpacingMeters)
{
EnsureFinitePose(
startPoseInWorld,
@@ -115,6 +166,9 @@ namespace MultiWheelC
EnsureFinitePositive(
turnRadiusMeters,
nameof(turnRadiusMeters));
EnsureFinitePositive(
turnAngleRadians,
nameof(turnAngleRadians));
EnsureFinitePositive(
curvatureTransitionLengthMeters,
nameof(curvatureTransitionLengthMeters));
@@ -122,8 +176,8 @@ namespace MultiWheelC
straightMaximumSpeedMetersPerSecond,
nameof(straightMaximumSpeedMetersPerSecond));
EnsureFinitePositive(
semicircleMaximumSpeedMetersPerSecond,
nameof(semicircleMaximumSpeedMetersPerSecond));
turnMaximumSpeedMetersPerSecond,
nameof(turnMaximumSpeedMetersPerSecond));
EnsureFinitePositive(
accelerationMetersPerSecondSquared,
nameof(accelerationMetersPerSecondSquared));
@@ -134,21 +188,28 @@ namespace MultiWheelC
pointSpacingMeters,
nameof(pointSpacingMeters));
var originalSemicircleLengthMeters =
Math.PI * turnRadiusMeters;
if (turnAngleRadians > 2.0 * Math.PI)
{
throw new ArgumentOutOfRangeException(
nameof(turnAngleRadians),
"单段平滑左转角度不能大于2π。");
}
var nominalTurnArcLengthMeters =
turnAngleRadians * turnRadiusMeters;
var constantCurvatureLengthMeters =
originalSemicircleLengthMeters -
nominalTurnArcLengthMeters -
curvatureTransitionLengthMeters;
if (constantCurvatureLengthMeters <= 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(curvatureTransitionLengthMeters),
"曲率过渡段长度必须小于半径对应的原始半圆弧长。");
"曲率过渡段长度必须小于指定转角对应的圆弧长度。");
}
// 两段平滑过渡的平均曲率均为最大曲率的一半；
// 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于π。
// 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于指定转角。
var turnLengthMeters =
2.0 * curvatureTransitionLengthMeters +
constantCurvatureLengthMeters;
@@ -184,7 +245,7 @@ namespace MultiWheelC
turnStartArcLengthMeters &&
arcLengthMeters <=
turnEndArcLengthMeters
? semicircleMaximumSpeedMetersPerSecond
? turnMaximumSpeedMetersPerSecond
: straightMaximumSpeedMetersPerSecond;
}
@@ -341,7 +402,7 @@ namespace MultiWheelC
}
/// <summary>
/// 计算180°左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
/// 计算平滑左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
/// </summary>
private static double CalculateSmoothTurnCurvature(
double distanceInTurnMeters,
MultiWheelC/Movements/MotionPlanExecutor.cs
+233
View File
@@ -0,0 +1,233 @@
using System;
using System.Collections.Generic;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using MDCSToolBox.Commons.Controllers;
using MultiWheelC.Control.Execution;
using MultiWheelC.StateEstimation;
using MultiWheelC.Trajectory;
using MyParking.Shared;
namespace MultiWheelC
{
/// <summary>
/// 表示组合运动计划中由一种控制方式完整执行的单个动作段。
/// </summary>
public abstract class MotionPlanSegment
{
}
/// <summary>
/// 表示使用新版几何控制器跟踪一条连续二维轨迹的动作段。
/// </summary>
public sealed class TrackMotionPlanSegment : MotionPlanSegment
{
public TrackMotionPlanSegment(Trajectory2D trajectory)
{
Trajectory = trajectory ??
throw new ArgumentNullException(nameof(trajectory));
}
public Trajectory2D Trajectory { get; }
/// <summary>
/// 获取或设置本段轨迹独立的终点距离容差；为空时沿用轨迹动作默认值。
/// </summary>
public double? FinishDistanceMeters { get; set; }
/// <summary>
/// 获取或设置本段轨迹独立的停车速度容差；为空时沿用轨迹动作默认值。
/// </summary>
public double? FinishSpeedMetersPerSecond { get; set; }
/// <summary>
/// 获取或设置本段轨迹独立的终点航向容差；为空时沿用轨迹动作默认值。
/// </summary>
public double? FinishHeadingToleranceRadians { get; set; }
}
/// <summary>
/// 表示车辆停车后原地旋转到指定世界航向的动作段。
/// </summary>
public sealed class RotateInPlaceMotionPlanSegment
: MotionPlanSegment
{
public RotateInPlaceMotionPlanSegment(
double targetYawRadians)
{
if (double.IsNaN(targetYawRadians) ||
double.IsInfinity(targetYawRadians))
{
throw new ArgumentOutOfRangeException(
nameof(targetYawRadians),
"原地自转目标航向必须是有限值。");
}
TargetYawRadians =
AngleMath.NormalizeRadians(targetYawRadians);
}
public double TargetYawRadians { get; }
}
/// <summary>
/// 顺序执行连续轨迹和原地自转动作，并在动作段边界完成停车与控制器切换。
/// </summary>
public sealed class MotionPlanExecutor : MovementDefinition
{
/// <summary>
/// 获取或设置一次性提交并按顺序执行的组合运动计划。
/// </summary>
public IReadOnlyList<MotionPlanSegment> Segments;
/// <summary>
/// 获取或设置所有动作段共享的车辆状态源；为空时使用Detour状态源。
/// </summary>
public IVehicleStateProvider StateProvider;
/// <summary>
/// 获取或设置创建每段轨迹动作后应用参数的回调。
/// </summary>
public Action<TrajectoryTrackingMovement>
ConfigureTrackingMovement;
/// <summary>
/// 获取或设置创建每段原地自转动作后应用参数的回调。
/// </summary>
public Action<MultiWheelRotateInPlace>
ConfigureRotationMovement;
/// <summary>
/// 获取或设置动作段开始前的通知，参数依次为索引和动作段。
/// </summary>
public Action<int, MotionPlanSegment> SegmentStarted;
/// <summary>
/// 获取或设置轨迹段每个有效控制周期后的诊断通知。
/// </summary>
public Action<int, ParkingGeometricController>
TrackingCycleObserver;
/// <summary>
/// 获取或设置自转段角速度命令通知，角速度单位为rad/s。
/// </summary>
public Action<int, double> RotationCommandObserver;
/// <summary>
/// 按计划顺序执行各动作段，任一动作失败时停止后续动作。
/// </summary>
public override IEnumerable<bool> Get()
{
if (Segments == null || Segments.Count == 0)
{
throw new InvalidOperationException(
"组合运动计划至少需要包含一个动作段。");
}
var stateProvider =
StateProvider ??
new DetourVehicleStateProvider();
for (var index = 0;
index < Segments.Count;
index++)
{
var segment = Segments[index] ??
throw new InvalidOperationException(
$"组合运动计划第{index}段为空。");
SegmentStarted?.Invoke(index, segment);
if (segment is TrackMotionPlanSegment track)
{
var movement =
new TrajectoryTrackingMovement
{
Trajectory = track.Trajectory,
StateProvider = stateProvider,
CycleObserver = controller =>
TrackingCycleObserver?.Invoke(
index,
controller)
};
ConfigureTrackingMovement?.Invoke(movement);
// 单段参数后应用，确保中间连接段可以覆盖组合动作的公共配置。
if (track.FinishDistanceMeters.HasValue)
{
movement.FinishDistanceMeters =
track.FinishDistanceMeters.Value;
}
if (track.FinishSpeedMetersPerSecond.HasValue)
{
movement.FinishSpeedMetersPerSecond =
track.FinishSpeedMetersPerSecond.Value;
}
if (track.FinishHeadingToleranceRadians.HasValue)
{
movement.FinishHeadingToleranceRadians =
track.FinishHeadingToleranceRadians.Value;
}
foreach (var keepRunning in movement.Get())
{
yield return keepRunning;
}
continue;
}
if (segment is RotateInPlaceMotionPlanSegment rotate)
{
var config = PilotDefinition.Conf;
var movement =
new MultiWheelRotateInPlace
{
AngleTarget =
(float)AngleMath.RadiansToDegrees(
rotate.TargetYawRadians),
StateProvider = stateProvider,
PidparamsRead = () => new PIDParams
{
Kp = config.InPlaceRotateKp,
Ki = config.InPlaceRotateKi,
Kd = config.InPlaceRotateKd,
DeadZone =
config.InPlaceRotateArriveDeg,
SpeedAccPerSec =
config.InPlaceRotateAcc,
OutputUpperThreshold =
config.InPlaceRotateMaxSpeed,
MaxI = config.InPlaceRotateMaxI
},
MinimumAngularSpeedDegreesPerSecond =
config.InPlaceRotateMinimumSpeed,
WheelAlignmentToleranceDegrees =
config.InPlaceRotateWheelAlignDeg,
RotationTimeoutSeconds =
config.InPlaceRotateTimeoutSec,
CommandAngularSpeedObserver =
commandDegreesPerSecond =>
RotationCommandObserver?.Invoke(
index,
AngleMath.DegreesToRadians(
commandDegreesPerSecond))
};
ConfigureRotationMovement?.Invoke(movement);
foreach (var keepRunning in movement.Get())
{
yield return keepRunning;
}
continue;
}
throw new NotSupportedException(
$"组合运动计划不支持动作段类型：{segment.GetType().FullName}。");
}
}
}
}
MultiWheelC/build/Clumsy/CommonUsage.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.pdb
BIN
View File
Binary file not shown.