コード例 #1
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="recoveryBehaviorType"></param>
 /// <param name="completionState"></param>
 /// <param name="abortState"></param>
 /// <param name="defcon"></param>
 public BlockageRecoveryState(Behavior recoveryBehavior, IState completionState,
                              IState abortState, BlockageRecoveryDEFCON defcon, EncounteredBlockageState ebs, BlockageRecoverySTATUS status)
 {
     this.RecoveryBehavior = recoveryBehavior;
     this.CompletionState  = completionState;
     this.AbortState       = abortState;
     this.Defcon           = defcon;
     this.RecoveryStatus   = status;
     this.EncounteredState = ebs;
 }
コード例 #2
0
        /// <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
        }
コード例 #3
0
        /// <summary>
        /// Reverse because of a blockage
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicleSpeed"></param>
        /// <param name="defcon"></param>
        /// <param name="saudi"></param>
        /// <param name="laneOpposing"></param>
        /// <param name="currentBlockage"></param>
        /// <param name="ebs"></param>
        /// <returns></returns>
        private Maneuver LaneReverseManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
            BlockageRecoveryDEFCON defcon, SAUDILevel saudi, bool laneOpposing, ITacticalBlockage currentBlockage, EncounteredBlockageState ebs)
        {
            // distance to reverse
            double distToReverse = TahoeParams.VL * 2.0;

            // just reverse and let brian catch it
            StayInLaneBehavior reverseBehavior = new StayInLaneBehavior(
                lane.LaneId, new StopAtDistSpeedCommand(distToReverse, true),
                new List<int>(), lane.LanePath(), lane.Width,
                lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));

            // get the saudi level
            List<BehaviorDecorator> decs = new List<BehaviorDecorator>(TurnDecorators.HazardDecorator.ToArray());
            decs.Add(new ShutUpAndDoItDecorator(saudi));
            reverseBehavior.Decorators = decs;

            // state
            IState laneState = new StayInLaneState(lane, CoreCommon.CorePlanningState);
            BlockageRecoveryState brs = new BlockageRecoveryState(
                reverseBehavior, laneState, laneState, defcon, ebs, BlockageRecoverySTATUS.EXECUTING);

            // check enough room in lane to reverse
            RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor;
            if (rearMonitor == null || !rearMonitor.lane.Equals(lane))
                this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor = new RearQuadrantMonitor(lane, SideObstacleSide.Driver);

            #region Start too close to start of lane

            // check distance to the start of the lane
            double laneStartDistanceFromFront = lane.DistanceBetween(lane.WaypointList[0].Position, vehicleState.Front);
            if (laneStartDistanceFromFront < TahoeParams.VL)
            {
                // make sure we're not at the very start of the lane
                if (laneStartDistanceFromFront < 0.5)
                {
                    // output
                    ArbiterOutput.Output("Too close to the start of the lane, raising defcon");

                    // go up chain
                    return new Maneuver(new NullBehavior(),
                        new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
                        TurnDecorators.NoDecorators,
                        vehicleState.Timestamp);
                }
                // otherwise back up to the start of the lane
                else
                {
                    // output
                    ArbiterOutput.Output("Too close to the start of the lane, setting reverse distance to TP.VL");

                    // set proper distance
                    distToReverse = TahoeParams.VL;

                    // set behavior speed (no car behind us as too close to start of lane)
                    LinePath bp = vehicleState.VehicleLinePath;
                    reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
                    StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
                    return new Maneuver(silb, brs, decs, vehicleState.Timestamp);
                }
            }

            #endregion

            #region Sparse

            // check distance to the start of the lane
            if (lane.GetClosestPartition(vehicleState.Front).Type == PartitionType.Sparse)
            {
                // set behavior speed (no car behind us as too close to start of lane)
                LinePath bp = vehicleState.VehicleLinePath;
                reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true);
                StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0);
                return new Maneuver(silb, brs, decs, vehicleState.Timestamp);
            }

            #endregion

            #region Vehicle Behind us

            // update and check for vehicle
            rearMonitor.Update(vehicleState);
            if (rearMonitor.CurrentVehicle != null)
            {
                // check for not failed forward vehicle
                if (rearMonitor.CurrentVehicle.QueuingState.Queuing != QueuingState.Failed)
                {
                    // check if enough room to rear vehicle
                    double vehicleDistance = lane.DistanceBetween(rearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Rear);
                    if (vehicleDistance < distToReverse - 2.0)
                    {
                        // revised distance given vehicle
                        double revisedDistance = vehicleDistance - 2.0;

                        // check enough room
                        if (revisedDistance > TahoeParams.VL)
                        {
                            // set the updated speed command
                            reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(revisedDistance, true);
                        }
                        // not enough room
                        else
                        {
                            // output not enough room because of vehicle
                            ArbiterOutput.Output("Blockage recovery, not enough room in rear because of rear vehicle, waiting for it to clear");
                            return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }
                }
                // check failed rear vehicle, up defcon
                else
                {
                    // failed vehicle in rear, go up chain
                    return new Maneuver(new NullBehavior(),
                        new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi),
                        TurnDecorators.NoDecorators,
                        vehicleState.Timestamp);
                }
            }

            #endregion

            // all clear, return reverse maneuver, set cooldown
            BlockageHandler.SetDefaultBlockageCooldown();
            return new Maneuver(reverseBehavior, brs, decs, vehicleState.Timestamp);
        }
