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> /// 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> /// Simple right lane change /// </summary> /// <param name="arbiterLane"></param> /// <param name="closestGood"></param> /// <param name="vehicleState"></param> /// <param name="blockages"></param> /// <returns></returns> private Maneuver?DefaultRightToGoodChange(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List <ITacticalBlockage> blockages, Coordinates xUpper, bool forcedOpposing) { bool adjRearClear = this.rightLateralReasoning.AdjacentAndRearClear(vehicleState); if (adjRearClear) { // notify ArbiterOutput.Output("Opposing Secondary: Adjacent and Rear Clear"); LaneChangeParameters lcp = new LaneChangeParameters( true, true, arbiterLane, true, closestGood, false, false, null, 3 * TahoeParams.VL, null, TurnDecorators.RightTurnDecorator, this.OpposingForwardMonitor.CurrentParamters.Value, xUpper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); lcp.ForcedOpposing = forcedOpposing; ChangeLanesState cls = new ChangeLanesState(lcp); return(new Maneuver(this.OpposingForwardMonitor.CurrentParamters.Value.Behavior, cls, lcp.Decorators, vehicleState.Timestamp)); } else { if (this.rightLateralReasoning.AdjacentVehicle != null && !this.rightLateralReasoning.AdjacentVehicle.IsStopped && this.rightLateralReasoning.AdjacentVehicle.Speed > CoreCommon.Communications.GetVehicleSpeed().Value - 2.0) { // notify ArbiterOutput.Output("Opposing Secondary: Adjacent and Rear NOT Clear, ADJACENT NOT STOPPED, vAdj: " + this.rightLateralReasoning.AdjacentVehicle.Speed.ToString("f1")); TravelingParameters tp = this.OpposingForwardMonitor.CurrentParamters.Value; if (tp.Behavior is StayInLaneBehavior) { StayInLaneBehavior silb = (StayInLaneBehavior)tp.Behavior; silb.SpeedCommand = new ScalarSpeedCommand(0.0); } return(new Maneuver(tp.Behavior, tp.NextState, tp.Decorators, vehicleState.Timestamp)); } else if (this.rightLateralReasoning.AdjacentVehicle != null && this.OpposingForwardMonitor.NaviationParameters.DistanceToGo < TahoeParams.VL * 4.0) { // notify ArbiterOutput.Output("Opposing Secondary: Adjacent and Rear NOT Clear, ADJACENT FILLED, too close to nav stop"); TravelingParameters tp = this.OpposingForwardMonitor.CurrentParamters.Value; if (tp.Behavior is StayInLaneBehavior) { StayInLaneBehavior silb = (StayInLaneBehavior)tp.Behavior; silb.SpeedCommand = new ScalarSpeedCommand(0.0); } return(new Maneuver(tp.Behavior, tp.NextState, tp.Decorators, vehicleState.Timestamp)); } else if (this.rightLateralReasoning.AdjacentVehicle == null && !adjRearClear) { // notify ArbiterOutput.Output("Opposing Secondary: REAR NOT Clear, WAITING"); TravelingParameters tp = this.OpposingForwardMonitor.CurrentParamters.Value; if (tp.Behavior is StayInLaneBehavior) { StayInLaneBehavior silb = (StayInLaneBehavior)tp.Behavior; silb.SpeedCommand = new ScalarSpeedCommand(0.0); } return(new Maneuver(tp.Behavior, tp.NextState, tp.Decorators, vehicleState.Timestamp)); } else { return(null); } } }
/// <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 }