376 lines
14 KiB
C#
376 lines
14 KiB
C#
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 (!TrajectoryExperimentInput
|
|
.TryReadLateralOffsetMeters(
|
|
out var lateralOffsetMeters))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (!MovementTestPreparation.AreWheelsForward())
|
|
{
|
|
return;
|
|
}
|
|
|
|
var chassis = PilotDefinition.Chassis as MultiWheelChassis;
|
|
if (chassis == null)
|
|
{
|
|
Console.WriteLine(
|
|
"当前底盘不是MultiWheelChassis，无法执行组合运动测试。");
|
|
return;
|
|
}
|
|
|
|
var detourStateProvider =
|
|
new DetourVehicleStateProvider();
|
|
if (!detourStateProvider.TryGetState(
|
|
out var initialState))
|
|
{
|
|
Console.WriteLine(
|
|
"无法读取组合运动起点位姿：" +
|
|
detourStateProvider.LastFailureReason);
|
|
return;
|
|
}
|
|
|
|
var stateProvider =
|
|
new WheelFeedbackVehicleStateProvider(
|
|
detourStateProvider,
|
|
chassis);
|
|
|
|
var planStartPose =
|
|
TrajectoryExperimentInput.OffsetPoseLaterally(
|
|
initialState.PoseInWorld,
|
|
lateralOffsetMeters);
|
|
var firstTrajectory =
|
|
TestTrajectoryFactory
|
|
.CreateStraightSmoothLeftTurnStraight(
|
|
planStartPose,
|
|
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:
|
|
TrajectoryExperimentInput.BuildTrajectoryName(
|
|
"SmoothTurnStopRotateStraight",
|
|
lateralOffsetMeters),
|
|
trialNumber: TrialNumber,
|
|
referenceStart: ToMillimeterVector(
|
|
firstTrajectory.StartPoint.PoseInWorld),
|
|
referenceEnd: ToMillimeterVector(
|
|
finalTrajectory.EndPoint.PoseInWorld),
|
|
referenceSpeed:
|
|
(float)StraightMaximumSpeedMetersPerSecond,
|
|
sampleIntervalMs: 50,
|
|
referenceAccelerationMetersPerSecondSquared:
|
|
(float)AccelerationMetersPerSecondSquared,
|
|
referenceDecelerationMetersPerSecondSquared:
|
|
(float)DecelerationMetersPerSecondSquared,
|
|
diagnosticChassis: chassis);
|
|
_recorder.Start();
|
|
|
|
var controlPointRadiusMeters =
|
|
chassis.ControlPointRadius /
|
|
MillimetersPerMeter;
|
|
var movement = new MotionPlanExecutor
|
|
{
|
|
Segments = plan,
|
|
StateProvider = stateProvider,
|
|
ConfigureTrackingMovement = tracking =>
|
|
{
|
|
tracking.StanleyUsesActualSpeed = true;
|
|
tracking.MaximumCommandSpeedMetersPerSecond =
|
|
StraightMaximumSpeedMetersPerSecond;
|
|
tracking
|
|
.LongitudinalSpeedErrorDeadbandMetersPerSecond =
|
|
0.025;
|
|
},
|
|
SegmentStarted = (index, segment) =>
|
|
{
|
|
_recorder?.ClearControlReference();
|
|
_recorder?.ClearGcpCommand();
|
|
_recorder?.UpdateCommand(0f, 0f);
|
|
Console.WriteLine(
|
|
$"组合运动开始第{index + 1}段：" +
|
|
segment.GetType().Name);
|
|
},
|
|
TrackingCycleObserver = (index, controller) =>
|
|
RecordTrackingCycle(
|
|
controller,
|
|
controlPointRadiusMeters,
|
|
stateProvider),
|
|
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,
|
|
WheelFeedbackVehicleStateProvider stateProvider)
|
|
{
|
|
if (controller.LastVehicleState.HasValue)
|
|
{
|
|
_recorder?.UpdateProcessedState(
|
|
controller.LastVehicleState.Value);
|
|
}
|
|
|
|
if (stateProvider.TryGetLatestVelocityDiagnostics(
|
|
out var detourBodyVx,
|
|
out var detourVelocityValid,
|
|
out var rawWheelBodyVx,
|
|
out var filteredWheelBodyVx,
|
|
out var wheelVelocityValid))
|
|
{
|
|
_recorder?.UpdateVelocityDiagnostics(
|
|
detourBodyVx,
|
|
detourVelocityValid,
|
|
rawWheelBodyVx,
|
|
filteredWheelBodyVx,
|
|
wheelVelocityValid);
|
|
}
|
|
|
|
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;
|
|
_recorder?.UpdateGcpCommand(
|
|
command.FrontAngleRadians,
|
|
command.RearAngleRadians);
|
|
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));
|
|
}
|
|
}
|
|
}
|