コード例 #4
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="recoveryBehaviorType"></param>
 /// <param name="completionState"></param>
 /// <param name="abortState"></param>
 /// <param name="defcon"></param>
 public BlockageRecoveryState(Behavior recoveryBehavior, IState completionState,
     IState abortState, BlockageRecoveryDEFCON defcon, EncounteredBlockageState ebs, BlockageRecoverySTATUS status)
 {
     this.RecoveryBehavior = recoveryBehavior;
     this.CompletionState = completionState;
     this.AbortState = abortState;
     this.Defcon = defcon;
     this.RecoveryStatus = status;
     this.EncounteredState = ebs;
 }
コード例 #5
0
        /// <summary>
        /// What to do when we complete a reverse maneuver
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicleSpeed"></param>
        /// <param name="defcon"></param>
        /// <param name="saudi"></param>
        /// <param name="laneOpposing"></param>
        /// <param name="currentBlockage"></param>
        /// <param name="ebs"></param>
        /// <returns></returns>
        private Maneuver LaneReverseCompleteManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, 
            BlockageRecoveryState brs, EncounteredBlockageState ebs, INavigationalPlan plan)
        {
            // get the lane recovery behavior
            StayInLaneBehavior testForwards = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, ebs);

            #region Test and Return

            // notify
            ArbiterOutput.Output("Attempting to test execute recovery complete behavior: " + testForwards.ToString());

            // test the test behavior
            CompletionReport testForwardsReport;
            bool canCompleteTestForwards = CoreCommon.Communications.TestExecute(testForwards, out testForwardsReport);

            // notify
            ArbiterOutput.Output("Received completion result ok: " + canCompleteTestForwards.ToString() + " with completion result: " + testForwardsReport.Result.ToString());

            // check completion ok of stop at distance
            if (canCompleteTestForwards)
            {
                // switch to the completion state
                brs.RecoveryStatus = BlockageRecoverySTATUS.EXECUTING;
                return new Maneuver(testForwards, brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
            }
            // not ok, report this blockage
            else
            {
                // return the blocked reversal maneuver
                return this.LaneReverseBlockedManeuver(lane, vehicleState, vehicleSpeed, brs, ebs, plan);
            }

            #endregion
        }
コード例 #6
0
        /// <summary>
        /// Maneuver if the reverse maneuver is blocked
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicleSpeed"></param>
        /// <param name="defcon"></param>
        /// <param name="saudi"></param>
        /// <param name="laneOpposing"></param>
        /// <param name="currentBlockage"></param>
        /// <param name="ebs"></param>
        /// <returns></returns>
        private Maneuver LaneReverseBlockedManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
            BlockageRecoveryState brs, EncounteredBlockageState ebs, INavigationalPlan plan)
        {
            // get closest partition
            ArbiterLanePartition alp = lane.GetClosestPartition(vehicleState.Front);

            // check type
            if (alp.Type == PartitionType.Sparse)
            {
                #region Get Recovery Behavior

                // get the recovery behavior
                StayInLaneBehavior testForwards = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, ebs);

                // up the saudi level
                SAUDILevel sl = brs.EncounteredState.Saudi < SAUDILevel.L2 ? brs.EncounteredState.Saudi++ : SAUDILevel.L2;
                testForwards.Decorators = new List<BehaviorDecorator>(new BehaviorDecorator[] { new ShutUpAndDoItDecorator(sl) });

                // return the behavior
                brs.EncounteredState.Saudi = sl;
                brs.RecoveryStatus = BlockageRecoverySTATUS.EXECUTING;
                return new Maneuver(testForwards, brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);

                #endregion
            }
            else
            {
                return new Maneuver(new NullBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
            }
        }
コード例 #7
0
        /// <summary>
        /// Get the default recovery behavior
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicleSpeed"></param>
        /// <param name="brs"></param>
        /// <param name="ebs"></param>
        /// <returns></returns>
        private StayInLaneBehavior LaneRecoveryBehavior(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, INavigationalPlan plan,
            BlockageRecoveryState brs, EncounteredBlockageState ebs)
        {
            #region Get the Recovery Behavior

            // set the default distance to go forwards
            double distanceForwards = TahoeParams.VL * 3.0;

            // check distance to next lane point
            this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.Primary(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>() , new List<ArbiterWaypoint>(), false);

            // get navigation distance to go
            double navDistanceToGo = this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.NavigationParameters.DistanceToGo;
            distanceForwards = navDistanceToGo < distanceForwards ? navDistanceToGo : distanceForwards;

            // check if there is a forward vehicle we should follow normally
            if (this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker)
            {
                // fv distance
                double fvDistance = this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation;

                // check distance forwards to vehicle
                if (fvDistance < distanceForwards)
                {
                    // distance modification
                    distanceForwards = fvDistance > 2.0 ? fvDistance - 2.0 : fvDistance;
                }
            }

            // test behavior
            StayInLaneBehavior testForwards = new StayInLaneBehavior(
                lane.LaneId, new StopAtDistSpeedCommand(distanceForwards), new List<int>(), lane.LanePath(), lane.Width,
                lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));

            // set the specifiec saudi level
            testForwards.Decorators = new List<BehaviorDecorator>(new BehaviorDecorator[] { new ShutUpAndDoItDecorator(brs.EncounteredState.Saudi) });

            // return the test
            return testForwards;

            #endregion
        }
