28 lines
1.1 KiB
C#
28 lines
1.1 KiB
C#
using System;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Converts immutable execution state into a controller-neutral longitudinal and yaw command.</summary>
|
|
public sealed class TrajectoryControlAdapter
|
|
{
|
|
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);
|
|
}
|
|
}
|