public bool ClearanceGranted(int edgeId, TimeSpan time)
{
if (!IsUnclearedEdgeAvailable(edgeId))
{
return(false);
}
var t = (time - Created).TotalSeconds;
// determine status at time of clearance granting
GetStatus(t, out var initialAcceleration, out var initialVelocity, out var initialDistance, out _, out _);
// Get all constraints starting from initial distance. First constraint will be shortened
var trimmedConstraintsList = GetConstraintsFrom(initialDistance);
// Get constraints until
var clearedConstraintsList = trimmedConstraintsList.TakeUntil(c => c.CorrespondingEdge.Id == edgeId);
if (clearedConstraintsList.Last().CorrespondingEdge.Id != edgeId)
{
_logger?.LogDebug($"Clearance granted WTF: Edge-{edgeId} => {string.Join(",", clearedConstraintsList.Select(c => c.CorrespondingEdge.Id))}");
}
_logger?.LogDebug($"Clearance granted: Edge-{edgeId} => {string.Join(",", clearedConstraintsList.Select(c => c.CorrespondingEdge.Id))}");
// create new motion profile
_jointMotionProfile = new JointMotionProfile(_parameters, initialAcceleration, initialVelocity, clearedConstraintsList);
_jointMotionProfileCreatedTime = t;
_jointMotionProfileCreatedDistance = initialDistance;
// increase cleared length
ClearedLength = initialDistance + clearedConstraintsList.Last().End;
return(true);
}
public void StartAndEndStatusAreCorrect()
{
var jmp = new JointMotionProfile(_parameters,
new VelocityConstraint(0, 1000, 500),
new VelocityConstraint(1000, 1000, 200),
new VelocityConstraint(2000, 1000, 800));
jmp.GetStatus(0, out _, out var v0, out var s0);
Assert.Equal(0.0, v0);
Assert.Equal(0.0, s0);
Assert.NotEqual(0.0, jmp.TotalDuration);
jmp.GetStatus(jmp.TotalDuration, out _, out var vEnd, out var sEnd);
Assert.Equal(0.0, vEnd);
Assert.Equal(3000.0, sEnd);
}