コード例 #8
0
        /// <summary>
        /// Plans the next maneuver
        /// </summary>
        /// <param name="roads"></param>
        /// <param name="mission"></param>
        /// <param name="vehicleState"></param>
        /// <param name="CoreCommon.CorePlanningState"></param>
        /// <param name="observedVehicles"></param>
        /// <param name="observedObstacles"></param>
        /// <param name="coreState"></param>
        /// <param name="carMode"></param>
        /// <returns></returns>
        public Maneuver Plan(VehicleState vehicleState, double vehicleSpeed,
            SceneEstimatorTrackedClusterCollection observedVehicles, SceneEstimatorUntrackedClusterCollection observedObstacles,
            CarMode carMode, INavigableNode goal)
        {
            // set blockages
            List<ITacticalBlockage> blockages = this.blockageHandler.DetermineBlockages(CoreCommon.CorePlanningState);

            #region Travel State

            if (CoreCommon.CorePlanningState is TravelState)
            {
                #region Stay in Lane State

                if (CoreCommon.CorePlanningState is StayInLaneState)
                {
                    // get lane state
                    StayInLaneState sils = (StayInLaneState)CoreCommon.CorePlanningState;

                    #region Blockages

                    // check blockages
                    if (blockages != null && blockages.Count > 0 && blockages[0] is LaneBlockage)
                    {
                        // create the blockage state
                        EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                        // check not from a dynamicly moving vehicle
                        if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
                        {
                            // go to a blockage handling tactical
                            return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            ArbiterOutput.Output("Lane blockage reported for moving vehicle, ignoring");
                    }

                    #endregion

                    // update the total time ignorable have been seen
                    sils.UpdateIgnoreList();

                    // nav plan to find poi
                    RoadPlan rp = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);

                    // check for unreachable route
                    if (rp.BestPlan.laneWaypointOfInterest.BestRoute != null &&
                        rp.BestPlan.laneWaypointOfInterest.BestRoute.Count == 0 &&
                        rp.BestPlan.laneWaypointOfInterest.RouteTime >= Double.MaxValue - 1.0)
                    {
                        ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
                        CoreCommon.Mission.MissionCheckpoints.Dequeue();
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else if (rp.BestPlan.laneWaypointOfInterest.TimeCostToPoint >= Double.MaxValue - 1.0)
                    {
                        ArbiterOutput.Output("Best Lane Waypoint of Interest is END OF LANE WITH NO INTERCONNECTS, LEADING NOWHERE");
                        ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
                        CoreCommon.Mission.MissionCheckpoints.Dequeue();
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }

                    #region Check Supra Lane Availability

                    // if the poi is at the end of this lane, is not stop, leads to another lane, and has no overlapping lanes
                    // or if the poi's best exit is an exit in this lane, is not a stop, has no overlapping lanes and leads to another lane
                    // create supralane

                    // check if navigation is corrent in saying we want to continue on the current lane and we're far enough along the lane, 30m for now
                    if(rp.BestPlan.Lane.Equals(sils.Lane.LaneId))
                    {
                        // get navigation poi
                        DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest;

                        // check that the poi is not stop and is not the current checkpoint
                        if(!dpoi.PointOfInterest.IsStop && !(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(dpoi.PointOfInterest.WaypointId)))
                        {
                            // get the best exit or the poi
                            ArbiterInterconnect ai = dpoi.BestExit;

                            // check if exit goes into a lane and not a uturn
                            if(ai != null && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection != ArbiterTurnDirection.UTurn)
                            {
                                // final lane or navigation poi interconnect
                                ArbiterLane al = ((ArbiterWaypoint)ai.FinalGeneric).Lane;

                                // check not same lane
                                if (!al.Equals(sils.Lane))
                                {
                                    // check if enough room to start
                                    bool enoughRoom = !sils.Lane.Equals(al) || sils.Lane.LanePath(sils.Lane.WaypointList[0].Position, vehicleState.Front).PathLength > 30;
                                    if (enoughRoom)
                                    {
                                        // try to get intersection associated with the exit
                                        ArbiterIntersection aInter = CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(dpoi.PointOfInterest.WaypointId) ?
                                            CoreCommon.RoadNetwork.IntersectionLookup[dpoi.PointOfInterest.WaypointId] : null;

                                        // check no intersection or no overlapping lanes
                                        if (aInter == null || !aInter.PriorityLanes.ContainsKey(ai) || aInter.PriorityLanes[ai].Count == 0)
                                        {
                                            // create the supra lane
                                            SupraLane sl = new SupraLane(sils.Lane, ai, al);

                                            // switch to the supra lane state
                                            StayInSupraLaneState sisls = new StayInSupraLaneState(sl, CoreCommon.CorePlanningState);
                                            sisls.UpdateState(vehicleState.Front);

                                            // set
                                            return new Maneuver(new NullBehavior(), sisls, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                                        }
                                    }
                                }
                            }
                        }
                    }

                    #endregion

                    // plan final tactical maneuver
                    Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Stay in Supra Lane State

                else if (CoreCommon.CorePlanningState is StayInSupraLaneState)
                {
                    // state
                    StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;

                    #region Blockages

                    // check blockages
                    if (blockages != null && blockages.Count > 0 && blockages[0] is LaneBlockage)
                    {
                        // create the blockage state
                        EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                        // check not from a dynamicly moving vehicle
                        if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
                        {
                            // go to a blockage handling tactical
                            return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            ArbiterOutput.Output("Lane blockage reported for moving vehicle, ignoring");
                    }

                    #endregion

                    // check if we are in the final lane
                    if (sisls.Lane.ClosestComponent(vehicleState.Position) == SLComponentType.Final)
                    {
                        // go to stay in lane
                        return new Maneuver(new NullBehavior(), new StayInLaneState(sisls.Lane.Final, sisls), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }

                    // update ignorable
                    sisls.UpdateIgnoreList();

                    // nav plan to find points
                    RoadPlan rp = navigation.PlanNavigableArea(sisls.Lane, vehicleState.Position, goal, sisls.WaypointsToIgnore);

                    // check for unreachable route
                    if (rp.BestPlan.laneWaypointOfInterest.BestRoute != null &&
                        rp.BestPlan.laneWaypointOfInterest.BestRoute.Count == 0 &&
                        rp.BestPlan.laneWaypointOfInterest.RouteTime >= Double.MaxValue - 1.0)
                    {
                        ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
                        CoreCommon.Mission.MissionCheckpoints.Dequeue();
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }

                    // plan
                    Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // update current state
                    sisls.UpdateState(vehicleState.Front);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Stopping At Stop State

                else if (CoreCommon.CorePlanningState is StoppingAtStopState)
                {
                    // get state
                    StoppingAtStopState sass = (StoppingAtStopState)CoreCommon.CorePlanningState;

                    // check to see if we're stopped
                    // check if in other lane
                    if (CoreCommon.Communications.HasCompleted((new StayInLaneBehavior(null, null, null)).GetType()))
                    {
                        // update intersection monitor
                        if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(sass.waypoint.AreaSubtypeWaypointId))
                        {
                            // nav plan
                            IntersectionPlan ip = navigation.PlanIntersection(sass.waypoint, goal);

                            // update intersection monitor
                            this.tactical.Update(observedVehicles, vehicleState);
                            IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
                                sass.waypoint,
                                CoreCommon.RoadNetwork.IntersectionLookup[sass.waypoint.AreaSubtypeWaypointId],
                                vehicleState, ip.BestOption);
                        }
                        else
                        {
                            IntersectionTactical.IntersectionMonitor = null;
                        }

                        // check if we've hit goal if stop is cp
                        if (sass.waypoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
                        {
                            ArbiterOutput.Output("Stopped at current goal: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + ", Removing");
                            CoreCommon.Mission.MissionCheckpoints.Dequeue();

                            if (CoreCommon.Mission.MissionCheckpoints.Count == 0)
                            {
                                return new Maneuver(new HoldBrakeBehavior(), new NoGoalsLeftState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                            }
                        }

                        // move to the intersection
                        IState next = new WaitingAtIntersectionExitState(sass.waypoint, sass.turnDirection, new IntersectionDescription(), sass.desiredExit);
                        Behavior b = new HoldBrakeBehavior();
                        return new Maneuver(b, next, sass.DefaultStateDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // otherwise update the stop parameters
                        Behavior b = sass.Resume(vehicleState, vehicleSpeed);
                        return new Maneuver(b, CoreCommon.CorePlanningState, sass.DefaultStateDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Change Lanes State

                else if (CoreCommon.CorePlanningState is ChangeLanesState)
                {
                    // get state
                    ChangeLanesState cls = (ChangeLanesState)CoreCommon.CorePlanningState;

                    #region Blockages

                    // check blockages
                    if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage)
                    {
                        // create the blockage state
                        EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                        // check not from a dynamicly moving vehicle
                        if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
                        {
                            // go to a blockage handling tactical
                            return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            ArbiterOutput.Output("Lane Change blockage reported for moving vehicle, ignoring");
                    }

                    #endregion

                    // get a good lane
                    ArbiterLane goodLane = null;
                    if(!cls.Parameters.InitialOncoming)
                        goodLane = cls.Parameters.Initial;
                    else if(!cls.Parameters.TargetOncoming)
                        goodLane = cls.Parameters.Target;
                    else
                        throw new Exception("not going from or to good lane");

                    // nav plan to find poi
                    #warning make goal better if there is none come to stop
                    RoadPlan rp = navigation.PlanNavigableArea(goodLane, vehicleState.Front,
                        CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List<ArbiterWaypoint>());

                    // check current behavior type
                    bool done = CoreCommon.Communications.HasCompleted((new ChangeLaneBehavior(null, null, false, 0, null, null)).GetType());

                    if (done)
                    {
                        if (cls.Parameters.TargetOncoming)
                            return new Maneuver(
                                new StayInLaneBehavior(cls.Parameters.Target.LaneId,
                                    new ScalarSpeedCommand(cls.Parameters.Parameters.RecommendedSpeed),
                                    cls.Parameters.Parameters.VehiclesToIgnore,
                                    cls.Parameters.Target.ReversePath,
                                    cls.Parameters.Target.Width,
                                    cls.Parameters.Target.NumberOfLanesRight(vehicleState.Front, !cls.Parameters.InitialOncoming),
                                    cls.Parameters.Target.NumberOfLanesLeft(vehicleState.Front, !cls.Parameters.InitialOncoming)),
                                new OpposingLanesState(cls.Parameters.Target, true, cls, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        else
                            return new Maneuver(
                                new StayInLaneBehavior(cls.Parameters.Target.LaneId,
                                    new ScalarSpeedCommand(cls.Parameters.Parameters.RecommendedSpeed),
                                    cls.Parameters.Parameters.VehiclesToIgnore,
                                    cls.Parameters.Target.LanePath(),
                                    cls.Parameters.Target.Width,
                                    cls.Parameters.Target.NumberOfLanesLeft(vehicleState.Front, !cls.Parameters.InitialOncoming),
                                    cls.Parameters.Target.NumberOfLanesRight(vehicleState.Front, !cls.Parameters.InitialOncoming)),
                                new StayInLaneState(cls.Parameters.Target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        return tactical.Plan(cls, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
                    }
                }

                #endregion

                #region Opposing Lanes State

                else if (CoreCommon.CorePlanningState is OpposingLanesState)
                {
                    // get state
                    OpposingLanesState ols = (OpposingLanesState)CoreCommon.CorePlanningState;
                    ols.SetClosestGood(vehicleState);

                    #region Blockages

                    // check blockages
                    if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage)
                    {
                        // create the blockage state
                        EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);

                        // check not from a dynamicly moving vehicle
                        if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
                        {
                            // go to a blockage handling tactical
                            return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            ArbiterOutput.Output("Opposing Lane blockage reported for moving vehicle, ignoring");
                    }

                    #endregion

                    // check closest good null
                    if (ols.ClosestGoodLane != null)
                    {
                        // nav plan to find poi
                        RoadPlan rp = navigation.PlanNavigableArea(ols.ClosestGoodLane, vehicleState.Position, goal, new List<ArbiterWaypoint>());

                        // plan final tactical maneuver
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                    // otherwise need to make a uturn
                    else
                    {
                        ArbiterOutput.Output("in opposing lane with no closest good, making a uturn");
                        ArbiterLanePartition alp = ols.OpposingLane.GetClosestPartition(vehicleState.Front);
                        Coordinates c1 = vehicleState.Front + alp.Vector().Normalize(8.0);
                        Coordinates c2 = vehicleState.Front - alp.Vector().Normalize(8.0);
                        LinePath lpTmp = new LinePath(new Coordinates[] { c1, c2 });
                        List<Coordinates> pCoords = new List<Coordinates>();
                        pCoords.AddRange(lpTmp.ShiftLateral(ols.OpposingLane.Width)); //* 1.5));
                        pCoords.AddRange(lpTmp.ShiftLateral(-ols.OpposingLane.Width));// / 2.0));
                        Polygon uturnPoly = Polygon.GrahamScan(pCoords);
                        uTurnState uts = new uTurnState(ols.OpposingLane, uturnPoly, true);
                        uts.Interconnect = alp.ToInterconnect;

                        // plan final tactical maneuver
                        Maneuver final = new Maneuver(new NullBehavior(), uts, TurnDecorators.LeftTurnDecorator, vehicleState.Timestamp);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Starting up off of chute state

                else if (CoreCommon.CorePlanningState is StartupOffChuteState)
                {
                    // cast the type
                    StartupOffChuteState socs = (StartupOffChuteState)CoreCommon.CorePlanningState;

                    // check if in lane part of chute
                    if (CoreCommon.Communications.HasCompleted((new TurnBehavior(null, null, null, null, null, null)).GetType()))
                    {
                        // go to lane state
                        return new Maneuver(new NullBehavior(), new StayInLaneState(socs.Final.Lane, new Probability(0.8, 0.2), true, socs), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // otherwise continue
                    else
                    {
                        // simple maneuver generation
                        TurnBehavior tb = (TurnBehavior)socs.Resume(vehicleState, 1.4);

                        // add bounds to observable
                        CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(tb.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
                        CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(tb.RightBound, ArbiterInformationDisplayObjectType.rightBound));

                        // final maneuver
                        return new Maneuver(tb, socs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Unknown

                else
                {
                    // non-handled state
                    throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
                }

                #endregion
            }

            #endregion

            #region Intersection State

            else if (CoreCommon.CorePlanningState is IntersectionState)
            {
                #region Waiting at Intersection Exit State

                if (CoreCommon.CorePlanningState is WaitingAtIntersectionExitState)
                {
                    // get state
                    WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)CoreCommon.CorePlanningState;

                    // nav plan
                    IntersectionPlan ip = navigation.PlanIntersection(waies.exitWaypoint, goal);

                    // plan
                    Maneuver final = tactical.Plan(waies, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Stopping At Exit State

                else if (CoreCommon.CorePlanningState is StoppingAtExitState)
                {
                    // get state
                    StoppingAtExitState saes = (StoppingAtExitState)CoreCommon.CorePlanningState;

                    // check to see if we're stopped
                    if (CoreCommon.Communications.HasCompleted((new StayInLaneBehavior(null, null, null)).GetType()))
                    {
                        // check if we've hit goal if stop is cp
                        if (saes.waypoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
                        {
                            ArbiterOutput.Output("Stopped at current goal: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + ", Removing");
                            CoreCommon.Mission.MissionCheckpoints.Dequeue();

                            if (CoreCommon.Mission.MissionCheckpoints.Count == 0)
                            {
                                return new Maneuver(new HoldBrakeBehavior(), new NoGoalsLeftState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                            }
                        }

                        // move to the intersection
                        IState next = new WaitingAtIntersectionExitState(saes.waypoint, saes.turnDirection, new IntersectionDescription(), saes.desiredExit);
                        Behavior b = new HoldBrakeBehavior();
                        return new Maneuver(b, next, saes.DefaultStateDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // nav plan
                        IntersectionPlan ip = navigation.PlanIntersection(saes.waypoint, goal);

                        // update the intersection monitor
                        if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
                        {
                            IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
                                saes.waypoint,
                                CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
                                vehicleState, ip.BestOption);
                        }
                        else
                            IntersectionTactical.IntersectionMonitor = null;

                        // plan final tactical maneuver
                        Maneuver final = tactical.Plan(saes, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Turn State

                else if (CoreCommon.CorePlanningState is TurnState)
                {
                    // get state
                    TurnState ts = (TurnState)CoreCommon.CorePlanningState;

                    // check if in other lane
                    if (CoreCommon.Communications.HasCompleted((new TurnBehavior(null, null, null, null, null, null)).GetType()))
                    {
                        if (ts.Interconnect.FinalGeneric is ArbiterWaypoint)
                        {
                            // get final wp, and if next cp, remove
                            ArbiterWaypoint final = (ArbiterWaypoint)ts.Interconnect.FinalGeneric;
                            if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(final.AreaSubtypeWaypointId))
                                CoreCommon.Mission.MissionCheckpoints.Dequeue();

                            // stay in target lane
                            IState nextState = new StayInLaneState(ts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
                            Behavior b = new NullBehavior();
                            return new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else if (ts.Interconnect.FinalGeneric is ArbiterPerimeterWaypoint)
                        {
                            // stay in target lane
                            IState nextState = new ZoneTravelingState(((ArbiterPerimeterWaypoint)ts.Interconnect.FinalGeneric).Perimeter.Zone, (INavigableNode)ts.Interconnect.FinalGeneric);
                            Behavior b = new NullBehavior();
                            return new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                            throw new Exception("unhandled unterconnect final wp type");
                    }

                    // get interconnect
                    if (ts.Interconnect.FinalGeneric is ArbiterWaypoint)
                    {
                        // nav plan
                        IntersectionPlan ip = navigation.PlanIntersection((ITraversableWaypoint)ts.Interconnect.InitialGeneric, goal);

                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                    // else to zone
                    else if (ts.Interconnect.FinalGeneric is ArbiterPerimeterWaypoint)
                    {
                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                    else
                    {
                        throw new Exception("method not imp");
                    }
                }

                #endregion

                #region uTurn State

                else if (CoreCommon.CorePlanningState is uTurnState)
                {
                    // get state
                    uTurnState uts = (uTurnState)CoreCommon.CorePlanningState;

                    // plan over the target segment, ignoring the initial waypoint of the target lane
                    ArbiterWaypoint initial = uts.TargetLane.GetClosestPartition(vehicleState.Position).Initial;
                    List<ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(initial.Lane.Way, vehicleState.Front, initial);
                    RoadPlan rp = navigation.PlanRoads(uts.TargetLane, vehicleState.Front, goal, iws);

                    // plan
                    Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Intersection Startup State

                else if (CoreCommon.CorePlanningState is IntersectionStartupState)
                {
                    // get startup state
                    IntersectionStartupState iss = (IntersectionStartupState)CoreCommon.CorePlanningState;

                    // get intersection
                    ArbiterIntersection ai = iss.Intersection;

                    // get plan
                    IEnumerable<ITraversableWaypoint> entries = ai.AllEntries.Values;
                    IntersectionStartupPlan isp = navigation.PlanIntersectionStartup(entries, goal);

                    // plan tac
                    Maneuver final = tactical.Plan(iss, isp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return
                    return final;
                }

                #endregion

                #region Unknown

                else
                {
                    // non-handled state
                    throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
                }

                #endregion
            }

            #endregion

            #region Zone State

            else if (CoreCommon.CorePlanningState is ZoneState)
            {
                #region Zone Travelling State

                if (CoreCommon.CorePlanningState is ZoneTravelingState)
                {
                    // set state
                    ZoneTravelingState zts = (ZoneTravelingState)CoreCommon.CorePlanningState;

                    // check to see if we're stopped
                    if (CoreCommon.Communications.HasCompleted((new ZoneTravelingBehavior(null, null, new Polygon[0], null, null, null, null)).GetType()))
                    {
                        // plan over state and zone
                        ZonePlan zp = this.navigation.PlanZone(zts.Zone, zts.Start, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]);

                        if (zp.ZoneGoal is ArbiterParkingSpotWaypoint)
                        {
                            // move to parking state
                            ParkingState ps = new ParkingState(zp.Zone, ((ArbiterParkingSpotWaypoint)zp.ZoneGoal).ParkingSpot);
                            return new Maneuver(new HoldBrakeBehavior(), ps, TurnDecorators.NoDecorators, vehicleState.Timestamp);

                        }
                        else if(zp.ZoneGoal is ArbiterPerimeterWaypoint)
                        {
                            // get plan
                            IntersectionPlan ip = navigation.GetIntersectionExitPlan((ITraversableWaypoint)zp.ZoneGoal, goal);

                            // move to exit
                            WaitingAtIntersectionExitState waies = new WaitingAtIntersectionExitState((ITraversableWaypoint)zp.ZoneGoal, ip.BestOption.ToInterconnect.TurnDirection, new IntersectionDescription(), ip.BestOption.ToInterconnect);
                            return new Maneuver(new HoldBrakeBehavior(), waies, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }
                    else
                    {
                        // plan over state and zone
                        ZonePlan zp = this.navigation.PlanZone(zts.Zone, zts.Start, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]);

                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, zp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Parking State

                else if (CoreCommon.CorePlanningState is ParkingState)
                {
                    // set state
                    ParkingState ps = (ParkingState)CoreCommon.CorePlanningState;

                    // check to see if we're stopped
                    if (CoreCommon.Communications.HasCompleted((new ZoneParkingBehavior(null, null, new Polygon[0], null, null, null, null, null, 0.0)).GetType()))
                    {
                        if (ps.ParkingSpot.Checkpoint.CheckpointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber))
                        {
                            ArbiterOutput.Output("Reached Goal, cp: " + ps.ParkingSpot.Checkpoint.CheckpointId.ToString());
                            CoreCommon.Mission.MissionCheckpoints.Dequeue();
                        }

                        // pull out of the space
                        PullingOutState pos = new PullingOutState(ps.Zone, ps.ParkingSpot);
                        return new Maneuver(new HoldBrakeBehavior(), pos, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Pulling Out State

                else if (CoreCommon.CorePlanningState is PullingOutState)
                {
                    // set state
                    PullingOutState pos = (PullingOutState)CoreCommon.CorePlanningState;

                    // plan over state and zone
                    ZonePlan zp = this.navigation.PlanZone(pos.Zone, pos.ParkingSpot.Checkpoint, goal);

                    // check to see if we're stopped
                    if (CoreCommon.Communications.HasCompleted((new ZoneParkingPullOutBehavior(null, null, new Polygon[0], null, null, null, null, null, null, null, null)).GetType()))
                    {
                        // maneuver to next place to go
                        return new Maneuver(new HoldBrakeBehavior(), new ZoneTravelingState(pos.Zone, pos.ParkingSpot.Checkpoint), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // plan
                        Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, zp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                        // return final maneuver
                        return final;
                    }
                }

                #endregion

                #region Zone Startup State

                else if (CoreCommon.CorePlanningState is ZoneStartupState)
                {
                    // feed through the plan from the zone tactical
                    Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return final maneuver
                    return final;
                }

                #endregion

                #region Zone Orientation

                else if (CoreCommon.CorePlanningState is ZoneOrientationState)
                {
                    ZoneOrientationState zos = (ZoneOrientationState)CoreCommon.CorePlanningState;

                    // add bounds to observable
                    LinePath lp = new LinePath(new Coordinates[] { zos.final.Start.Position, zos.final.End.Position });
                    CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lp.ShiftLateral(TahoeParams.T), ArbiterInformationDisplayObjectType.leftBound));
                    CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lp.ShiftLateral(-TahoeParams.T), ArbiterInformationDisplayObjectType.rightBound));

                    // check to see if we're stopped
                    //if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
                    //{
                        // maneuver to next place to go
                        return new Maneuver(new HoldBrakeBehavior(), new ZoneTravelingState(zos.Zone, zos.final.End), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    //}
                    // not stopped doing hte maneuver
                    //else
                    //	return new Maneuver(zos.Resume(vehicleState, 1.4), zos, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                }

                #endregion

                #region Unknown

                else
                {
                    // non-handled state
                    throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
                }

                #endregion
            }

            #endregion

            #region Other State

            else if (CoreCommon.CorePlanningState is OtherState)
            {
                #region Start Up State

                if (CoreCommon.CorePlanningState is StartUpState)
                {
                    // get state
                    StartUpState sus = (StartUpState)CoreCommon.CorePlanningState;

                    // make a new startup agent
                    StartupReasoning sr = new StartupReasoning(this.laneAgent);

                    // get final state
                    IState nextState = sr.Startup(vehicleState, carMode);

                    // return no op ad zero all decorators
                    Behavior nextBehavior = sus.Resume(vehicleState, vehicleSpeed);

                    // return maneuver
                    return new Maneuver(nextBehavior, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                }

                #endregion

                #region Paused State

                else if (CoreCommon.CorePlanningState is PausedState)
                {
                    // if switch back to run
                    if (carMode == CarMode.Run)
                    {
                        // get state
                        PausedState ps = (PausedState)CoreCommon.CorePlanningState;

                        // get what we were previously doing
                        IState previousState = ps.PreviousState();

                        // check if can resume
                        if (previousState != null && previousState.CanResume())
                        {
                            // resume state is next
                            return new Maneuver(new HoldBrakeBehavior(), new ResumeState(previousState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        // otherwise go to startup
                        else
                        {
                            // next state is startup
                            IState nextState = new StartUpState();

                            // return no op
                            Behavior nextBehavior = new HoldBrakeBehavior();

                            // return maneuver
                            return new Maneuver(nextBehavior, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }
                    // otherwise stay stopped
                    else
                    {
                        // stay stopped and paused
                        return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Human State

                else if (CoreCommon.CorePlanningState is HumanState)
                {
                    // change to startup
                    if (carMode == CarMode.Run)
                    {
                        // next is startup
                        IState next = new StartUpState();

                        // next behavior just stay iin place for now
                        Behavior behavior = new HoldBrakeBehavior();

                        // return startup maneuver
                        return new Maneuver(behavior, next, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // in human mode still
                    else
                    {
                        // want to remove old behavior stuff
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Resume State

                else if (CoreCommon.CorePlanningState is ResumeState)
                {
                    // get state
                    ResumeState rs = (ResumeState)CoreCommon.CorePlanningState;

                    // make sure can resume (this is simple action)
                    if (rs.StateToResume != null && rs.StateToResume.CanResume())
                    {
                        // return old behavior
                        Behavior nextBehavior = rs.Resume(vehicleState, vehicleSpeed);

                        // return maneuver
                        return new Maneuver(nextBehavior, rs.StateToResume, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // otherwise just startup
                    else
                    {
                        // startup
                        return new Maneuver(new HoldBrakeBehavior(), new StartUpState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region No Goals Left State

                else if (CoreCommon.CorePlanningState is NoGoalsLeftState)
                {
                    // check if goals available
                    if (CoreCommon.Mission.MissionCheckpoints.Count > 0)
                    {
                        // startup
                        return new Maneuver(new HoldBrakeBehavior(), new StartUpState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    else
                    {
                        // stay paused
                        return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region eStopped State

                else if (CoreCommon.CorePlanningState is eStoppedState)
                {
                    // change to startup
                    if (carMode == CarMode.Run)
                    {
                        // next is startup
                        IState next = new StartUpState();

                        // next behavior just stay iin place for now
                        Behavior behavior = new HoldBrakeBehavior();

                        // return startup maneuver
                        return new Maneuver(behavior, next, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // in human mode still
                    else
                    {
                        // want to remove old behavior stuff
                        return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }

                #endregion

                #region Unknown

                else
                {
                    // non-handled state
                    throw new ArgumentException("Unknown OtherState type", "CoreCommon.CorePlanningState");
                }

                #endregion
            }

            #endregion

            #region Blockage State

            else if (CoreCommon.CorePlanningState is BlockageState)
            {
                #region Blockage State

                // something is blocked, in the encountered state we want to filter to base components of state
                if (CoreCommon.CorePlanningState is EncounteredBlockageState)
                {
                    // cast blockage state
                    EncounteredBlockageState bs = (EncounteredBlockageState)CoreCommon.CorePlanningState;

                    // plan through the blockage state with no road plan as just a quick filter
                    Maneuver final = tactical.Plan(bs, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return the final maneuver
                    return final;
                }

                #endregion

                #region Blockage Recovery State

                // recover from blockages
                else if (CoreCommon.CorePlanningState is BlockageRecoveryState)
                {
                    // get the blockage recovery state
                    BlockageRecoveryState brs = (BlockageRecoveryState)CoreCommon.CorePlanningState;

                    #region Check Various Statuses of Completion

                    // check successful completion report of behavior
                    if (brs.RecoveryBehavior != null && CoreCommon.Communications.HasCompleted(brs.RecoveryBehavior.GetType()))
                    {
                        // set updated status
                        ArbiterOutput.Output("Successfully received completion of behavior: " + brs.RecoveryBehavior.ToShortString() + ", " + brs.RecoveryBehavior.ShortBehaviorInformation());
                        brs.RecoveryStatus = BlockageRecoverySTATUS.COMPLETED;

                        // move to the tactical plan
                        return this.tactical.Plan(brs, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
                    }
                    // check operational startup
                    else if (CoreCommon.Communications.HasCompleted((new OperationalStartupBehavior()).GetType()))
                    {
                        // check defcon types
                        if (brs.Defcon == BlockageRecoveryDEFCON.REVERSE)
                        {
                            // abort maneuver as operational has no state information
                            return new Maneuver(new NullBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }

                    #endregion

                    #region Information

                    // set recovery information
                    CoreCommon.CurrentInformation.FQMState = brs.EncounteredState.ShortDescription();
                    CoreCommon.CurrentInformation.FQMStateInfo = brs.EncounteredState.StateInformation();
                    CoreCommon.CurrentInformation.FQMBehavior = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.ToShortString() : "NONE";
                    CoreCommon.CurrentInformation.FQMBehaviorInfo = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.ShortBehaviorInformation() : "NONE";
                    CoreCommon.CurrentInformation.FQMSpeed = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.SpeedCommandString() : "NONE";

                    #endregion

                    #region Blocked

                    if (brs.RecoveryStatus == BlockageRecoverySTATUS.BLOCKED)
                    {
                        if (brs.RecoveryBehavior is ChangeLaneBehavior)
                        {
                            brs.RecoveryStatus = BlockageRecoverySTATUS.ENCOUNTERED;
                            brs.Defcon = BlockageRecoveryDEFCON.CHANGELANES_FORWARD;
                            return new Maneuver(new HoldBrakeBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                        else
                        {
                            ArbiterOutput.Output("Recovery behavior blocked, reverting to abort state: " + brs.AbortState.ToString());
                            return new Maneuver(new HoldBrakeBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }

                    #endregion

                    #region Navigational Plan

                    // set navigational plan
                    INavigationalPlan navPlan = null;

                    #region Encountered

                    // blockage
                    if (brs.RecoveryStatus == BlockageRecoverySTATUS.ENCOUNTERED)
                    {
                        // get state
                        if (brs.AbortState is StayInLaneState)
                        {
                            // lane state
                            StayInLaneState sils = (StayInLaneState)brs.AbortState;
                            navPlan = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
                        }
                    }

                    #endregion

                    #region Completion

                    // blockage
                    if (brs.RecoveryStatus == BlockageRecoverySTATUS.COMPLETED)
                    {
                        // get state
                        if (brs.CompletionState is StayInLaneState)
                        {
                            // lane state
                            StayInLaneState sils = (StayInLaneState)brs.CompletionState;
                            navPlan = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
                        }
                    }

                    #endregion

                    #endregion

                    // move to the tactical plan
                    Maneuver final = this.tactical.Plan(brs, navPlan, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);

                    // return the final maneuver
                    return final;
                }

                #endregion
            }

            #endregion

            #region Unknown

            else
            {
                // non-handled state
                throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
            }

            // for now, return null
            return new Maneuver();

            #endregion
        }
コード例 #9
0
        /// <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;
            }
        }