/// <summary> /// Makes new parameterization for nav /// </summary> /// <param name="lane"></param> /// <param name="lanePlan"></param> /// <param name="speed"></param> /// <param name="distance"></param> /// <param name="stopType"></param> /// <returns></returns> public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance, ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state) { // get min dist double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance; #region Get Decorators // turn direction default ArbiterTurnDirection atd = ArbiterTurnDirection.Straight; List <BehaviorDecorator> decorators = TurnDecorators.NoDecorators; // check if need decorators if (lane is ArbiterLane && stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) && roadPlan.BestPlan.laneWaypointOfInterest.IsExit && distance < 40.0) { if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null) { ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization"); } else { switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } else if (lane is SupraLane) { SupraLane sl = (SupraLane)lane; double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position); if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect) { switch (sl.Interconnect.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } #endregion #region Get Maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new StopAtDistSpeedCommand(distance); #region Distance Cutoff // check if distance is less than cutoff if (distance < distanceCutOff && stopType != StopType.EndOfLane) { // default behavior Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List <int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); // stopping so not using speed param usingSpeed = false; // exit is next if (stopType == StopType.Exit) { // exit means stopping at a good exit in our current lane IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front); m = new Maneuver(b, nextState, decorators, state.Timestamp); } // stop line is left else if (stopType == StopType.StopLine) { // determine if hte stop line is the best exit bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint); // get turn direction atd = isNavExit ? atd : ArbiterTurnDirection.Straight; // predetermine interconnect if best exit ArbiterInterconnect desired = null; if (isNavExit) { desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit; } else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25) { desired = stopWaypoint.NextPartition.ToInterconnect; } // set decorators decorators = isNavExit ? decorators : TurnDecorators.NoDecorators; // stop at the stop IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired); b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List <int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); m = new Maneuver(b, nextState, decorators, state.Timestamp); sc = new StopAtLineSpeedCommand(); } else if (stopType == StopType.LastGoal) { // stop at the last goal IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState); m = new Maneuver(b, nextState, decorators, state.Timestamp); } } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // set speed sc = new ScalarSpeedCommand(speed); // check if lane if (lane is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)lane; // default behavior Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List <int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp); } // check if supra lane else if (lane is SupraLane) { // get lane SupraLane sl = (SupraLane)lane; // get sl state StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; // get default beheavior Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List <int>()); // standard behavior is fine for maneuver m = new Maneuver(b, sisls, decorators, state.Timestamp); } } #endregion #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = distance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List <int>(); // return navigation params return(tp); #endregion }
public static INavigableNode FilterGoal(VehicleState state) { // get goal INavigableNode goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null; if (waitRemoveLastGoal && CoreCommon.Mission.MissionCheckpoints.Count != 1) { waitRemoveLastGoal = false; } // id IArbiterWaypoint goalWp = null; if (goal != null) { goalWp = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]; } // check lane change or opposing if (goal != null && (CoreCommon.CorePlanningState is OpposingLanesState && ((OpposingLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId)) || (CoreCommon.CorePlanningState is ChangeLanesState && ((ChangeLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId))) { if (CoreCommon.Mission.MissionCheckpoints.Count == 1) { waitRemoveLastGoal = true; ArbiterOutput.Output("Waiting to remove last Checkpoint: " + goal.ToString()); } else { // set hit ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString()); CoreCommon.Mission.MissionCheckpoints.Dequeue(); // update goal goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null; } } else if (goal != null && CoreCommon.Mission.MissionCheckpoints.Count == 1 && waitRemoveLastGoal && (CoreCommon.CorePlanningState is StayInLaneState || CoreCommon.CorePlanningState is StayInSupraLaneState)) { // set hit ArbiterOutput.Output("Wait over, Reached Checkpoint: " + goal.ToString()); CoreCommon.Mission.MissionCheckpoints.Dequeue(); // update goal goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null; } // TODO implement full version of hit test // check if we have hit the goal (either by being in opposing lane or going to opposing and next to it or in lane and pass over it else if (goal != null) { bool reachedCp = false; if (CoreCommon.CorePlanningState is StayInLaneState) { StayInLaneState sils = (StayInLaneState)CoreCommon.CorePlanningState; if (goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(sils.Lane)) { if (CoreCommon.Mission.MissionCheckpoints.Count != 1) { double distanceAlong = sils.Lane.DistanceBetween(state.Front, goal.Position); if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0) { reachedCp = true; } } else { double distanceAlong = sils.Lane.DistanceBetween(state.Front, goal.Position); double distanceAlong2 = sils.Lane.DistanceBetween(state.Position, goal.Position); if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.005 && Math.Abs(distanceAlong) < 0.3 || CoreCommon.Communications.GetVehicleState().VehiclePolygon.IsInside(goal.Position) || (distanceAlong <= 0.0 && distanceAlong2 >= 0)) { reachedCp = true; } } } } else if (CoreCommon.CorePlanningState is ChangeLanesState) { ChangeLanesState cls = (ChangeLanesState)CoreCommon.CorePlanningState; if (cls.Parameters.Initial.Way.Equals(cls.Parameters.Target.Way) && goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(cls.Parameters.Target)) { double distanceAlong = cls.Parameters.Target.DistanceBetween(state.Front, goal.Position); if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0) { reachedCp = true; ArbiterOutput.Output("Removed goal changing lanes"); } } } if (reachedCp) { // set hit ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString()); CoreCommon.Mission.MissionCheckpoints.Dequeue(); // update goal goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null; } } // set goal info CoreCommon.CurrentInformation.RouteCheckpoint = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? goal.ToString() : "NONE"; CoreCommon.CurrentInformation.GoalsRemaining = CoreCommon.Mission.MissionCheckpoints.Count.ToString(); CoreCommon.CurrentInformation.RouteCheckpointId = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() : "NONE"; // return current return(goal); }
/// <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 }
/// <summary> /// Plans what maneuer we should take next /// </summary> /// <param name="planningState"></param> /// <param name="navigationalPlan"></param> /// <param name="vehicleState"></param> /// <param name="vehicles"></param> /// <param name="obstacles"></param> /// <param name="blockage"></param> /// <returns></returns> public Maneuver Plan(IState planningState, RoadPlan navigationalPlan, VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages, double vehicleSpeed) { // assign vehicles to their lanes this.roadMonitor.Assign(vehicles); // navigation tasks this.taskReasoning.navigationPlan = navigationalPlan; #region Stay in lane // maneuver given we are in a lane if (planningState is StayInLaneState) { // get state StayInLaneState sils = (StayInLaneState)planningState; // check reasoning if needs to be different if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sils.Lane)) { if (sils.Lane.LaneOnLeft == null) { this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver); } else if (sils.Lane.LaneOnLeft.Way.Equals(sils.Lane.Way)) { this.leftLateralReasoning = new LateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver); } else { this.leftLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver); } if (sils.Lane.LaneOnRight == null) { this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger); } else if (sils.Lane.LaneOnRight.Way.Equals(sils.Lane.Way)) { this.rightLateralReasoning = new LateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger); } else { this.rightLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger); } this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sils.Lane); } // populate navigation with road plan taskReasoning.SetRoadPlan(navigationalPlan, sils.Lane); // as penalties for lane changes already taken into account, can just look at // best lane plan to figure out what to do TypeOfTasks bestTask = taskReasoning.Best; // get the forward lane plan Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore); // get the secondary Maneuver?secondaryManeuver = forwardReasoning.AdvancedSecondary(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask); //forwardReasoning.SecondaryManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask); // check behavior type for uturn if (secondaryManeuver.HasValue && secondaryManeuver.Value.PrimaryBehavior is UTurnBehavior) { return(secondaryManeuver.Value); } // check if we wish to change lanes here if (bestTask != TypeOfTasks.Straight) { // parameters LaneChangeParameters parameters; secondaryManeuver = this.forwardReasoning.AdvancedDesiredLaneChangeManeuver(sils.Lane, bestTask == TypeOfTasks.Left ? true : false, navigationalPlan.BestPlan.laneWaypointOfInterest.PointOfInterest, navigationalPlan, vehicleState, blockages, sils.WaypointsToIgnore, new LaneChangeInformation(LaneChangeReason.Navigation, this.forwardReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle), secondaryManeuver, out parameters); } // final maneuver Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver; // set opposing vehicle flag if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is StayInLaneBehavior) { StayInLaneBehavior silb = (StayInLaneBehavior)finalManeuver.PrimaryBehavior; OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning; olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState); if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null) { ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState); BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count]; finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds); finalManeuver.PrimaryBehavior.Decorators = new List <BehaviorDecorator>(bds); silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed)); ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s"); } finalManeuver.PrimaryBehavior = silb; } // return the final return(finalManeuver); } #endregion #region Stay in supra lane else if (CoreCommon.CorePlanningState is StayInSupraLaneState) { // get state StayInSupraLaneState sisls = (StayInSupraLaneState)planningState; // check reasoning if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sisls.Lane)) { if (sisls.Lane.Initial.LaneOnLeft == null) { this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver); } else if (sisls.Lane.Initial.LaneOnLeft.Way.Equals(sisls.Lane.Initial.Way)) { this.leftLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver); } else { this.leftLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver); } if (sisls.Lane.Initial.LaneOnRight == null) { this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger); } else if (sisls.Lane.Initial.LaneOnRight.Way.Equals(sisls.Lane.Initial.Way)) { this.rightLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger); } else { this.rightLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger); } this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sisls.Lane); } // populate navigation with road plan taskReasoning.SetSupraRoadPlan(navigationalPlan, sisls.Lane); // as penalties for lane changes already taken into account, can just look at // best lane plan to figure out what to do // TODO: NOTE THAT THIS BEST TASK SHOULD BE IN THE SUPRA LANE!! (DO WE NEED THIS) TypeOfTasks bestTask = taskReasoning.Best; // get the forward lane plan Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore); // get hte secondary Maneuver?secondaryManeuver = forwardReasoning.AdvancedSecondary(sisls.Lane, vehicleState, navigationalPlan, blockages, new List <ArbiterWaypoint>(), bestTask); //forwardReasoning.SecondaryManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore, bestTask); // final maneuver Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver; // check if stay in lane if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is SupraLaneBehavior) { SupraLaneBehavior silb = (SupraLaneBehavior)finalManeuver.PrimaryBehavior; OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning; olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState); if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null) { ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState); BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count]; finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds); finalManeuver.PrimaryBehavior.Decorators = new List <BehaviorDecorator>(bds); silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed)); ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s"); } finalManeuver.PrimaryBehavior = silb; } // return the final return(finalManeuver); // notify /*if (secondaryManeuver.HasValue) * ArbiterOutput.Output("Secondary Maneuver"); * * // check for forward's secondary maneuver for desired behavior other than going straight * if (secondaryManeuver.HasValue) * { * // return the secondary maneuver * return secondaryManeuver.Value; * } * // otherwise our default behavior and posibly desired is going straight * else * { * // return default forward maneuver * return forwardManeuver; * }*/ } #endregion #region Change Lanes State // maneuver given we are changing lanes else if (planningState is ChangeLanesState) { // get state ChangeLanesState cls = (ChangeLanesState)planningState; LaneChangeReasoning lcr = new LaneChangeReasoning(); Maneuver final = lcr.PlanLaneChange(cls, vehicleState, navigationalPlan, blockages, new List <ArbiterWaypoint>()); #warning need to filter through waypoints to ignore so don't get stuck by a stop line //Maneuver final = new Maneuver(cls.Resume(vehicleState, vehicleSpeed), cls, cls.DefaultStateDecorators, vehicleState.Timestamp); // return the final planned maneuver return(final); /*if (!cls.parameters..TargetIsOnComing) * { * // check reasoning * if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(cls.TargetLane)) * { * if (cls.TargetLane.LaneOnLeft.Way.Equals(cls.TargetLane.Way)) * this.leftLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnLeft); * else * this.leftLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnLeft); * * if (cls.TargetLane.LaneOnRight.Way.Equals(cls.TargetLane.Way)) * this.rightLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnRight); * else * this.rightLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnRight); * * this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, cls.TargetLane); * } * * * // get speed command * double speed; * double distance; * this.forwardReasoning.ForwardMonitor.StoppingParams(new ArbiterWaypoint(cls.TargetUpperBound.pt, null), cls.TargetLane, vehicleState.Front, vehicleState.ENCovariance, out speed, out distance); * SpeedCommand sc = new ScalarSpeedCommand(Math.Max(speed, 0.0)); * cls.distanceLeft = distance; * * // get behavior * ChangeLaneBehavior clb = new ChangeLaneBehavior(cls.InitialLane.LaneId, cls.TargetLane.LaneId, cls.InitialLane.LaneOnLeft != null && cls.InitialLane.LaneOnLeft.Equals(cls.TargetLane), * distance, sc, new List<int>(), cls.InitialLane.PartitionPath, cls.TargetLane.PartitionPath, cls.InitialLane.Width, cls.TargetLane.Width); * * // plan over the target lane * //Maneuver targetManeuver = forwardReasoning.ForwardManeuver(cls.TargetLane, vehicleState, !cls.TargetIsOnComing, blockage, cls.InitialLaneState.IgnorableWaypoints); * * // plan over the initial lane * //Maneuver initialManeuver = forwardReasoning.ForwardManeuver(cls.InitialLane, vehicleState, !cls.InitialIsOncoming, blockage, cls.InitialLaneState.IgnorableWaypoints); * * // generate the change lanes command * //Maneuver final = laneChangeReasoning.PlanLaneChange(cls, initialManeuver, targetManeuver); * * } * else * { * throw new Exception("Change lanes into oncoming not supported yet by road tactical"); * }*/ } #endregion #region Opposing Lanes State // maneuver given we are in an opposing lane else if (planningState is OpposingLanesState) { // get state OpposingLanesState ols = (OpposingLanesState)planningState; ArbiterWayId opposingWay = ols.OpposingWay; ols.SetClosestGood(vehicleState); ols.ResetLaneAgent = false; // check reasoning if (this.opposingReasoning == null || !this.opposingReasoning.Lane.Equals(ols.OpposingLane)) { if (ols.OpposingLane.LaneOnRight == null) { this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver); } else if (!ols.OpposingLane.LaneOnRight.Way.Equals(ols.OpposingLane.Way)) { this.leftLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver); } else { this.leftLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver); } if (ols.OpposingLane.LaneOnLeft == null) { this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger); } else if (!ols.OpposingLane.LaneOnLeft.Way.Equals(ols.OpposingLane.Way)) { this.rightLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger); } else { this.rightLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger); } this.opposingReasoning = new OpposingReasoning(this.leftLateralReasoning, this.rightLateralReasoning, ols.OpposingLane); } // get the forward lane plan Maneuver forwardManeuver = this.opposingReasoning.ForwardManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, navigationalPlan, blockages); // get the secondary maneuver Maneuver?secondaryManeuver = null; if (ols.ClosestGoodLane != null) { secondaryManeuver = this.opposingReasoning.SecondaryManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, blockages, ols.EntryParameters); } // check for reasonings secondary maneuver for desired behavior other than going straight if (secondaryManeuver != null) { // return the secondary maneuver return(secondaryManeuver.Value); } // otherwise our default behavior and posibly desired is going straight else { // return default forward maneuver return(forwardManeuver); } } #endregion #region not imp /* #region Uturn * * // we are making a uturn * else if (planningState is uTurnState) * { * // get the uturn state * uTurnState uts = (uTurnState)planningState; * * // get the final lane we wish to be in * ArbiterLane targetLane = uts.TargetLane; * * // get operational state * Type operationalBehaviorType = CoreCommon.Communications.GetCurrentOperationalBehavior(); * * // check if we have completed the uturn * bool complete = operationalBehaviorType == typeof(StayInLaneBehavior); * * // default next behavior * Behavior nextBehavior = new StayInLaneBehavior(targetLane.LaneId, new ScalarSpeedCommand(CoreCommon.OperationalStopSpeed), new List<int>()); * nextBehavior.Decorators = TurnDecorators.NoDecorators; * * // check if complete * if (complete) * { * // stay in lane * List<ArbiterLaneId> aprioriLanes = new List<ArbiterLaneId>(); * aprioriLanes.Add(targetLane.LaneId); * return new Maneuver(nextBehavior, new StayInLaneState(targetLane), null, null, aprioriLanes, true); * } * // otherwise keep same * else * { * // set abort behavior * ((StayInLaneBehavior)nextBehavior).SpeedCommand = new ScalarSpeedCommand(0.0); * * // maneuver * return new Maneuver(uts.DefaultBehavior, uts, nextBehavior, new StayInLaneState(targetLane)); * } * } * #endregion*/ #endregion #region Unknown // unknown state else { throw new Exception("Unknown Travel State type: planningState: " + planningState.ToString() + "\n with type: " + planningState.GetType().ToString()); } #endregion }
/// <summary> /// Plans what maneuer we should take next /// </summary> /// <param name="planningState"></param> /// <param name="navigationalPlan"></param> /// <param name="vehicleState"></param> /// <param name="vehicles"></param> /// <param name="obstacles"></param> /// <param name="blockage"></param> /// <returns></returns> public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages) { #region Waiting At Intersection Exit if (planningState is WaitingAtIntersectionExitState) { // state WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)planningState; // get intersection plan IntersectionPlan ip = (IntersectionPlan)navigationalPlan; // nullify turn reasoning this.TurnReasoning = null; #region Intersection Monitor Updates // check correct intersection monitor if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(waies.exitWaypoint.AreaSubtypeWaypointId) && (IntersectionTactical.IntersectionMonitor == null || !IntersectionTactical.IntersectionMonitor.OurMonitor.Waypoint.Equals(waies.exitWaypoint))) { // create new intersection monitor IntersectionTactical.IntersectionMonitor = new IntersectionMonitor( waies.exitWaypoint, CoreCommon.RoadNetwork.IntersectionLookup[waies.exitWaypoint.AreaSubtypeWaypointId], vehicleState, ip.BestOption); } // update if exists if (IntersectionTactical.IntersectionMonitor != null) { // update monitor IntersectionTactical.IntersectionMonitor.Update(vehicleState); // print current ArbiterOutput.Output(IntersectionTactical.IntersectionMonitor.IntersectionStateString()); } #endregion #region Desired Behavior // get best option from previously saved IConnectAreaWaypoints icaw = null; if (waies.desired != null) { ArbiterInterconnect tmpInterconnect = waies.desired; if (waies.desired.InitialGeneric is ArbiterWaypoint) { ArbiterWaypoint init = (ArbiterWaypoint)waies.desired.InitialGeneric; if (init.NextPartition != null && init.NextPartition.Final.Equals(tmpInterconnect.FinalGeneric)) { icaw = init.NextPartition; } else { icaw = waies.desired; } } else { icaw = waies.desired; } } else { icaw = ip.BestOption; waies.desired = icaw.ToInterconnect; } #endregion #region Turn Feasibility Reasoning // check uturn if (waies.desired.TurnDirection == ArbiterTurnDirection.UTurn) { waies.turnTestState = TurnTestState.Completed; } // check already determined feasible if (waies.turnTestState == TurnTestState.Unknown || waies.turnTestState == TurnTestState.Failed) { #region Determine Behavior to Accomplish Turn // get default turn behavior TurnBehavior testTurnBehavior = this.DefaultTurnBehavior(icaw); // set saudi decorator if (waies.saudi != SAUDILevel.None) { testTurnBehavior.Decorators.Add(new ShutUpAndDoItDecorator(waies.saudi)); } // set to ignore all vehicles testTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 }); #endregion #region Check Turn Feasible // check if we have completed CompletionReport turnCompletionReport; bool completedTest = CoreCommon.Communications.TestExecute(testTurnBehavior, out turnCompletionReport); //CoreCommon.Communications.AsynchronousTestHasCompleted(testTurnBehavior, out turnCompletionReport, true); // if we have completed the test if (completedTest || ((TrajectoryBlockedReport)turnCompletionReport).BlockageType != BlockageType.Dynamic) { #region Can Complete // check success if (turnCompletionReport.Result == CompletionResult.Success) { // set completion state of the turn waies.turnTestState = TurnTestState.Completed; } #endregion #region No Saudi Level, Found Initial Blockage // otherwise we cannot do the turn, check if saudi is still none else if (waies.saudi == SAUDILevel.None) { // notify ArbiterOutput.Output("Increased Saudi Level of Turn to L1"); // up the saudi level, set as turn failed and no other option waies.saudi = SAUDILevel.L1; waies.turnTestState = TurnTestState.Failed; } #endregion #region Already at L1 Saudi else if (waies.saudi == SAUDILevel.L1) { // notify ArbiterOutput.Output("Turn with Saudi L1 Level failed"); // get an intersection plan without this interconnect IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect( waies.exitWaypoint, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], waies.desired); // check that the plan exists if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) && testPlan.BestRouteTime < double.MaxValue - 1.0) { // get the desired interconnect ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect; #region Check that the reset interconnect is feasible // test the reset interconnect TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset); // set to ignore all vehicles testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 }); // check if we have completed CompletionReport turnResetCompletionReport; bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport); // check to see if this is feasible if (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95) { // notify ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to current interconnect: " + waies.desired.ToString()); // set the interconnect as being blocked CoreCommon.Navigation.AddInterconnectBlockage(waies.desired); // reset all waies.desired = reset; waies.turnTestState = TurnTestState.Completed; waies.saudi = SAUDILevel.None; waies.useTurnBounds = true; IntersectionMonitor.ResetDesired(reset); } #endregion #region No Lane Bounds // otherwise try without lane bounds else { // notify ArbiterOutput.Output("Had to fallout to using no turn bounds"); // up the saudi level, set as turn failed and no other option waies.saudi = SAUDILevel.L1; waies.turnTestState = TurnTestState.Completed; waies.useTurnBounds = false; } #endregion } #region No Lane Bounds // otherwise try without lane bounds else { // up the saudi level, set as turn failed and no other option waies.saudi = SAUDILevel.L1; waies.turnTestState = TurnTestState.Unknown; waies.useTurnBounds = false; } #endregion } #endregion // want to reset ourselves return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } #endregion } #endregion #region Entry Monitor Blocked // checks the entry monitor vehicle for failure if (IntersectionMonitor != null && IntersectionMonitor.EntryAreaMonitor != null && IntersectionMonitor.EntryAreaMonitor.Vehicle != null && IntersectionMonitor.EntryAreaMonitor.Failed) { ArbiterOutput.Output("Entry area blocked"); // get an intersection plan without this interconnect IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect( waies.exitWaypoint, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], waies.desired, true); // check that the plan exists if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) && testPlan.BestRouteTime < double.MaxValue - 1.0) { // get the desired interconnect ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect; #region Check that the reset interconnect is feasible // test the reset interconnect TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset); // set to ignore all vehicles testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 }); // check if we have completed CompletionReport turnResetCompletionReport; bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport); // check to see if this is feasible if (reset.TurnDirection == ArbiterTurnDirection.UTurn || (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95)) { // notify ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to all possible turns into final"); // set all the interconnects to the final as being blocked if (((ITraversableWaypoint)waies.desired.FinalGeneric).IsEntry) { foreach (ArbiterInterconnect toBlock in ((ITraversableWaypoint)waies.desired.FinalGeneric).Entries) { CoreCommon.Navigation.AddInterconnectBlockage(toBlock); } } // check if exists previous partition to block if (waies.desired.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint finWaypoint = (ArbiterWaypoint)waies.desired.FinalGeneric; if (finWaypoint.PreviousPartition != null) { CoreCommon.Navigation.AddBlockage(finWaypoint.PreviousPartition, finWaypoint.Position, false); } } // reset all waies.desired = reset; waies.turnTestState = TurnTestState.Completed; waies.saudi = SAUDILevel.None; waies.useTurnBounds = true; IntersectionMonitor.ResetDesired(reset); // want to reset ourselves return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } #endregion } else { ArbiterOutput.Output("Entry area blocked, but no otehr valid route found"); } } #endregion // check if can traverse if (IntersectionTactical.IntersectionMonitor == null || IntersectionTactical.IntersectionMonitor.CanTraverse(icaw, vehicleState)) { #region If can traverse the intersection // quick check not interconnect if (!(icaw is ArbiterInterconnect)) { icaw = icaw.ToInterconnect; } // get interconnect ArbiterInterconnect ai = (ArbiterInterconnect)icaw; // clear all old completion reports CoreCommon.Communications.ClearCompletionReports(); // check if uturn if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection == ArbiterTurnDirection.UTurn) { // go into turn List <ArbiterLane> involvedLanes = new List <ArbiterLane>(); involvedLanes.Add(((ArbiterWaypoint)ai.InitialGeneric).Lane); involvedLanes.Add(((ArbiterWaypoint)ai.FinalGeneric).Lane); uTurnState nextState = new uTurnState(((ArbiterWaypoint)ai.FinalGeneric).Lane, IntersectionToolkit.uTurnBounds(vehicleState, involvedLanes)); nextState.Interconnect = ai; // hold brake Behavior b = new HoldBrakeBehavior(); // return maneuver return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } else { if (ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric; // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL); // go into turn IState nextState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds); // hold brake Behavior b = new HoldBrakeBehavior(); // return maneuver return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } else { // final perimeter waypoint ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric; // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL); // go into turn IState nextState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds); // hold brake Behavior b = new HoldBrakeBehavior(); // return maneuver return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } } #endregion } // otherwise need to wait else { IState next = waies; return(new Maneuver(new HoldBrakeBehavior(), next, next.DefaultStateDecorators, vehicleState.Timestamp)); } } #endregion #region Stopping At Exit else if (planningState is StoppingAtExitState) { // cast to exit stopping StoppingAtExitState saes = (StoppingAtExitState)planningState; saes.currentPosition = vehicleState.Front; // get intersection plan IntersectionPlan ip = (IntersectionPlan)navigationalPlan; // if has an intersection if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId)) { // create new intersection monitor IntersectionTactical.IntersectionMonitor = new IntersectionMonitor( saes.waypoint, CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId], vehicleState, ip.BestOption); // update it IntersectionTactical.IntersectionMonitor.Update(vehicleState); } else { IntersectionTactical.IntersectionMonitor = null; } // otherwise update the stop parameters saes.currentPosition = vehicleState.Front; Behavior b = saes.Resume(vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); return(new Maneuver(b, saes, saes.DefaultStateDecorators, vehicleState.Timestamp)); } #endregion #region In uTurn else if (planningState is uTurnState) { // get state uTurnState uts = (uTurnState)planningState; // check if in other lane if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType())) { // quick check if (uts.Interconnect != null && uts.Interconnect.FinalGeneric is ArbiterWaypoint) { // get the closest partition to the new lane ArbiterLanePartition alpClose = uts.TargetLane.GetClosestPartition(vehicleState.Front); // waypoints ArbiterWaypoint partitionInitial = alpClose.Initial; ArbiterWaypoint uturnEnd = (ArbiterWaypoint)uts.Interconnect.FinalGeneric; // check initial past end if (partitionInitial.WaypointId.Number > uturnEnd.WaypointId.Number) { // get waypoints inclusive List <ArbiterWaypoint> inclusive = uts.TargetLane.WaypointsInclusive(uturnEnd, partitionInitial); bool found = false; // loop through foreach (ArbiterWaypoint aw in inclusive) { if (!found && aw.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId)) { // notiofy ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as passed over in uturn"); // remove CoreCommon.Mission.MissionCheckpoints.Dequeue(); // set found found = true; } } } // default check else if (uts.Interconnect.FinalGeneric.Equals(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId])) { // notiofy ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as end of uturn"); // remove CoreCommon.Mission.MissionCheckpoints.Dequeue(); } } // check if the uturn is for a blockage else if (uts.Interconnect == null) { // get final lane ArbiterLane targetLane = uts.TargetLane; // check has opposing if (targetLane.Way.Segment.Lanes.Count > 1) { // check the final checkpoint is in our lane if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.AreaSubtypeId.Equals(targetLane.LaneId)) { // check that the final checkpoint is within the uturn polygon if (uts.Polygon != null && uts.Polygon.IsInside(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position)) { // remove the checkpoint ArbiterOutput.Output("Found checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + " inside blockage uturn area, dequeuing"); CoreCommon.Mission.MissionCheckpoints.Dequeue(); } } } } // stay in target lane IState nextState = new StayInLaneState(uts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState); Behavior b = new StayInLaneBehavior(uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0), new List <int>(), uts.TargetLane.LanePath(), uts.TargetLane.Width, uts.TargetLane.NumberOfLanesLeft(vehicleState.Front, true), uts.TargetLane.NumberOfLanesRight(vehicleState.Front, true)); return(new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } // otherwise continue uturn else { // get polygon Polygon p = uts.Polygon; // add polygon to observable CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(p, ArbiterInformationDisplayObjectType.uTurnPolygon)); // check the type of uturn if (!uts.singleLaneUturn) { // get ending path LinePath endingPath = uts.TargetLane.LanePath(); // next state is current IState nextState = uts; // behavior Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0)); // maneuver return(new Maneuver(b, nextState, null, vehicleState.Timestamp)); } else { // get ending path LinePath endingPath = uts.TargetLane.LanePath().Clone(); endingPath = endingPath.ShiftLateral(-2.0); //uts.TargetLane.Width); // add polygon to observable CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(endingPath, ArbiterInformationDisplayObjectType.leftBound)); // next state is current IState nextState = uts; // behavior Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0)); // maneuver return(new Maneuver(b, nextState, null, vehicleState.Timestamp)); } } } #endregion #region In Turn else if (planningState is TurnState) { // get state TurnState ts = (TurnState)planningState; // add bounds to observable if (ts.LeftBound != null && ts.RightBound != null) { CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.LeftBound, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.RightBound, ArbiterInformationDisplayObjectType.rightBound)); } // create current turn reasoning if (this.TurnReasoning == null) { this.TurnReasoning = new TurnReasoning(ts.Interconnect, IntersectionTactical.IntersectionMonitor != null ? IntersectionTactical.IntersectionMonitor.EntryAreaMonitor : null); } // get primary maneuver Maneuver primary = this.TurnReasoning.PrimaryManeuver(vehicleState, blockages, ts); // get secondary maneuver Maneuver?secondary = this.TurnReasoning.SecondaryManeuver(vehicleState, (IntersectionPlan)navigationalPlan); // return the manevuer return(secondary.HasValue ? secondary.Value : primary); } #endregion #region Itnersection Startup else if (planningState is IntersectionStartupState) { // state and plan IntersectionStartupState iss = (IntersectionStartupState)planningState; IntersectionStartupPlan isp = (IntersectionStartupPlan)navigationalPlan; // initial path LinePath vehiclePath = new LinePath(new Coordinates[] { vehicleState.Rear, vehicleState.Front }); List <ITraversableWaypoint> feasibleEntries = new List <ITraversableWaypoint>(); // vehicle polygon forward of us Polygon vehicleForward = vehicleState.ForwardPolygon; // best waypoint ITraversableWaypoint best = null; double bestCost = Double.MaxValue; // given feasible choose best, no feasible choose random if (feasibleEntries.Count == 0) { foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values) { if (vehicleForward.IsInside(itw.Position)) { feasibleEntries.Add(itw); } } if (feasibleEntries.Count == 0) { foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values) { feasibleEntries.Add(itw); } } } // get best foreach (ITraversableWaypoint itw in feasibleEntries) { if (isp.NodeTimeCosts.ContainsKey(itw)) { KeyValuePair <ITraversableWaypoint, double> lookup = new KeyValuePair <ITraversableWaypoint, double>(itw, isp.NodeTimeCosts[itw]); if (best == null || lookup.Value < bestCost) { best = lookup.Key; bestCost = lookup.Value; } } } // get something going to this waypoint ArbiterInterconnect interconnect = null; if (best.IsEntry) { ArbiterInterconnect closest = null; double closestDistance = double.MaxValue; foreach (ArbiterInterconnect ai in best.Entries) { double dist = ai.InterconnectPath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); if (closest == null || dist < closestDistance) { closest = ai; closestDistance = dist; } } interconnect = closest; } else if (best is ArbiterWaypoint && ((ArbiterWaypoint)best).PreviousPartition != null) { interconnect = ((ArbiterWaypoint)best).PreviousPartition.ToInterconnect; } // get state if (best is ArbiterWaypoint) { // go to this turn state LinePath finalPath; LineList leftBound; LineList rightBound; IntersectionToolkit.TurnInfo((ArbiterWaypoint)best, out finalPath, out leftBound, out rightBound); return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, ((ArbiterWaypoint)best).Lane, finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } else { // go to this turn state LinePath finalPath; LineList leftBound; LineList rightBound; IntersectionToolkit.ZoneTurnInfo(interconnect, (ArbiterPerimeterWaypoint)best, out finalPath, out leftBound, out rightBound); return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, null, finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } } #endregion #region Unknown else { throw new Exception("Unknown planning state in intersection tactical plan: " + planningState.ToString()); } #endregion }