private void HandleBehavior(ChangeLaneBehavior cb) { AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp); startingLanePath = cb.BackupStartLanePath.Transform(absTransform); endingLanePath = cb.BackupTargetLanePath.Transform(absTransform); startingNumLanesLeft = cb.StartingNumLanesLeft; startingNumLanesRight = cb.StartingNumLaneRights; startingLaneWidth = cb.StartLaneWidth; endingLaneWidth = cb.TargetLaneWidth; startingLaneID = cb.StartLane; endingLaneID = cb.TargetLane; changeLeft = cb.ChangeLeft; if (cb.MaxDist > -TahoeParams.FL) { dist = cb.MaxDist + TahoeParams.FL; } else { dist = 0; } speedCommand = cb.SpeedCommand; ignorableObstacles = cb.IgnorableObstacles; Services.ObstaclePipeline.LastIgnoredObstacles = cb.IgnorableObstacles; behaviorTimestamp = absTransform.Timestamp; }
public override void OnBehaviorReceived(Behavior b) { if (b is ChangeLaneBehavior) { ChangeLaneBehavior cb = (ChangeLaneBehavior)b; if (cb.StartLane.Equals(startingLaneID) && cb.TargetLane.Equals(endingLaneID)) { LinePath startingLanePath = cb.BackupStartLanePath; LinePath endingLanePath = cb.BackupTargetLanePath; Services.UIService.PushAbsolutePath(startingLanePath, cb.TimeStamp, "original path1"); Services.UIService.PushAbsolutePath(endingLanePath, cb.TimeStamp, "original path2"); Services.BehaviorManager.QueueParam(cb); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in change lanes -- got change lanes behavior with same start/end lane"); } else { // this is a new behavior Services.BehaviorManager.Execute(b, null, false); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in change lanes -- got change lanes with different start/end lane ({0}/{1})", cb.StartLane, cb.TargetLane); } } else { Services.BehaviorManager.Execute(b, null, false); } }
public UrbanChallenge.Behaviors.Behavior Resume(VehicleState currentState, double speed) { double distLeft = Parameters.TargetOncoming ? Parameters.Target.DistanceBetween(Parameters.DepartUpperBound, currentState.Front) : Parameters.Target.DistanceBetween(currentState.Front, Parameters.DepartUpperBound); LaneChangeParameters chosen = this.Parameters; this.Parameters.DistanceToDepartUpperBound = distLeft; // distance to stop from max v given desired acceleration //double stopEnvelopeLength = -Math.Pow(this.Parameters.Target.Way.Segment.SpeedLimits.MaximumSpeed, 2) / // (2.0 * -0.5); //double tmpSpeed = distLeft / this.Parameters.Parameters.DistanceToGo * this.Parameters.Parameters.RecommendedSpeed; // create behavior ChangeLaneBehavior clb = new ChangeLaneBehavior( chosen.Initial.LaneId, chosen.Target.LaneId, chosen.ToLeft, distLeft, new ScalarSpeedCommand(chosen.Parameters.RecommendedSpeed), chosen.Parameters.VehiclesToIgnore, chosen.InitialOncoming ? chosen.Initial.ReversePath : chosen.Initial.LanePath(), chosen.TargetOncoming ? chosen.Target.ReversePath : chosen.Target.LanePath(), chosen.Initial.Width, chosen.Target.Width, chosen.InitialOncoming ? chosen.Initial.NumberOfLanesRight(currentState.Position, true) : chosen.Initial.NumberOfLanesLeft(currentState.Position, true), chosen.InitialOncoming ? chosen.Initial.NumberOfLanesLeft(currentState.Position, true) : chosen.Initial.NumberOfLanesRight(currentState.Position, true)); // return behavior return(clb); }
public override void Initialize(Behavior b) { base.Initialize(b); Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; ChangeLaneBehavior cb = (ChangeLaneBehavior)b; Services.UIService.PushAbsolutePath(cb.BackupStartLanePath, cb.TimeStamp, "original path1"); Services.UIService.PushAbsolutePath(cb.BackupTargetLanePath, cb.TimeStamp, "original path2"); startTimestamp = b.TimeStamp; HandleBehavior(cb); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in change lanes -- initializing, start timestamp {0}", b.TimeStamp); }
public override void Process(object param) { try { Trace.CorrelationManager.StartLogicalOperation("ChangeLanes"); if (!base.BeginProcess()) { return; } // check if we were given a parameter if (param != null && param is ChangeLaneBehavior) { ChangeLaneBehavior clParam = (ChangeLaneBehavior)param; HandleBehavior(clParam); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "got new param -- speed {0}, dist {1}, dist timestamp {2}", clParam.SpeedCommand, clParam.MaxDist, clParam.TimeStamp); } // project the lane paths up to the current time RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp); LinePath curStartingPath = startingLanePath.Transform(relTransform); LinePath curEndingPath = endingLanePath.Transform(relTransform); // get the starting and ending lane models ILaneModel startingLaneModel, endingLaneModel; Services.RoadModelProvider.GetLaneChangeModels(curStartingPath, startingLaneWidth, startingNumLanesLeft, startingNumLanesRight, curEndingPath, endingLaneWidth, changeLeft, behaviorTimestamp, out startingLaneModel, out endingLaneModel); // calculate the max speed // TODO: make this look ahead for slowing down settings.maxSpeed = GetMaxSpeed(endingLanePath, endingLanePath.ZeroPoint); // get the remaining lane change distance double remainingDist = GetRemainingLaneChangeDistance(); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "remaining distance is {0}", remainingDist); if (cancelled) { return; } // check if we're done if (remainingDist <= 0) { // create a new stay in lane behavior int deltaLanes = changeLeft ? -1 : 1; StayInLaneBehavior stayInLane = new StayInLaneBehavior(endingLaneID, speedCommand, null, endingLanePath, endingLaneWidth, startingNumLanesLeft + deltaLanes, startingNumLanesRight - deltaLanes); stayInLane.TimeStamp = behaviorTimestamp.ts; Services.BehaviorManager.Execute(stayInLane, null, false); // send completion report ForwardCompletionReport(new SuccessCompletionReport(typeof(ChangeLaneBehavior))); return; } // calculate the planning distance double planningDist = GetPlanningDistance(); if (planningDist < remainingDist + TahoeParams.VL) { planningDist = remainingDist + TahoeParams.VL; } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "planning dist {0}", planningDist); // create a linearization options structure LinearizationOptions laneOpts = new LinearizationOptions(); laneOpts.Timestamp = curTimestamp; // get the center line of the target lane starting at the distance we want to enter into it laneOpts.StartDistance = remainingDist; laneOpts.EndDistance = planningDist; LinePath centerLine = endingLaneModel.LinearizeCenterLine(laneOpts); // add the final center line as a weighting AddTargetPath(centerLine.Clone(), default_lane_alpha_w); // pre-pend (0,0) as our position centerLine.Insert(0, new Coordinates(0, 0)); LinePath leftBound = null; LinePath rightBound = null; // figure out if the lane is to the right or left of us laneOpts.EndDistance = planningDist; double leftEndingStartDist, rightEndingStartDist; GetLaneBoundStartDists(curEndingPath, endingLaneWidth, out leftEndingStartDist, out rightEndingStartDist); if (changeLeft) { // we're the target lane is to the left // get the left bound of the target lane laneOpts.StartDistance = Math.Max(leftEndingStartDist, TahoeParams.FL); leftBound = endingLaneModel.LinearizeLeftBound(laneOpts); } else { // we're changing to the right, get the right bound of the target lane laneOpts.StartDistance = Math.Max(rightEndingStartDist, TahoeParams.FL); rightBound = endingLaneModel.LinearizeRightBound(laneOpts); } // get the other bound as the starting lane up to 5m before the remaining dist laneOpts.StartDistance = TahoeParams.FL; laneOpts.EndDistance = Math.Max(0, remainingDist - 5); if (changeLeft) { if (laneOpts.EndDistance > 0) { rightBound = startingLaneModel.LinearizeRightBound(laneOpts); } else { rightBound = new LinePath(); } } else { if (laneOpts.EndDistance > 0) { leftBound = startingLaneModel.LinearizeLeftBound(laneOpts); } else { leftBound = new LinePath(); } } // append on the that bound of the target lane starting at the remaining dist laneOpts.StartDistance = Math.Max(remainingDist, TahoeParams.FL); laneOpts.EndDistance = planningDist; if (changeLeft) { rightBound.AddRange(endingLaneModel.LinearizeRightBound(laneOpts)); } else { leftBound.AddRange(endingLaneModel.LinearizeLeftBound(laneOpts)); } // set up the planning smootherBasePath = centerLine; AddLeftBound(leftBound, !changeLeft); AddRightBound(rightBound, changeLeft); Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true); Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true); Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true); if (cancelled) { return; } // set auxillary options settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan; // smooth and track that stuff SmoothAndTrack(commandLabel, true); } finally { Trace.CorrelationManager.StopLogicalOperation(); } }
/// <summary> /// Plan a lane change /// </summary> /// <param name="cls"></param> /// <param name="initialManeuver"></param> /// <param name="targetManeuver"></param> /// <returns></returns> public Maneuver PlanLaneChange(ChangeLanesState cls, VehicleState vehicleState, RoadPlan roadPlan, List <ITacticalBlockage> blockages, List <ArbiterWaypoint> ignorable) { // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage) { // create the blockage state EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); // go to a blockage handling tactical return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } // lanes of the lane change ArbiterLane initial = cls.Parameters.Initial; ArbiterLane target = cls.Parameters.Target; #region Initial Forwards if (!cls.Parameters.InitialOncoming) { ForwardReasoning initialReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), initial); #region Target Forwards if (!cls.Parameters.TargetOncoming) { // target reasoning ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target); #region Navigation if (cls.Parameters.Reason == LaneChangeReason.Navigation) { // parameters to follow List <TravelingParameters> tps = new List <TravelingParameters>(); // vehicles to ignore List <int> ignorableVehicles = new List <int>(); // params for forward lane initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable); TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle); ArbiterOutput.Output("initial dist to go: " + initialParams.DistanceToGo.ToString("f3")); if (initialParams.Type == TravellingType.Vehicle && !initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) { tps.Add(initialParams); } else { tps.Add(initialReasoning.ForwardMonitor.NavigationParameters); } ignorableVehicles.AddRange(initialParams.VehiclesToIgnore); // get params for the final lane targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List <ArbiterWaypoint>()); TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters; tps.Add(targetParams); ignorableVehicles.AddRange(targetParams.VehiclesToIgnore); try { if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 && targetParams.Type == TravellingType.Vehicle && targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle != null && targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed) { return(new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } } catch (Exception) { } ArbiterOutput.Output("target dist to go: " + targetParams.DistanceToGo.ToString("f3")); // decorators List <BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator; // distance double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // check if need to modify distance to go if (initialParams.Type == TravellingType.Vehicle && initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) { double curDistToUpper = cls.Parameters.DistanceToDepartUpperBound; double newVhcDistToUpper = initial.DistanceBetween(vehicleState.Front, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0; if (curDistToUpper > newVhcDistToUpper) { distanceToGo = newVhcDistToUpper; } } // get final tps.Sort(); // get the proper speed command ScalarSpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed); if (sc.Speed < 8.84) { sc = new ScalarSpeedCommand(Math.Min(targetParams.RecommendedSpeed, 8.84)); } // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo, sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true), initial.NumberOfLanesRight(vehicleState.Front, true)); // standard maneuver return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp)); } #endregion #region Failed Forwards else if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle) { // parameters to follow List <TravelingParameters> tps = new List <TravelingParameters>(); // vehicles to ignore List <int> ignorableVehicles = new List <int>(); // params for forward lane initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable); TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null); tps.Add(initialParams); ignorableVehicles.AddRange(initialParams.VehiclesToIgnore); // get params for the final lane targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List <ArbiterWaypoint>()); TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters; tps.Add(targetParams); ignorableVehicles.AddRange(targetParams.VehiclesToIgnore); // decorators List <BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator; // distance double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // get final tps.Sort(); // get the proper speed command SpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed); // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo, sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true), initial.NumberOfLanesRight(vehicleState.Front, true)); // standard maneuver return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp)); } #endregion #region Slow else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } #endregion else { // fallout exception throw new Exception("currently unsupported lane change type"); } } #endregion #region Target Oncoming else { OpposingReasoning targetReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), target); #region Failed Forward if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle) { // parameters to follow List <TravelingParameters> tps = new List <TravelingParameters>(); // ignore the forward vehicle but keep params for forward lane initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable); TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null); tps.Add(initialParams); // get params for the final lane targetReasoning.ForwardManeuver(target, initial, vehicleState, roadPlan, blockages); TravelingParameters targetParams = targetReasoning.OpposingForwardMonitor.CurrentParamters.Value; tps.Add(targetParams); // decorators List <BehaviorDecorator> decorators = cls.Parameters.ToLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator; // distance double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // get final tps.Sort(); // get the proper speed command SpeedCommand sc = new ScalarSpeedCommand(Math.Min(tps[0].RecommendedSpeed, 2.24)); // check final for stopped failed opposing VehicleAgent forwardVa = targetReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle; if (forwardVa != null) { // dist between double distToFV = -targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition); // check stopped bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5; // check distance bool distOk = distToFV < 2.5 * TahoeParams.VL; // check failed bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed; // notify ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString()); // check all for failed if (stopped && distOk && failed) { // check inside target if (target.LanePolygon.IsInside(vehicleState.Front)) { // blockage recovery StayInLaneState sils = new StayInLaneState(initial, CoreCommon.CorePlanningState); StayInLaneBehavior silb = new StayInLaneBehavior(initial.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List <int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false)); BlockageRecoveryState brs = new BlockageRecoveryState(silb, sils, sils, BlockageRecoveryDEFCON.REVERSE, new EncounteredBlockageState(new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, 4.0, BlockageType.Static, -1, true, silb.GetType())), sils, BlockageRecoveryDEFCON.INITIAL, SAUDILevel.None), BlockageRecoverySTATUS.EXECUTING); return(new Maneuver(silb, brs, TurnDecorators.HazardDecorator, vehicleState.Timestamp)); } // check which lane we are in else { // return to forward lane return(new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(initial, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } } } // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo, sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.ReversePath, initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true), initial.NumberOfLanesRight(vehicleState.Front, true)); // standard maneuver return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp)); } #endregion #region Other else if (cls.Parameters.Reason == LaneChangeReason.Navigation) { // fallout exception throw new Exception("currently unsupported lane change type"); } else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } else { // fallout exception throw new Exception("currently unsupported lane change type"); } #endregion } #endregion } #endregion #region Initial Oncoming else { OpposingReasoning initialReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), initial); #region Target Forwards if (!cls.Parameters.TargetOncoming) { ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target); if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } #region Navigation else if (cls.Parameters.Reason == LaneChangeReason.Navigation) { // parameters to follow List <TravelingParameters> tps = new List <TravelingParameters>(); // distance to the upper bound of the change double distanceToGo = target.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // get params for the initial lane initialReasoning.ForwardManeuver(initial, target, vehicleState, roadPlan, blockages); // current params of the fqm TravelingParameters initialParams = initialReasoning.OpposingForwardMonitor.CurrentParamters.Value; if (initialParams.Type == TravellingType.Vehicle) { if (!initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) { tps.Add(initialParams); } else { tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters); distanceToGo = initial.DistanceBetween(initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, vehicleState.Front) - TahoeParams.VL; } } else { tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters); } // get params for forward lane targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, ignorable); TravelingParameters targetParams = targetReasoning.ForwardMonitor.ParameterizationHelper(target, target, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? target.WaypointList[target.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle); tps.Add(targetParams); // ignoring vehicles add List <int> ignoreVehicles = initialParams.VehiclesToIgnore; ignoreVehicles.AddRange(targetParams.VehiclesToIgnore); // decorators List <BehaviorDecorator> decorators = !cls.Parameters.ToLeft ? TurnDecorators.RightTurnDecorator : TurnDecorators.LeftTurnDecorator; // get final tps.Sort(); // get the proper speed command SpeedCommand sc = tps[0].SpeedCommand; if (sc is StopAtDistSpeedCommand) { sc = new ScalarSpeedCommand(0.0); } // check final for stopped failed opposing VehicleAgent forwardVa = targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle; if (forwardVa != null) { // dist between double distToFV = targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition); // check stopped bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5; // check distance bool distOk = distToFV < 2.5 * TahoeParams.VL; // check failed bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed; // notify ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString()); // check all for failed if (stopped && distOk && failed) { // check which lane we are in if (initial.LanePolygon.IsInside(vehicleState.Front)) { // return to opposing lane return(new Maneuver(new HoldBrakeBehavior(), new OpposingLanesState(initial, true, CoreCommon.CorePlanningState, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } else { // lane state return(new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } } } // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo, sc, ignoreVehicles, initial.ReversePath, target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false)); // standard maneuver return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp)); } #endregion else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } else { // fallout exception throw new Exception("currently unsupported lane change type"); } } #endregion else { // fallout exception throw new Exception("currently unsupported lane change type"); } } #endregion }