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; }