private void ExecutePass()
{
// determine what to do
// get the moving order
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
curTimestamp = pose.timestamp;
ParkerVehicleState state = new ParkerVehicleState(pose.xy, Coordinates.FromAngle(pose.heading));
if (pulloutMode)
{
movingOrder = parker.GetNextPulloutOrder(state);
}
else
{
movingOrder = parker.GetNextParkingOrder(state);
}
// test for completion
bool isPullinCompleted = false;
if (parkingSpotLine.P0.DistanceTo(pose.xy) < 1.5 && Coordinates.FromAngle(pose.heading).Dot(parkingSpotLine.UnitVector) >= 0.5)
{
isPullinCompleted = true;
}
finalPass = false;
if (movingOrder != null && movingOrder.DestState != null && parkingSpotLine.P0.DistanceTo(movingOrder.DestState.Loc) < 1.5 && parkingSpotLine.UnitVector.Dot(movingOrder.DestState.Heading) >= 0.5)
{
finalPass = true;
}
if (movingOrder.Completed || (isPullinCompleted && !pulloutMode))
{
Type type;
if (pulloutMode)
{
type = typeof(ZoneParkingPullOutBehavior);
}
else
{
type = typeof(ZoneParkingBehavior);
}
Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(type));
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
}
else
{
// determine the stopping distance
TransmissionGear targetGear;
if (movingOrder.Forward)
{
targetGear = TransmissionGear.First;
}
else
{
targetGear = TransmissionGear.Reverse;
}
bool counterClockwise = false;
if ((movingOrder.Forward && movingOrder.TurningRadius > 0) || (!movingOrder.Forward && movingOrder.TurningRadius < 0))
{
counterClockwise = true;
}
else
{
counterClockwise = false;
}
double radius = pose.xy.DistanceTo(movingOrder.CenterPoint);
double startAngle = (pose.xy - movingOrder.CenterPoint).ArcTan;
double endAngle = (movingOrder.DestState.Loc - movingOrder.CenterPoint).ArcTan;
if (counterClockwise)
{
if (endAngle < startAngle)
{
endAngle += 2 * Math.PI;
}
}
else
{
if (endAngle > startAngle)
{
endAngle -= 2 * Math.PI;
}
}
double stopDistance;
if (Math.Abs(radius) < 100)
{
stopDistance = Math.Abs(radius * (endAngle - startAngle));
}
else
{
stopDistance = pose.xy.DistanceTo(movingOrder.DestState.Loc);
}
this.stopDist = stopDistance;
this.stopTimestamp = curTimestamp;
Services.UIService.PushPoint(movingOrder.DestState.Loc, curTimestamp, "uturn stop point", false);
//double sign = movingOrder.Forward ? 1 : -1;
double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1 / movingOrder.TurningRadius, parking_speed);
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(targetGear);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), parking_speed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = command_label;
Services.TrackingManager.QueueCommand(cmd);
}
passCompleted = false;
}
private void ExecutePass()
{
// determine what to do
// get the moving order
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
curTimestamp = pose.timestamp;
ParkerVehicleState state = new ParkerVehicleState(pose.xy, Coordinates.FromAngle(pose.heading));
if (pulloutMode) {
movingOrder = parker.GetNextPulloutOrder(state);
}
else {
movingOrder = parker.GetNextParkingOrder(state);
}
// test for completion
bool isPullinCompleted = false;
if (parkingSpotLine.P0.DistanceTo(pose.xy) < 1.5 && Coordinates.FromAngle(pose.heading).Dot(parkingSpotLine.UnitVector) >= 0.5) {
isPullinCompleted = true;
}
finalPass = false;
if (movingOrder != null && movingOrder.DestState != null && parkingSpotLine.P0.DistanceTo(movingOrder.DestState.Loc) < 1.5 && parkingSpotLine.UnitVector.Dot(movingOrder.DestState.Heading) >= 0.5) {
finalPass = true;
}
if (movingOrder.Completed || (isPullinCompleted && !pulloutMode)) {
Type type;
if (pulloutMode) {
type = typeof(ZoneParkingPullOutBehavior);
}
else {
type = typeof(ZoneParkingBehavior);
}
Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(type));
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
}
else {
// determine the stopping distance
TransmissionGear targetGear;
if (movingOrder.Forward) {
targetGear = TransmissionGear.First;
}
else {
targetGear = TransmissionGear.Reverse;
}
bool counterClockwise = false;
if ((movingOrder.Forward && movingOrder.TurningRadius > 0) || (!movingOrder.Forward && movingOrder.TurningRadius < 0)) {
counterClockwise = true;
}
else {
counterClockwise = false;
}
double radius = pose.xy.DistanceTo(movingOrder.CenterPoint);
double startAngle = (pose.xy - movingOrder.CenterPoint).ArcTan;
double endAngle = (movingOrder.DestState.Loc - movingOrder.CenterPoint).ArcTan;
if (counterClockwise) {
if (endAngle < startAngle) {
endAngle += 2*Math.PI;
}
}
else {
if (endAngle > startAngle) {
endAngle -= 2*Math.PI;
}
}
double stopDistance;
if (Math.Abs(radius) < 100) {
stopDistance = Math.Abs(radius * (endAngle - startAngle));
}
else {
stopDistance = pose.xy.DistanceTo(movingOrder.DestState.Loc);
}
this.stopDist = stopDistance;
this.stopTimestamp = curTimestamp;
Services.UIService.PushPoint(movingOrder.DestState.Loc, curTimestamp, "uturn stop point", false);
//double sign = movingOrder.Forward ? 1 : -1;
double steeringCommand = SteeringUtilities.CurvatureToSteeringWheelAngle(1/movingOrder.TurningRadius, parking_speed);
ISpeedCommandGenerator shiftSpeedCommand = new ShiftSpeedCommand(targetGear);
ISteeringCommandGenerator initialSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, steeringRate, true);
ISpeedCommandGenerator passSpeedCommand = new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new TravelledDistanceProvider(curTimestamp, stopDistance), parking_speed));
ISteeringCommandGenerator passSteeringCommand = new ConstantSteeringCommandGenerator(steeringCommand, null, false);
ChainedTrackingCommand cmd = new ChainedTrackingCommand(
new TrackingCommand(shiftSpeedCommand, initialSteeringCommand, true),
new TrackingCommand(passSpeedCommand, passSteeringCommand, false));
cmd.Label = command_label;
Services.TrackingManager.QueueCommand(cmd);
}
passCompleted = false;
}
