32 lines
1.6 KiB
C#
32 lines
1.6 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>将不可变执行状态转换为控制器中立的纵向速度和 yaw rate 命令，并在停稳/完成时强制零运动。</summary>
|
|
public sealed class TrajectoryControlAdapter
|
|
{
|
|
/// <summary>依据选中轨迹点和换向状态创建安全控制命令。</summary>
|
|
/// <param name="trajectoryPoint">已验证轨迹中的选中点，提供带符号速度、yaw rate 和方向。</param>
|
|
/// <param name="executionState">不可变执行状态；停零、完成或不允许运动时优先于轨迹点运动字段。</param>
|
|
/// <returns>常规跟随时保留轨迹点 m/s 和 rad/s；保持或完成时返回零速度、零 yaw rate 和制动命令。</returns>
|
|
public TrajectoryControlCommand CreateCommand(EmTrajectoryPoint trajectoryPoint,
|
|
TrajectoryExecutionState executionState)
|
|
{
|
|
if (trajectoryPoint == null)
|
|
throw new ArgumentNullException(nameof(trajectoryPoint));
|
|
if (executionState == null)
|
|
throw new ArgumentNullException(nameof(executionState));
|
|
|
|
bool holdBrake = executionState.HoldZero || executionState.IsTrajectoryComplete ||
|
|
!executionState.AllowsTrajectoryMotion;
|
|
if (holdBrake)
|
|
{
|
|
return new TrajectoryControlCommand(0d, 0d, trajectoryPoint.Direction,
|
|
executionState.RequestDirectionChange, true, executionState.IsTrajectoryComplete);
|
|
}
|
|
|
|
return new TrajectoryControlCommand(trajectoryPoint.SignedLongitudinalVelocity, trajectoryPoint.YawRate,
|
|
trajectoryPoint.Direction, false, false, false);
|
|
}
|
|
}
|