/// <summary> /// Constructor /// </summary> /// <param name="leftLateral"></param> /// <param name="rightLateral"></param> public OpposingReasoning(ILateralReasoning leftLateral, ILateralReasoning rightLateral, ArbiterLane lane) { this.Lane = lane; this.leftLateralReasoning = leftLateral; this.rightLateralReasoning = rightLateral; this.OpposingForwardMonitor = new OpposingForwardQuadrantMonitor(); }
/// <summary> /// Constructor /// </summary> /// <param name="leftLateral"></param> /// <param name="rightLateral"></param> public OpposingReasoning(ILateralReasoning leftLateral, ILateralReasoning rightLateral, ArbiterLane lane) { this.Lane = lane; this.leftLateralReasoning = leftLateral; this.rightLateralReasoning = rightLateral; this.OpposingForwardMonitor = new OpposingForwardQuadrantMonitor(); }
/// <summary> /// Constructor /// </summary> /// <param name="leftLateral"></param> /// <param name="rightLateral"></param> public ForwardReasoning(ILateralReasoning leftLateral, ILateralReasoning rightLateral, IFQMPlanable lane) { this.Lane = lane; this.leftLateralReasoning = leftLateral; this.rightLateralReasoning = rightLateral; this.ForwardMonitor = new ForwardQuadrantMonitor(); this.blockageTimer = new Stopwatch(); if (lane is ArbiterLane) { this.RearMonitor = new RearQuadrantMonitor((ArbiterLane)lane, SideObstacleSide.Driver); } else { this.RearMonitor = new RearQuadrantMonitor(((SupraLane)lane).Initial, SideObstacleSide.Driver); } }
/// <summary> /// Behavior we would like to do other than going straight /// </summary> /// <param name="arbiterLane"></param> /// <param name="vehicleState"></param> /// <param name="p"></param> /// <param name="blockages"></param> /// <returns></returns> /// <remarks>tries to go right, if not goest left if needs /// to if forward vehicle ahead and we're stopped because of them</remarks> public Maneuver?SecondaryManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List <ITacticalBlockage> blockages, LaneChangeParameters?entryParameters) { // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage) { // 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)); } // check dist needed to complete double neededDistance = (Math.Abs(arbiterLane.LaneId.Number - closestGood.LaneId.Number) * 1.5 * TahoeParams.VL) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // get upper bound LinePath.PointOnPath xFront = arbiterLane.LanePath().GetClosestPoint(vehicleState.Front); Coordinates xUpper = arbiterLane.LanePath().AdvancePoint(xFront, -neededDistance).Location; if (entryParameters.HasValue) { // check if we should get back, keep speed nice n' lowi fpassing failed if (entryParameters.Value.Reason == LaneChangeReason.FailedForwardVehicle) { double xToReturn = arbiterLane.DistanceBetween(entryParameters.Value.DefaultReturnLowerBound, vehicleState.Front); if (xToReturn >= 0.0) { ArbiterOutput.Output("Distance until must return to lane: " + xToReturn); } else { ArbiterOutput.Output("Can return to lane from arbitrary upper bound: " + xToReturn); } // check can return if (xToReturn < 0) { // check if right lateral exists exactly here if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning good and exists exactly here"); return(this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true)); } else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here"); if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood)) { this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger); } if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState)) { ILateralReasoning tmpReasoning = this.rightLateralReasoning; this.rightLateralReasoning = this.secondaryLateralReasoning; Maneuver?tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true); this.rightLateralReasoning = tmpReasoning; return(tmp); } else { ArbiterOutput.Output("Cosest good lane does not exist here??"); return(null); } } else { ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() + ", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString()); return(null); } } else { return(null); } } } // lane change info LaneChangeInformation lci = new LaneChangeInformation(LaneChangeReason.Navigation, null); // notify ArbiterOutput.Output("In Opposing with no Previous state knowledge, attempting to return"); // check if right lateral exists exactly here if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning good and exists exactly here"); return(this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false)); } else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here"); if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood)) { this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger); } if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState)) { ILateralReasoning tmpReasoning = this.rightLateralReasoning; this.rightLateralReasoning = this.secondaryLateralReasoning; Maneuver?tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false); this.rightLateralReasoning = tmpReasoning; return(tmp); } else { ArbiterOutput.Output("Cosest good lane does not exist here??"); return(null); } } else { ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() + ", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString()); return(null); } }
/// <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> /// Makes use of parameterizations made from the initial forward maneuver plan /// </summary> /// <param name="arbiterLane"></param> /// <param name="vehicleState"></param> /// <param name="roadPlan"></param> /// <param name="blockages"></param> /// <param name="ignorable"></param> /// <returns></returns> public Maneuver? SecondaryManeuver(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask) { // check if we might be able to pass here bool validArea = arbiterLane is ArbiterLane || (((SupraLane)arbiterLane).ClosestComponent(vehicleState.Front) == SLComponentType.Initial); ArbiterLane ourForwardLane = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial; // check if the forward vehicle exists and we're in a valid area if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && validArea) { // check if we should pass the vehicle ahead LaneChangeInformation lci; bool sp = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lci); // make sure we should do something before processing extras if(sp) { // available parameterizations for the lane change List<LaneChangeParameters> changeParams = new List<LaneChangeParameters>(); // get lane ArbiterLane al = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial; // get the location we need to return by Coordinates absoluteUpperBound = arbiterLane is ArbiterLane ? roadPlan.LanePlans[al.LaneId].laneWaypointOfInterest.PointOfInterest.Position : ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position; #region Failed Forward // if failed, parameterize ourselved if we're following them if (lci.Reason == LaneChangeReason.FailedForwardVehicle && this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle) { // notify ArbiterOutput.Output("Failed Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.VehicleId.ToString()); // get traveling params from FQM to make sure we stopped for vehicle, behind vehicle double v = CoreCommon.Communications.GetVehicleSpeed().Value; TravelingParameters fqmParams = this.ForwardMonitor.CurrentParameters; double d = this.ForwardMonitor.ForwardVehicle.DistanceToVehicle(arbiterLane, vehicleState.Front); Coordinates departUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d - 3.0).Location; // check stopped behing failed forward try { if (fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // check for checkpoint within 4VL of front of failed vehicle ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek(); if (acCurrecnt.WaypointId.AreaSubtypeId.Equals(al.LaneId)) { // check distance ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId]; double cpDistacne = Lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position); if (cpDistacne < d || cpDistacne - d < TahoeParams.VL * 4.5) { ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " as failed vehicle over it"); CoreCommon.Mission.MissionCheckpoints.Dequeue(); return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } } }catch (Exception) { } #region Right Lateral Reasoning Forwards // check right lateral reasoning for existence, if so parametrize if (rightLateralReasoning.Exists && fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // get lane ArbiterLane lane = al; // determine failed vehicle lane change distance params Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location; Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location; Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location; // get params for lane change LaneChangeParameters? lcp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null), lane, lane.LaneOnRight, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if exists to generate full param if (lcp.HasValue) { // get param LaneChangeParameters tp = lcp.Value; // notify ArbiterOutput.WriteToLog("Failed Forward: Right Lateral Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasable: " + tp.Feasible.ToString()); // get behavior ChangeLaneBehavior clb = new ChangeLaneBehavior( al.LaneId, al.LaneOnRight.LaneId, false, al.DistanceBetween(vehicleState.Front, departUpperBound), new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore, al.LanePath(), al.LaneOnRight.LanePath(), al.Width, al.LaneOnRight.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); tp.Behavior = clb; tp.Decorators = TurnDecorators.RightTurnDecorator; // next state ChangeLanesState cls = new ChangeLanesState(tp); tp.NextState = cls; // add parameterization to possible changeParams.Add(tp); } } #endregion #region Left Lateral Reasoning // check left lateral reasoning if(leftLateralReasoning.Exists) { #region Left Lateral Opposing // check opposing ArbiterLane closestOpposingLane = this.GetClosestOpposing(ourForwardLane, vehicleState); if(closestOpposingLane != null && (leftLateralReasoning.IsOpposing || !closestOpposingLane.Equals(leftLateralReasoning.LateralLane))) { // check room of initial bool enoughRoom = (arbiterLane is ArbiterLane && (!roadPlan.BestPlan.laneWaypointOfInterest.IsExit || ((ArbiterLane)arbiterLane).DistanceBetween(vehicleState.Front, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > TahoeParams.VL * 5.0)) || (arbiterLane is SupraLane && ((SupraLane)arbiterLane).DistanceBetween(vehicleState.Front, ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position) > TahoeParams.VL * 5.0); // check opposing enough room bool oppEnough = closestOpposingLane.DistanceBetween(closestOpposingLane.LanePath().StartPoint.Location, vehicleState.Front) > TahoeParams.VL * 5.0; // check if enough room if (enoughRoom && oppEnough) { // check if we're stopped and the current trav params were for a vehicle and we're close to the vehicle bool stoppedBehindFV = fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle; // check that we're stopped behind forward vehicle before attempting to change lanes if (stoppedBehindFV) { #region Check Segment Blockage // check need to make uturn (hack) bool waitForUTurnCooldown; BlockageTactical bt = CoreCommon.BlockageDirector; StayInLaneBehavior tmpBlockBehavior = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(2.0), new List<int>(), al.LanePath(), al.Width, 0, 0); ITacticalBlockage itbTmp = new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, TahoeParams.VL, BlockageType.Static, -1, false, tmpBlockBehavior.GetType())); Maneuver tmpBlockManeuver = bt.LaneRecoveryManeuver(al, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value, roadPlan, new BlockageRecoveryState(tmpBlockBehavior, new StayInLaneState(al, CoreCommon.CorePlanningState), new StayInLaneState(al, CoreCommon.CorePlanningState), BlockageRecoveryDEFCON.REVERSE, new EncounteredBlockageState(itbTmp, CoreCommon.CorePlanningState, BlockageRecoveryDEFCON.REVERSE, SAUDILevel.None), BlockageRecoverySTATUS.ENCOUNTERED), true, out waitForUTurnCooldown); if (!waitForUTurnCooldown && tmpBlockManeuver.PrimaryBehavior is UTurnBehavior) return tmpBlockManeuver; else if (waitForUTurnCooldown) return null; #endregion // distance to forward vehicle too small double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); double distToReverse = Math.Max(1.0, 8.0 - distToForwards); if (distToForwards < 8.0) { // notify ArbiterOutput.WriteToLog("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2")); this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver); this.RearMonitor.Update(vehicleState); if (this.RearMonitor.CurrentVehicle != null) { double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL; double distNeedClear = distToReverse + 2.0; if (distToRearVehicle < distNeedClear) { // notify ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2")); return null; } } double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL; if (distToReverse > distToLaneStart) { // notify ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2")); return null; } else { // notify ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2")); StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp); } } else { // notify ArbiterOutput.WriteToLog("Secondary: Left Lateral Opposing: Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString()); // determine failed vehicle lane change distance params Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location; Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.5)).Location; Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location; // check if enough room if (al.DistanceBetween(vehicleState.Front, defaultReturnUpperBound) >= d + TahoeParams.VL * 4.5) { // get hte closest oppoing ArbiterLane closestOpposing = this.GetClosestOpposing(al, vehicleState); // check exists if (closestOpposing != null) { // set/check secondary if (this.secondaryLeftLateralReasoning == null || !this.secondaryLeftLateralReasoning.LateralLane.Equals(closestOpposing)) this.secondaryLeftLateralReasoning = new OpposingLateralReasoning(closestOpposing, SideObstacleSide.Driver); // check the state of hte lanes next to us if (this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && this.leftLateralReasoning.ExistsExactlyHere(vehicleState)) { #region Plan // need to make sure that we wait for 3 seconds with the blinker on (resetting with pause) if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE"); // get parameterization LaneChangeParameters? tp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle), al, leftLateralReasoning.LateralLane, true, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if available and feasible if (tp.HasValue && tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE"); LaneChangeParameters lcp = tp.Value; lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior; lcp.Decorators = TurnDecorators.LeftTurnDecorator; lcp.Behavior.Decorators = lcp.Decorators; // next state ChangeLanesState cls = new ChangeLanesState(tp.Value); lcp.NextState = cls; // add parameterization to possible changeParams.Add(lcp); } // check if not available now but still feasible else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING"); // wait and blink maneuver TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters; tp2.Decorators = TurnDecorators.LeftTurnDecorator; tp2.Behavior.Decorators = tp2.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft, true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } } // otherwise timer not running or not been long enough else { // check if timer running if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning) this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start(); double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0); ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds"); // wait and blink maneuver TravelingParameters tp = this.ForwardMonitor.CurrentParameters; tp.Decorators = TurnDecorators.LeftTurnDecorator; tp.Behavior.Decorators = tp.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft, true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } #endregion } else if (!this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && !this.leftLateralReasoning.ExistsRelativelyHere(vehicleState)) { // set and notify ArbiterOutput.Output("superceeded left lateral reasoning with override for non adjacent left lateral reasoning"); ILateralReasoning tmpReasoning = this.leftLateralReasoning; this.leftLateralReasoning = this.secondaryLeftLateralReasoning; try { #region Plan // need to make sure that we wait for 3 seconds with the blinker on (resetting with pause) if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE"); // get parameterization LaneChangeParameters? tp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle), al, leftLateralReasoning.LateralLane, true, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if available and feasible if (tp.HasValue && tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE"); LaneChangeParameters lcp = tp.Value; lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior; lcp.Decorators = TurnDecorators.LeftTurnDecorator; lcp.Behavior.Decorators = TurnDecorators.LeftTurnDecorator; // next state ChangeLanesState cls = new ChangeLanesState(tp.Value); lcp.NextState = cls; // add parameterization to possible changeParams.Add(lcp); } // check if not available now but still feasible else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING"); // wait and blink maneuver TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters; tp2.Decorators = TurnDecorators.LeftTurnDecorator; tp2.Behavior.Decorators = tp2.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane, true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } } // otherwise timer not running or not been long enough else { // check if timer running if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning) this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start(); double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0); ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds"); // wait and blink maneuver TravelingParameters tp = this.ForwardMonitor.CurrentParameters; tp.Decorators = TurnDecorators.LeftTurnDecorator; tp.Behavior.Decorators = tp.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane, true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } #endregion } catch (Exception ex) { ArbiterOutput.Output("Core intelligence thread caught exception in forward reasoning secondary maneuver when non-standard adjacent left: " + ex.ToString()); } // restore this.leftLateralReasoning = tmpReasoning; } } else { // do nuttin ArbiterOutput.Output("no opposing adjacent"); } } else { // notify ArbiterOutput.Output("Secondary: LeftLatOpp: Stopped Behind FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", but not enough room to pass"); } } } else { this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop(); this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset(); // notify ArbiterOutput.Output("Secondary: Left Lateral Opposing: NOT Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString()); } } else { ArbiterOutput.Output("Secondary Opposing: enough room to pass opposing: initial: " + enoughRoom.ToString() + ", opposing: " + oppEnough.ToString()); } } #endregion #region Left Lateral Forwards // otherwise parameterize else if(fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // get lane ArbiterLane lane = al; // determine failed vehicle lane change distance params Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location; Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location; Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location; // get params for lane change LaneChangeParameters? lcp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null), lane, lane.LaneOnLeft, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if exists to generate full param if (lcp.HasValue) { // set param LaneChangeParameters tp = lcp.Value; // notify ArbiterOutput.Output("Secondary Failed Forward Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasible: " + tp.Feasible.ToString()); // get behavior ChangeLaneBehavior clb = new ChangeLaneBehavior( al.LaneId, al.LaneOnLeft.LaneId, true, al.DistanceBetween(vehicleState.Front, departUpperBound), new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore, al.LanePath(), al.LaneOnLeft.LanePath(), al.Width, al.LaneOnLeft.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); tp.Behavior = clb; tp.Decorators = TurnDecorators.LeftTurnDecorator; // next state ChangeLanesState cls = new ChangeLanesState(tp); tp.NextState = cls; // add parameterization to possible changeParams.Add(tp); } } #endregion } #endregion } #endregion #region Slow Forward // if pass, determine if should pass in terms or vehicles adjacent and in front then call lane change function for maneuver else if (lci.Reason == LaneChangeReason.SlowForwardVehicle) { // if left exists and is not opposing, parameterize if (leftLateralReasoning.Exists && !leftLateralReasoning.IsOpposing) { throw new Exception("slow forward vehicle pass not implemented yet"); } // if right exists and is not opposing, parameterize if (rightLateralReasoning.Exists && !rightLateralReasoning.IsOpposing) { throw new Exception("slow forward vehicle pass not implemented yet"); } } #endregion #region Parameterize // check params to see if any are good and available if(changeParams != null && changeParams.Count > 0) { // sort the parameterizations changeParams.Sort(); // get first LaneChangeParameters final = changeParams[0]; // notify ArbiterOutput.Output("Secondary Reasoning Final: Available: " + final.Available.ToString() + ", Feasible: " + final.Feasible.ToString()); // make sure ok if (final.Available && final.Feasible) { // return best param return new Maneuver(changeParams[0].Behavior, changeParams[0].NextState, changeParams[0].Decorators, vehicleState.Timestamp); } } #endregion } } // fallout is null return null; }
/// <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> /// Behavior we would like to do other than going straight /// </summary> /// <param name="arbiterLane"></param> /// <param name="vehicleState"></param> /// <param name="p"></param> /// <param name="blockages"></param> /// <returns></returns> /// <remarks>tries to go right, if not goest left if needs /// to if forward vehicle ahead and we're stopped because of them</remarks> public Maneuver? SecondaryManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List<ITacticalBlockage> blockages, LaneChangeParameters? entryParameters) { // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage) { // 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); } // check dist needed to complete double neededDistance = (Math.Abs(arbiterLane.LaneId.Number - closestGood.LaneId.Number) * 1.5 * TahoeParams.VL) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // get upper bound LinePath.PointOnPath xFront = arbiterLane.LanePath().GetClosestPoint(vehicleState.Front); Coordinates xUpper = arbiterLane.LanePath().AdvancePoint(xFront, -neededDistance).Location; if (entryParameters.HasValue) { // check if we should get back, keep speed nice n' lowi fpassing failed if (entryParameters.Value.Reason == LaneChangeReason.FailedForwardVehicle) { double xToReturn = arbiterLane.DistanceBetween(entryParameters.Value.DefaultReturnLowerBound, vehicleState.Front); if(xToReturn >= 0.0) ArbiterOutput.Output("Distance until must return to lane: " + xToReturn); else ArbiterOutput.Output("Can return to lane from arbitrary upper bound: " + xToReturn); // check can return if (xToReturn < 0) { // check if right lateral exists exactly here if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning good and exists exactly here"); return this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true); } else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here"); if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood)) this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger); if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState)) { ILateralReasoning tmpReasoning = this.rightLateralReasoning; this.rightLateralReasoning = this.secondaryLateralReasoning; Maneuver? tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true); this.rightLateralReasoning = tmpReasoning; return tmp; } else { ArbiterOutput.Output("Cosest good lane does not exist here??"); return null; } } else { ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() + ", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString()); return null; } } else return null; } } // lane change info LaneChangeInformation lci = new LaneChangeInformation(LaneChangeReason.Navigation, null); // notify ArbiterOutput.Output("In Opposing with no Previous state knowledge, attempting to return"); // check if right lateral exists exactly here if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning good and exists exactly here"); return this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false); } else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here"); if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood)) this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger); if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState)) { ILateralReasoning tmpReasoning = this.rightLateralReasoning; this.rightLateralReasoning = this.secondaryLateralReasoning; Maneuver? tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false); this.rightLateralReasoning = tmpReasoning; return tmp; } else { ArbiterOutput.Output("Cosest good lane does not exist here??"); return null; } } else { ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() + ", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString()); return null; } }