コード例 #1
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="lane"></param>
 public StayInLaneState(ArbiterLane lane, Probability confidence, IState previous)
 {
     this.Lane = lane;
     this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId, confidence);
     this.IgnorableWaypoints = new List<IgnorableWaypoint>();
     this.CheckPreviousState(previous);
 }
コード例 #2
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="safetyZoneEnd"></param>
 /// <param name="safetyZoneBegin"></param>
 public ArbiterSafetyZone(ArbiterLane lane, LinePath.PointOnPath safetyZoneEnd, LinePath.PointOnPath safetyZoneBegin)
 {
     this.safetyZoneBegin = safetyZoneBegin;
     this.safetyZoneEnd = safetyZoneEnd;
     this.lane = lane;
     this.GenerateSafetyZone();
 }
コード例 #3
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="lane"></param>
 public OpposingLateralReasoning(ArbiterLane lane, SideObstacleSide sos)
 {
     this.lane = lane;
     this.ForwardMonitor = new OpposingForwardQuadrantMonitor();
     this.LateralMonitor = new OpposingLateralQuadrantMonitor(sos);
     this.RearMonitor = new OpposingRearQuadrantMonitor(lane, sos);
 }
        /// <summary>
        /// Checks if hte opposing lane is clear to pass an opposing vehicle
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        public bool ClearForDisabledVehiclePass(ArbiterLane lane, VehicleState state, double vUs, Coordinates minReturn)
        {
            // update the forward vehicle
            this.ForwardVehicle.Update(lane, state);

            // check if the rear vehicle exists and is moving along with us
            if (this.ForwardVehicle.ShouldUseForwardTracker && this.ForwardVehicle.CurrentVehicle != null)
            {
                // distance from other to us
                double currentDistance = lane.DistanceBetween(this.ForwardVehicle.CurrentVehicle.ClosestPosition, state.Front) - (2 * TahoeParams.VL);
                double minChangeDist = lane.DistanceBetween(minReturn, state.Front);

                // check if he's within min return dist
                if(currentDistance > minChangeDist)
                {
                    // params
                    double vOther = this.ForwardVehicle.CurrentVehicle.StateMonitor.Observed.speedValid ? this.ForwardVehicle.CurrentVehicle.Speed : lane.Way.Segment.SpeedLimits.MaximumSpeed;

                    // get distance of envelope for him to slow to our speed
                    double xEnvelope = (Math.Pow(vUs, 2.0) - Math.Pow(vOther, 2.0)) / (2.0 * -0.5);

                    // check to see if vehicle is outside of the envelope to slow down for us after 3 seconds
                    double xSafe = currentDistance - minChangeDist - (xEnvelope + (vOther * 15.0));
                    return xSafe > 0 ? true : false;
                }
                else
                    return false;
            }
            else
                return true;
        }
コード例 #5
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="safetyZoneEnd"></param>
 /// <param name="safetyZoneBegin"></param>
 public ArbiterSafetyZone(ArbiterLane lane, LinePath.PointOnPath safetyZoneEnd, LinePath.PointOnPath safetyZoneBegin)
 {
     this.safetyZoneBegin = safetyZoneBegin;
     this.safetyZoneEnd   = safetyZoneEnd;
     this.lane            = lane;
     this.GenerateSafetyZone();
 }
コード例 #6
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="leftLateral"></param>
 /// <param name="rightLateral"></param>
 public OpposingReasoning(ILateralReasoning leftLateral, ILateralReasoning rightLateral, ArbiterLane lane)
 {
     this.Lane = lane;
     this.leftLateralReasoning = leftLateral;
     this.rightLateralReasoning = rightLateral;
     this.OpposingForwardMonitor = new OpposingForwardQuadrantMonitor();
 }
コード例 #7
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="lane"></param>
 /// <param name="stopPoint"></param>
 /// <param name="currentPosition"></param>
 /// <param name="timeStamp"></param>
 public StoppingState(ArbiterLane lane, Coordinates stopPoint, Coordinates currentPosition, double timeStamp)
 {
     this.lane = lane;
     this.stopPoint = stopPoint;
     this.currentPosition = currentPosition;
     this.timeStamp = timeStamp;
     this.internalState = new InternalState(lane.LaneId, lane.LaneId);
 }
コード例 #8
0
        /// <summary>
        /// Plans the forward maneuver and secondary maneuver
        /// </summary>
        /// <param name="arbiterLane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="p"></param>
        /// <param name="blockage"></param>
        /// <returns></returns>
        public Maneuver ForwardManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, RoadPlan roadPlan,
            List<ITacticalBlockage> blockages)
        {
            // get primary maneuver
            Maneuver primary = this.OpposingForwardMonitor.PrimaryManeuver(arbiterLane, closestGood, vehicleState, blockages);

            // return primary for now
            return primary;
        }
コード例 #9
0
        /// <summary>
        /// Generates adjacency of a partition to another lane
        /// </summary>
        /// <param name="alp"></param>
        /// <param name="al"></param>
        private void GenerateSinglePartitionAdjacency(ArbiterLanePartition alp, ArbiterLane al)
        {
            // fake path
            List <IPathSegment> fakePathSegments = new List <IPathSegment>();

            fakePathSegments.Add(new LinePathSegment(alp.Initial.Position, alp.Final.Position));
            Path fakePath = new Path(fakePathSegments);

            // partitions adjacent
            List <ArbiterLanePartition> adjacent = new List <ArbiterLanePartition>();

            // tracks
            PointOnPath current   = fakePath.StartPoint;
            double      increment = 0.5;
            double      tmpInc    = 0;

            // increment along
            while (tmpInc == 0)
            {
                // loop over partitions
                foreach (ArbiterLanePartition alpar in al.Partitions)
                {
                    // get fake path for partition
                    List <IPathSegment> ips = new List <IPathSegment>();
                    ips.Add(new LinePathSegment(alpar.Initial.Position, alpar.Final.Position));
                    Path alpPath = new Path(ips);

                    // get closest point on tmp partition to current
                    PointOnPath closest = alpPath.GetClosest(current.pt);

                    // check general distance
                    if (closest.pt.DistanceTo(current.pt) < 20)
                    {
                        // check not start or end
                        if (!closest.Equals(alpPath.StartPoint) && !closest.Equals(alpPath.EndPoint))
                        {
                            // check not already added
                            if (!adjacent.Contains(alpar))
                            {
                                // add
                                adjacent.Add(alpar);
                            }
                        }
                    }
                }

                // set inc
                tmpInc = increment;

                // increment point
                current = fakePath.AdvancePoint(current, ref tmpInc);
            }

            // add adjacent
            alp.NonLaneAdjacentPartitions.AddRange(adjacent);
        }
コード例 #10
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="lane"></param>
 /// <param name="waypoint"></param>
 /// <param name="turnDirection"></param>
 /// <param name="isNavigationExit"></param>
 /// <param name="desiredExit"></param>
 public StoppingAtStopState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
     bool isNavigationExit, ArbiterInterconnect desiredExit)
 {
     this.lane = lane;
     this.waypoint = waypoint;
     this.turnDirection = turnDirection;
     this.isNavigationExit = isNavigationExit;
     this.desiredExit = desiredExit;
     this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
 }
コード例 #11
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="interconnect"></param>
 /// <param name="turn"></param>
 public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
     LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
 {
     this.Interconnect = interconnect;
     this.turnDirection = turn;
     this.TargetLane = target;
     this.EndingPath = endingPath;
     this.LeftBound = left;
     this.RightBound = right;
     this.SpeedCommand = speed;
     this.Saudi = saudi;
     this.UseTurnBounds = useTurnBounds;
 }
コード例 #12
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="lane"></param>
 /// <param name="waypoint"></param>
 /// <param name="turnDirection"></param>
 /// <param name="isNavigationExit"></param>
 /// <param name="desiredExit"></param>
 public StoppingAtExitState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection,
     bool isNavigationExit, ArbiterInterconnect desiredExit, double timeStamp, Coordinates currentPosition)
 {
     this.lane = lane;
     this.waypoint = waypoint;
     this.turnDirection = turnDirection;
     this.isNavigationExit = isNavigationExit;
     this.desiredExit = desiredExit;
     this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId);
     this.timeStamp = timeStamp;
     this.currentPosition = currentPosition;
     this.internalLaneState = new InternalState(this.lane.LaneId, this.lane.LaneId);
 }
コード例 #13
0
        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="globalMonitor"></param>
        /// <param name="involved"></param>
        public DominantLaneEntryMonitor(IntersectionMonitor globalMonitor, IntersectionInvolved involved)
        {
            this.waitingTimer = new Stopwatch();
            this.globalMonitor = globalMonitor;
            this.lane = (ArbiterLane)involved.Area;
            this.involved = involved;

            if (involved.Exit != null)
                this.exit = (ArbiterWaypoint)involved.Exit;
            else
                this.exit = null;

            if (this.exit != null)
            {
                // create the polygon
                this.exitPolygon = this.ExitPolygon();
            }
        }
コード例 #14
0
        /// <summary>
        /// Gets closest lane to a point
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public ArbiterLane ClosestLane(Coordinates point)
        {
            ArbiterLane closest = null;
            double      best    = Double.MaxValue;

            foreach (ArbiterSegment asg in ArbiterSegments.Values)
            {
                foreach (ArbiterLane al in asg.Lanes.Values)
                {
                    PointOnPath closestPoint = al.PartitionPath.GetClosest(point);
                    double      tmp          = closestPoint.pt.DistanceTo(point);
                    if (tmp < best)
                    {
                        closest = al;
                        best    = tmp;
                    }
                }
            }

            return(closest);
        }
コード例 #15
0
        public OpposingLanesState(ArbiterLane opposingLane, bool resetLaneAgent, IState previous, VehicleState vs)
        {
            this.resetLaneAgent = resetLaneAgent;
            this.OpposingLane = opposingLane;
            this.OpposingWay = opposingLane.Way.WayId;
            this.ClosestGoodLane = null;

            this.SetClosestGood(vs);

            if (previous is ChangeLanesState)
            {
                EntryParameters = ((ChangeLanesState)previous).Parameters;
            }
            else if (previous is OpposingLanesState)
            {
                EntryParameters = ((OpposingLanesState)previous).EntryParameters;
            }
            else
            {
                EntryParameters = null;
            }
        }
コード例 #16
0
        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="initial"></param>
        /// <param name="next"></param>
        /// <param name="final"></param>
        public SupraLane(ArbiterLane initial, ArbiterInterconnect next, ArbiterLane final)
        {
            Components = new SupraLaneComponentList();
            Components.Add(initial);
            Components.Add(next);
            Components.Add(final);
            this.Initial      = initial;
            this.Final        = final;
            this.Interconnect = next;

            if (!this.Initial.Equals(this.Final))
            {
                this.supraWaypoints = new List <ArbiterWaypoint>();
                this.supraWaypoints.AddRange(this.Initial.WaypointsInclusive(this.Initial.WaypointList[0], (ArbiterWaypoint)this.Interconnect.InitialGeneric));
                this.supraWaypoints.AddRange(this.Final.WaypointsInclusive((ArbiterWaypoint)this.Interconnect.FinalGeneric, this.Final.WaypointList[this.Final.WaypointList.Count - 1]));
                this.SetDefaultLanePath();
            }
            else
            {
                this.supraWaypoints = this.Initial.WaypointList;
                this.supraPath      = this.Initial.LanePath();
                this.supraPath.Add(this.Interconnect.FinalGeneric.Position);
            }
        }
コード例 #17
0
        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="initial"></param>
        /// <param name="next"></param>
        /// <param name="final"></param>
        public SupraLane(ArbiterLane initial, ArbiterInterconnect next, ArbiterLane final)
        {
            Components = new SupraLaneComponentList();
            Components.Add(initial);
            Components.Add(next);
            Components.Add(final);
            this.Initial = initial;
            this.Final = final;
            this.Interconnect = next;

            if (!this.Initial.Equals(this.Final))
            {
                this.supraWaypoints = new List<ArbiterWaypoint>();
                this.supraWaypoints.AddRange(this.Initial.WaypointsInclusive(this.Initial.WaypointList[0], (ArbiterWaypoint)this.Interconnect.InitialGeneric));
                this.supraWaypoints.AddRange(this.Final.WaypointsInclusive((ArbiterWaypoint)this.Interconnect.FinalGeneric, this.Final.WaypointList[this.Final.WaypointList.Count - 1]));
                this.SetDefaultLanePath();
            }
            else
            {
                this.supraWaypoints = this.Initial.WaypointList;
                this.supraPath = this.Initial.LanePath();
                this.supraPath.Add(this.Interconnect.FinalGeneric.Position);
            }
        }
コード例 #18
0
        /// <summary>
        /// Check if a side sick blocking obstacle is detected
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        private bool SideSickObstacleDetected(ArbiterLane lane, VehicleState state)
        {
            try
            {
                // get distance from vehicle to lane opp side
                Coordinates vec = state.Front - state.Position;
                vec = this.VehicleSide == SideObstacleSide.Driver ? vec.Rotate90() : vec.RotateM90();

                SideObstacles sobs = CoreCommon.Communications.GetSideObstacles(this.VehicleSide);
                if (sobs != null && sobs.obstacles != null)
                {
                    foreach (SideObstacle so in sobs.obstacles)
                    {
                        Coordinates cVec = state.Position + vec.Normalize(so.distance);
                        if (so.height > 0.7 && lane.LanePolygon.IsInside(cVec))
                            return true;
                    }
                }
            }
            catch (Exception ex)
            {
                ArbiterOutput.Output("side sick obstacle exception: " + ex.ToString());
            }

            return false;
        }
コード例 #19
0
        /// <summary>
        /// Maneuver for recovering from a lane blockage, used for lane changes as well
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicleSpeed"></param>
        /// <param name="defcon"></param>
        /// <param name="saudi"></param>
        /// <returns></returns>
        public Maneuver OpposingLaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
            INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly)
        {
            // get the blockage
            ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage;

            // get the turn state
            OpposingLanesState sils = (OpposingLanesState)brs.EncounteredState.PlanningState;

            #region Reverse

            // check the state of the recovery for being the initial state
            if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
            {
                // determine if the reverse behavior is recommended
                if (laneBlockageReport.BlockageReport.ReverseRecommended)
                {
                    // notify
                    ArbiterOutput.Output("Initial encounter, reverse recommended, reversing");

                    // path
                    LinePath revPath = lane.LanePath().Clone();
                    revPath.Reverse();

                    // dist to reverse
                    double distToReverse;
                    if (double.IsNaN(laneBlockageReport.BlockageReport.DistanceToBlockage))
                        distToReverse = TahoeParams.VL * 1.5;
                    else if (laneBlockageReport.BlockageReport.DistanceToBlockage < 0 || laneBlockageReport.BlockageReport.DistanceToBlockage > 2 * TahoeParams.VL)
                        distToReverse = TahoeParams.VL * 1.5;
                    else
                        distToReverse = TahoeParams.VL - laneBlockageReport.BlockageReport.DistanceToBlockage;

                    // get reverse behavior
                    StayInLaneBehavior reverseRecovery = new StayInLaneBehavior(lane.LaneId,
                        new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(),
                        revPath, lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false));

                    // get recovery state
                    BlockageRecoveryState brsT = new BlockageRecoveryState(
                        reverseRecovery, CoreCommon.CorePlanningState, new OpposingLanesState(lane, false, CoreCommon.CorePlanningState, vehicleState),
                        brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                    brsT.CompletionState = brsT;

                    // reset blockages
                    BlockageHandler.SetDefaultBlockageCooldown();

                    // maneuver
                    return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
                }
            }

            #endregion

            #region Recovery Escalation

            // plan forward reasoning
            this.tacticalUmbrella.roadTactical.opposingReasoning.ForwardManeuver(lane,
                ((OpposingLanesState)brs.EncounteredState.PlanningState).ClosestGoodLane,
                vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>());

            // check clear on right, or if it does nto exist here
            ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
            bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) ||
                rightLateral.AdjacentAndRearClear(vehicleState);

            // check clear on left, or if it does nto exist here
            ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
            bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) ||
                leftLateral.AdjacentAndRearClear(vehicleState);
            if (leftClear && leftLateral is OpposingLateralReasoning)
                leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24,
                    new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
                    lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location);

            // check both clear to widen
            bool widenOk = rightClear && leftClear;

            // Notify
            ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftClear: " + leftClear.ToString());

            // path
            LinePath revPath2 = lane.LanePath().Clone();
            revPath2.Reverse();

            // check widen
            if (widenOk)
            {
                // output
                ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear");

                if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD)
                {
                    // check change lanes back to good
                    ChangeLaneBehavior clb = new ChangeLaneBehavior(
                        lane.LaneId, sils.ClosestGoodLane.LaneId, false, TahoeParams.VL * 3.0, new StopAtDistSpeedCommand(TahoeParams.VL * 3.0), new List<int>(),
                        revPath2, sils.ClosestGoodLane.LanePath(), lane.Width, sils.ClosestGoodLane.Width,
                        lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false));

                    // change lanes
                    brs.Defcon = BlockageRecoveryDEFCON.CHANGELANES_FORWARD;

                    // cehck ok
                    CompletionReport clROk;
                    bool clCheck = CoreCommon.Communications.TestExecute(clb, out clROk);

                    // check change lanes ok
                    if (clCheck)
                    {
                        ArbiterOutput.Output("Change lanes behaviro ok, executing");
                        // return manevuer
                        return new Maneuver(clb, brs, TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
                    }
                    else
                    {
                        ArbiterOutput.Output("Change lanes behaviro not ok");
                    }
                }

                // if we can't widen for some reason just go through with no widen
                StayInLaneBehavior silb = new StayInLaneBehavior(lane.LaneId,
                new StopAtDistSpeedCommand(TahoeParams.VL * 2.0), new List<int>(),
                revPath2,
                lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false));

                // mini icrease width
                silb.LaneWidth = silb.LaneWidth + TahoeParams.T;

                // check execute none saudi
                CompletionReport l0Cr;
                bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr);

                // check mini ok
                if (l0TestOk)
                {
                    // notify
                    ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok");

                    // update the current state
                    BlockageRecoveryState brsL0 = new BlockageRecoveryState(
                        silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                    return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                }
                else
                {
                    // notify
                    ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen");

                    // increase width
                    silb.LaneWidth = silb.LaneWidth * 3.0;

                    // check execute l1
                    CompletionReport l1Cr;
                    bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);

                    // check ok
                    if (l1TestOk)
                    {
                        // notify
                        ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok");

                        // update the current state
                        BlockageRecoveryState brsL1 = new BlockageRecoveryState(
                            silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                        return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // go to l2 for all the marbles
                    else
                    {
                        // notify
                        ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi");

                        ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1);
                        List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 });
                        silb.Decorators = saudi2bds;
                        BlockageRecoveryState brsL2 = new BlockageRecoveryState(
                            silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);

                        // check execute l1
                        CompletionReport l2Cr;
                        bool l2TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);

                        // go
                        return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp);
                    }
                }
            }

            #endregion

            // fallout goes back to lane state
            return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
        }
コード例 #20
0
        /// <summary>
        /// Maneuver for recovering from a lane blockage, used for lane changes as well
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicleSpeed"></param>
        /// <param name="defcon"></param>
        /// <param name="saudi"></param>
        /// <returns></returns>
        public Maneuver LaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, 
            INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly, out bool waitForUTurn)
        {
            // get the blockage
            ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage;

            // get the turn state
            StayInLaneState sils = new StayInLaneState(lane, CoreCommon.CorePlanningState);

            // set wait false
            waitForUTurn = false;

            #region Reverse

            // check the state of the recovery for being the initial state
            if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
            {
                // determine if the reverse behavior is recommended
                if (laneBlockageReport.BlockageReport.ReverseRecommended)
                {
                    // notify
                    ArbiterOutput.Output("Initial encounter, reverse recommended, reversing");

                    // get reverse behavior
                    StayInLaneBehavior reverseRecovery = this.LaneReverseRecovery(lane, vehicleState, vehicleSpeed, brs.EncounteredState.TacticalBlockage.BlockageReport);
                    reverseRecovery.TimeStamp = vehicleState.Timestamp;

                    // get recovery state
                    BlockageRecoveryState brsT = new BlockageRecoveryState(
                        reverseRecovery, null, new StayInLaneState(lane, CoreCommon.CorePlanningState),
                        brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                    brsT.CompletionState = brsT;

                    // reset blockages
                    BlockageHandler.SetDefaultBlockageCooldown();

                    // maneuver
                    return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
                }
            }

            #endregion

            #region uTurn

            // check if uturn is available
            ArbiterLane opp = this.tacticalUmbrella.roadTactical.forwardReasoning.GetClosestOpposing(lane, vehicleState);

            // resoning
            OpposingLateralReasoning olrTmp = new OpposingLateralReasoning(opp, SideObstacleSide.Driver);

            if (opp != null && olrTmp.ExistsExactlyHere(vehicleState) && opp.IsInside(vehicleState.Front) && opp.LanePath().GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front) < TahoeParams.VL * 3.0)
            {
                // check possible to reach goal given block partition way
                RoadPlan uTurnNavTest = CoreCommon.Navigation.PlanRoadOppositeWithoutPartition(
                    lane.GetClosestPartition(vehicleState.Front),
                    opp.GetClosestPartition(vehicleState.Front),
                    CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
                    true,
                    vehicleState.Front,
                    true);

                // flag to try the uturn
                bool uturnTry = false;

                // all polygons to include
                List<Polygon> allPolys = BlockageTactical.AllForwardPolygons(vehicleState, failedVehiclesPersistentlyOnly);// .AllObstacleAndStoppedVehiclePolygons(vehicleState);

                // test blockage takes up segment
                double numLanesLeft = lane.NumberOfLanesLeft(vehicleState.Front, true);
                double numLanesRight = lane.NumberOfLanesRight(vehicleState.Front, true);
                LinePath leftBound = lane.LanePath().ShiftLateral((lane.Width * numLanesLeft) + (lane.Width / 2.0));
                LinePath rightBound = lane.LanePath().ShiftLateral((lane.Width * numLanesRight) + (lane.Width / 2.0));
                bool segmentBlockage = BlockageTester.TestBlockage(allPolys, leftBound, rightBound);
                uturnTry = segmentBlockage;

                // check if we should try the uturn
                if(uturnTry)
                {
                    // check uturn timer
                    if(uTurnTimer.ElapsedMilliseconds / 1000.0 > 2.0)
                    {
                        #region Determine Checkpoint

                        // check route feasible
                        if (uTurnNavTest.BestPlan.laneWaypointOfInterest.RouteTime < double.MaxValue - 1
                            && uTurnNavTest.BestPlan.laneWaypointOfInterest.TimeCostToPoint < double.MaxValue - 1)
                        {
                            ArbiterOutput.Output("valid route to checkpoint by rerouting, uturning");
                        }
                        // otherwise remove the next checkpoint
                        else
                        {
                            // notiify
                            ArbiterOutput.Output("NO valid route to checkpoint by rerouting");

                            // get the next cp
                            ArbiterCheckpoint cp1 = CoreCommon.Mission.MissionCheckpoints.Peek();

                            // get distance to blockage
                            double blockageDistance = 15.0;

                            // check cp is in our lane
                            if (cp1.WaypointId.AreaSubtypeId.Equals(lane.LaneId))
                            {
                                // check distance to cp1
                                double distanceToCp1 = lane.DistanceBetween(vehicleState.Front, CoreCommon.RoadNetwork.ArbiterWaypoints[cp1.WaypointId].Position);

                                // check that this is an already inserted waypoint
                                if (cp1.Type == CheckpointType.Inserted)
                                {
                                    // remove cp
                                    ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                    ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as inserted type of checkpoint");

                                    ArbiterCheckpoint ac2 = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                    ArbiterOutput.Output("removed checkpoint: " + ac2.WaypointId.ToString() + " as blocked type of checkpoint");
                                }
                                // closer to us than the blockage
                                else if (distanceToCp1 < blockageDistance)
                                {
                                    // remove cp
                                    ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                    ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as between us and the blockage ~ 15m away");
                                }
                                // very close to blockage on the other side
                                else if (distanceToCp1 < blockageDistance + 5.0)
                                {
                                    // remove cp
                                    ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                    ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as on the other side of blockage ~ 15m away");
                                }
                                // further away from the blockage
                                else
                                {
                                    // check over all checkpoints if there exists a checkpoint cp2 in the opposing lane within 5m along our lane
                                    ArbiterCheckpoint cp2 = null;
                                    foreach (ArbiterWaypoint oppAw in opp.WaypointList)
                                    {
                                        // check checkpoint
                                        if (oppAw.IsCheckpoint)
                                        {
                                            // distance between us and that waypoint
                                            double distanceToCp2 = lane.DistanceBetween(vehicleState.Front, oppAw.Position);

                                            // check along distance < 5.0m
                                            if (Math.Abs(distanceToCp1 - distanceToCp2) < 5.0)
                                            {
                                                // set cp
                                                cp2 = new ArbiterCheckpoint(oppAw.CheckpointId, oppAw.WaypointId, CheckpointType.Inserted);
                                            }
                                        }
                                    }

                                    // check close cp exists
                                    if (cp2 != null)
                                    {
                                        // remove cp1 and replace with cp2
                                        ArbiterOutput.Output("inserting checkpoint: " + cp2.WaypointId.ToString() + " before checkpoint: " + cp1.WaypointId.ToString() + " as can be replaced with adjacent opposing cp2");
                                        //CoreCommon.Mission.MissionCheckpoints.Dequeue();

                                        // get current checkpoints
                                        ArbiterCheckpoint[] updatedAcs = new ArbiterCheckpoint[CoreCommon.Mission.MissionCheckpoints.Count + 1];
                                        updatedAcs[0] = cp2;
                                        CoreCommon.Mission.MissionCheckpoints.CopyTo(updatedAcs, 1);
                                        updatedAcs[1].Type = CheckpointType.Blocked;
                                        CoreCommon.Mission.MissionCheckpoints = new Queue<ArbiterCheckpoint>(new List<ArbiterCheckpoint>(updatedAcs));
                                    }
                                    // otherwise remove cp1
                                    else
                                    {
                                        // remove cp
                                        ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                        ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
                                    }
                                }
                            }
                            // otherwise we remove the checkpoint
                            else
                            {
                                // remove cp
                                ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
                                ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
                            }
                        }

                        #endregion

                        #region Plan Uturn

                        // notify
                        ArbiterOutput.Output("Segment blocked, uturn available");

                        // nav penalties
                        ArbiterLanePartition alpClose = lane.GetClosestPartition(vehicleState.Front);
                        CoreCommon.Navigation.AddHarshBlockageAcrossSegment(alpClose, vehicleState.Front);

                        // uturn
                        LinePath lpTmp = new LinePath(new Coordinates[] { vehicleState.Front, opp.LanePath().GetClosestPoint(vehicleState.Front).Location });
                        Coordinates vector = lpTmp[1] - lpTmp[0];
                        lpTmp[1] = lpTmp[1] + vector.Normalize(opp.Width / 2.0);
                        lpTmp[0] = lpTmp[0] - vector.Normalize(lane.Width / 2.0);
                        LinePath lb = lpTmp.ShiftLateral(15.0);
                        LinePath rb = lpTmp.ShiftLateral(-15.0);
                        List<Coordinates> uturnPolyCOords = new List<Coordinates>();
                        uturnPolyCOords.AddRange(lb);
                        uturnPolyCOords.AddRange(rb);
                        uTurnState uts = new uTurnState(opp, Polygon.GrahamScan(uturnPolyCOords));

                        // reset blockages
                        BlockageHandler.SetDefaultBlockageCooldown();

                        // reset the timers
                        this.Reset();

                        // go to uturn
                        return new Maneuver(uts.Resume(vehicleState, 2.0), uts, TurnDecorators.NoDecorators, vehicleState.Timestamp);

                        #endregion
                    }
                    // uturn timer not enough
                    else
                    {
                        // check timer running
                        if(!uTurnTimer.IsRunning)
                        {
                            uTurnTimer.Stop();
                            uTurnTimer.Reset();
                            uTurnTimer.Start();
                        }

                        // if gets here, need to wait
                        double time = uTurnTimer.ElapsedMilliseconds / 1000.0;
                        ArbiterOutput.Output("uTurn behavior evaluated to success, cooling down for: " + time.ToString("f2") + " out of 2");
                        waitForUTurn = true;
                        return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                }
                else
                {
                    waitForUTurn = false;
                    this.Reset();
                }
            }
            else
            {
                waitForUTurn = false;
                this.Reset();
            }

            #endregion

            #region Recovery Escalation

            // plan forward reasoning
            this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardManeuver(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>());

            // check clear on right, or if it does nto exist here
            ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
            bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) ||
                rightLateral.AdjacentAndRearClear(vehicleState);

            // check clear on left, or if it does nto exist here
            ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
            bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) ||
                leftLateral.AdjacentAndRearClear(vehicleState);
            if(leftClear && leftLateral is OpposingLateralReasoning)
                leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24,
                    new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
                    lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location);

            // check both clear to widen
            bool widenOk = rightClear && leftClear;

            // Notify
            ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftCler: " + leftClear.ToString());

            // if we can't widen for some reason just go through with no widen
            StayInLaneBehavior silb = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, brs.EncounteredState);
            silb.TimeStamp = vehicleState.Timestamp;

            // check widen
            if (widenOk)
            {
                // output
                ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear");

                // mini icrease width
                silb.LaneWidth = silb.LaneWidth + TahoeParams.T;

                // check execute none saudi
                CompletionReport l0Cr;
                bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr);

                // check mini ok
                if (l0TestOk)
                {
                    // notify
                    ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok");

                    // update the current state
                    BlockageRecoveryState brsL0 = new BlockageRecoveryState(
                        silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                    return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                }
                else
                {
                    // notify
                    ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen");

                    #region Change Lanes

                    // check not in change lanes
                    if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD && brs.Defcon != BlockageRecoveryDEFCON.WIDENBOUNDS)
                    {
                        // check normal change lanes reasoning
                        bool shouldWait;
                        IState laneChangeCompletionState;
                        ChangeLaneBehavior changeLanesBehavior = this.ChangeLanesRecoveryBehavior(lane, vehicleState, out shouldWait, out laneChangeCompletionState);

                        // check change lanes behaviore exists
                        if (changeLanesBehavior != null)
                        {
                            ArbiterOutput.Output("Lane Blockage Recovery: Found adjacent lane available change lanes beahvior: " + changeLanesBehavior.ToShortString() + ", " + changeLanesBehavior.ShortBehaviorInformation());

                            // update the current state
                            BlockageRecoveryState brsCL = new BlockageRecoveryState(
                                changeLanesBehavior, laneChangeCompletionState, sils, BlockageRecoveryDEFCON.CHANGELANES_FORWARD, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                            return new Maneuver(changeLanesBehavior, brsCL, changeLanesBehavior.ChangeLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
                        }
                        // check should wait
                        else if (shouldWait)
                        {
                            ArbiterOutput.Output("Lane Blockage Recovery: Should wait for the forward lane, waiting");
                            return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                        }
                    }

                    #endregion

                    // notify
                    ArbiterOutput.Output("Lane Blockage Recovery: Fell Through Forward Change Lanes");

                    // increase width
                    silb.LaneWidth = silb.LaneWidth * 3.0;

                    // check execute l1
                    CompletionReport l1Cr;
                    bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);

                    // check ok
                    if (l1TestOk)
                    {
                        // notify
                        ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok");

                        // update the current state
                        BlockageRecoveryState brsL1 = new BlockageRecoveryState(
                            silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                        return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp);
                    }
                    // go to l2 for all the marbles
                    else
                    {
                        // notify
                        ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi");

                        ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1);
                        List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 });
                        silb.Decorators = saudi2bds;
                        BlockageRecoveryState brsL2 = new BlockageRecoveryState(
                            silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
                        return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp);
                    }
                }
            }

            #endregion

            // fallout goes back to lane state
            return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
        }
コード例 #21
0
        /// <summary>
        /// Reverse in the lane
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="vehicleState"></param>
        /// <param name="vehicleSpeed"></param>
        /// <returns></returns>
        private StayInLaneBehavior LaneReverseRecovery(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, TrajectoryBlockedReport blockage)
        {
            // distance to reverse
            double distToReverse = double.IsNaN(blockage.DistanceToBlockage) ? TahoeParams.VL * 2.0 : TahoeParams.VL - blockage.DistanceToBlockage;
            if (distToReverse <= 3.0)
                distToReverse = TahoeParams.VL;

            // 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));

            #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)
                {
                    // do nothing
                    return null;
                }
                // 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 silb;
                }
            }

            #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 silb;
            }

            #endregion

            #region Vehicle Behind us

            // rear monitor
            RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor;

            // 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 null;
                        }
                    }
                }
            }

            #endregion

            // all clear, return reverse maneuver, set cooldown
            return reverseBehavior;
        }
コード例 #22
0
        /// <summary>
        /// Check if found a stop in a lane as part of exits or a test waypoint
        /// </summary>
        /// <param name="initialTest"></param>
        /// <param name="exits"></param>
        /// <param name="lane"></param>
        /// <returns></returns>
        private bool FoundStop(ArbiterWaypoint initialTest, IEnumerable<ITraversableWaypoint> exits, ArbiterLane lane)
        {
            if (initialTest != null && initialTest.IsStop)
                return true;

            foreach (ITraversableWaypoint exit in exits)
            {
                if (exit is ArbiterWaypoint)
                {
                    ArbiterWaypoint aw = (ArbiterWaypoint)exit;
                    if (aw.IsStop && aw.Lane.Equals(lane))
                    {
                        return true;
                    }
                }
            }

            return false;
        }
コード例 #23
0
        /// <summary>
        /// Gets parameterization
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="speed"></param>
        /// <param name="distance"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        public TravelingParameters GetParameters(ArbiterLane lane, double speed, double distance, VehicleState state)
        {
            double distanceCutOff = CoreCommon.OperationslStopLineSearchDistance;
            Maneuver m = new Maneuver();
            SpeedCommand sc;
            bool usingSpeed = true;

            #region Distance Cutoff

            // check if distance is less than cutoff
            if (distance < distanceCutOff)
            {
                // default behavior
                sc = new StopAtDistSpeedCommand(distance);
                Behavior b = new StayInLaneBehavior(lane.LaneId, sc, new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(state.Front, true), lane.NumberOfLanesRight(state.Front, true));

                // stopping so not using speed param
                usingSpeed = false;

                // standard behavior is fine for maneuver
                m = new Maneuver(b, new StayInLaneState(lane, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp);
            }

            #endregion

            #region Outisde Distance Envelope

            // not inside distance envalope
            else
            {
                // get lane
                ArbiterLane al = lane;

                // default behavior
                sc = new ScalarSpeedCommand(speed);
                Behavior b = new StayInLaneBehavior(al.LaneId, sc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));

                // standard behavior is fine for maneuver
                m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp);
            }

            #endregion

            #region Parameterize

            // create new params
            TravelingParameters tp = new TravelingParameters();
            tp.Behavior = m.PrimaryBehavior;
            tp.Decorators = m.PrimaryBehavior.Decorators;
            tp.DistanceToGo = distance;
            tp.NextState = m.PrimaryState;
            tp.RecommendedSpeed = speed;
            tp.Type = TravellingType.Navigation;
            tp.UsingSpeed = usingSpeed;
            tp.SpeedCommand = sc;
            tp.VehiclesToIgnore = new List<int>();

            // return navigation params
            return tp;

            #endregion
        }
コード例 #24
0
        public int NumberOfLanesLeft(Coordinates position, bool forwards)
        {
            if (forwards)
            {
                if (this.LaneOnLeft != null)
                {
                    // our lane #
                    int n = this.LaneId.Number;

                    // left lane #
                    int l = this.LaneOnLeft.LaneId.Number;

                    // count
                    int count = 0;

                    // if left lane numbers going down
                    if (n > l)
                    {
                        // loop over left lanes going down
                        for (int i = n - 1; i > 0; i--)
                        {
                            // determine who is close to this position
                            ArbiterLaneId lane1 = new ArbiterLaneId(i, this.Way.Segment.Way1.WayId);
                            ArbiterLaneId lane2 = new ArbiterLaneId(i, this.Way.Segment.Way2.WayId);

                            // get lane
                            ArbiterLane lane = null;
                            if (this.Way.Segment.Lanes.ContainsKey(lane1))
                            {
                                lane = this.Way.Segment.Lanes[lane1];
                            }
                            else if (this.Way.Segment.Lanes.ContainsKey(lane2))
                            {
                                lane = this.Way.Segment.Lanes[lane2];
                            }

                            // check lane exists
                            if (lane != null)
                            {
                                // determine if close and inside
                                bool close  = lane.LanePath().GetClosestPoint(position).Location.DistanceTo(position) < 15;
                                bool inside = lane.IsInside(position);

                                // add if both
                                if (close && inside)
                                {
                                    count++;
                                }
                            }
                        }
                    }
                    else
                    {
                        // loop over left lanes
                        for (int i = n + 1; i <= this.Way.Segment.Lanes.Count; i++)
                        {
                            // determine who is close to this position
                            ArbiterLaneId lane1 = new ArbiterLaneId(i, this.Way.Segment.Way1.WayId);
                            ArbiterLaneId lane2 = new ArbiterLaneId(i, this.Way.Segment.Way2.WayId);

                            // get lane
                            ArbiterLane lane = null;
                            if (this.Way.Segment.Lanes.ContainsKey(lane1))
                            {
                                lane = this.Way.Segment.Lanes[lane1];
                            }
                            else if (this.Way.Segment.Lanes.ContainsKey(lane2))
                            {
                                lane = this.Way.Segment.Lanes[lane2];
                            }

                            // check lane exists
                            if (lane != null)
                            {
                                // determine if close and inside
                                bool close  = lane.LanePath().GetClosestPoint(position).Location.DistanceTo(position) < 15;
                                bool inside = lane.IsInside(position);

                                // add if both
                                if (close && inside)
                                {
                                    count++;
                                }
                            }
                        }
                    }

                    // return the count
                    return(count);
                }
                else
                {
                    return(0);
                }
            }
            else
            {
                return(this.NumberOfLanesRight(position, true));
            }
        }
コード例 #25
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
        }
コード例 #26
0
        /// <summary>
        /// Determine a change lanes recovery behavior
        /// </summary>
        /// <param name="current"></param>
        /// <param name="vs"></param>
        /// <param name="shouldWait"></param>
        /// <returns></returns>
        private ChangeLaneBehavior ChangeLanesRecoveryBehavior(ArbiterLane current, VehicleState vs, out bool shouldWait, out IState completionState)
        {
            // set defaults
            shouldWait = false;
            completionState = null;

            // check partition type to make sure normal
            if(current.GetClosestPartition(vs.Front).Type != PartitionType.Normal)
                return null;

            // make sure not in a safety zone
            foreach(ArbiterSafetyZone asz in current.SafetyZones)
            {
                if(asz.IsInSafety(vs.Front))
                    return null;
            }

            // check not inside an intersection safety zone
            foreach(ArbiterIntersection aInter in current.Way.Segment.RoadNetwork.ArbiterIntersections.Values)
            {
                if(aInter.IntersectionPolygon.IsInside(vs.Front) && (aInter.StoppedExits != null && aInter.StoppedExits.Count > 0))
                    return null;
            }

            // get the distance to the next lane major
            ArbiterWaypoint nextWaypoint = current.GetClosestPartition(vs.Front).Final;
            List<WaypointType> majorLaneTypes = new List<WaypointType>(new WaypointType[]{ WaypointType.End, WaypointType.Stop});
            double distanceToNextMajor = current.DistanceBetween(vs.Front, current.GetNext(nextWaypoint, majorLaneTypes, new List<ArbiterWaypoint>()).Position);

            // check distance > 3.0
            if(distanceToNextMajor >  30.0)
            {
                // check clear on right, or if it does not exist here
                ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
                bool useRight = rightLateral.Exists && rightLateral.ExistsExactlyHere(vs) && rightLateral is LateralReasoning;

                // check clear on left, or if it does not exist here
                ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
                bool useLeft = leftLateral.Exists && leftLateral.ExistsExactlyHere(vs) && leftLateral is LateralReasoning;

                // notify
                ArbiterOutput.Output("Blockage recovery: lane change left ok to use: " + useLeft.ToString() + ", use righrt: " + useRight.ToString());

                #region Test Right

                // determine if should use, should wait, or should not use
                LaneChangeMonitorResult rightLCMR = LaneChangeMonitorResult.DontUse;

                // check usability of the right lateral maneuver
                if(useRight && rightLateral is LateralReasoning)
                {
                    // check adj and rear clear
                    bool adjRearClear = rightLateral.AdjacentAndRearClear(vs);

                    // wait maybe if not clear
                    if(!adjRearClear && (rightLateral.AdjacentVehicle == null || rightLateral.AdjacentVehicle.QueuingState.Queuing != QueuingState.Failed))
                        rightLCMR = LaneChangeMonitorResult.Wait;
                    else if(adjRearClear)
                        rightLCMR = LaneChangeMonitorResult.Use;
                    else
                        rightLCMR = LaneChangeMonitorResult.DontUse;

                    // check down
                    if (rightLCMR == LaneChangeMonitorResult.Wait)
                    {
                        ArbiterOutput.Output("Blockage lane changes waiting for right lateral lane to clear");
                        shouldWait = true;
                        return null;
                    }
                    // check use
                    else if (rightLCMR == LaneChangeMonitorResult.Use)
                    {
                        // get the behavior
                        ChangeLaneBehavior rightClb = new ChangeLaneBehavior(current.LaneId, rightLateral.LateralLane.LaneId, false,
                            TahoeParams.VL * 1.5, new ScalarSpeedCommand(2.24), new List<int>(), current.LanePath(), rightLateral.LateralLane.LanePath(),
                            current.Width, rightLateral.LateralLane.Width, current.NumberOfLanesLeft(vs.Front, true), current.NumberOfLanesRight(vs.Front, true));

                        // test
                        CompletionReport rightClbCr;
                        bool successCL = CoreCommon.Communications.TestExecute(rightClb, out rightClbCr);
                        if (successCL && rightClbCr is SuccessCompletionReport)
                        {
                            ArbiterOutput.Output("Blockage lane changes found good behavior to right lateral lane");
                            shouldWait = false;
                            completionState = new StayInLaneState(rightLateral.LateralLane, CoreCommon.CorePlanningState);
                            return rightClb;
                        }
                    }
                }

                #endregion

                #region Test Left Forwards

                if(useLeft && leftLateral is LateralReasoning)
                {
                    // determine if should use, should wait, or should not use
                    LaneChangeMonitorResult leftLCMR = LaneChangeMonitorResult.DontUse;

                    // check usability of the left lateral maneuver
                    if (useLeft && leftLateral is LateralReasoning)
                    {
                        // check adj and rear clear
                        bool adjRearClear = leftLateral.AdjacentAndRearClear(vs);

                        // wait maybe if not clear
                        if (!adjRearClear && (leftLateral.AdjacentVehicle == null || leftLateral.AdjacentVehicle.QueuingState.Queuing != QueuingState.Failed))
                            leftLCMR = LaneChangeMonitorResult.Wait;
                        else if (adjRearClear)
                            leftLCMR = LaneChangeMonitorResult.Use;
                        else
                            leftLCMR = LaneChangeMonitorResult.DontUse;

                        ArbiterOutput.Output("Blockage recovery, lane change left: " + leftLCMR.ToString());

                        // check down
                        if (leftLCMR == LaneChangeMonitorResult.Wait)
                        {
                            ArbiterOutput.Output("Blockage recovery, Blockage lane changes waiting for left lateral lane to clear");
                            shouldWait = true;
                            return null;
                        }
                        // check use
                        else if (leftLCMR == LaneChangeMonitorResult.Use)
                        {
                            // get the behavior
                            ChangeLaneBehavior leftClb = new ChangeLaneBehavior(current.LaneId, leftLateral.LateralLane.LaneId, false,
                                TahoeParams.VL * 1.5, new ScalarSpeedCommand(2.24), new List<int>(), current.LanePath(), leftLateral.LateralLane.LanePath(),
                                current.Width, leftLateral.LateralLane.Width, current.NumberOfLanesLeft(vs.Front, true), current.NumberOfLanesLeft(vs.Front, true));

                            // test
                            CompletionReport leftClbCr;
                            bool successCL = CoreCommon.Communications.TestExecute(leftClb, out leftClbCr);
                            if (successCL && leftClbCr is SuccessCompletionReport)
                            {
                                ArbiterOutput.Output("Blockage recovery, Blockage lane changes found good behavior to left lateral lane");
                                completionState = new StayInLaneState(leftLateral.LateralLane, CoreCommon.CorePlanningState);
                                shouldWait = false;
                                return leftClb;
                            }
                        }
                    }
                }

                #endregion
            }

            // fallout return null
            return null;
        }
コード例 #27
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="lane"></param>
 public RearQuadrantMonitor(ArbiterLane lane, SideObstacleSide sos)
 {
     this.lane = lane;
     this.RearClearStopwatch = new Stopwatch();
     this.VehicleSide = sos;
 }
コード例 #28
0
        // Is the posterior evidence consistent over time?
        private bool Ct(List<PosteriorEvidence> e1t)
        {
            ArbiterLane lane = null;

            if (e1t.Count == 50)
            {
                // check if previous are consistent
                for (int i = 0; i < e1t.Count; i++)
                {
                    if (e1t[i].LaneProbabilities.Count > 0)
                    {
                        double max = Double.MinValue;
                        ArbiterLane curLane = null;

                        foreach (KeyValuePair<ArbiterLane, double> est in e1t[i].LaneProbabilities)
                        {
                            if (est.Value > max || curLane == null)
                            {
                                max = est.Value;
                                curLane = est.Key;
                            }
                        }

                        if (lane == null)
                            lane = curLane;
                        else
                        {
                            if (!lane.Equals(curLane))
                            {
                                return false;
                            }
                        }
                    }
                }

                if (lane == null)
                    return false;
                else
                {
                    this.sceneLikelyLane = lane;
                    CoreCommon.CurrentInformation.LASceneLikelyLane = this.sceneLikelyLane.ToString();
                    return true;
                }
            }
            else
            {
                this.sceneLikelyLane = null;
                return false;
            }
        }
コード例 #29
0
        public void SetClosestGood(VehicleState vs)
        {
            ArbiterLane current = this.OpposingLane.LaneOnLeft;
            while (current != null)
            {
                if (!current.Way.WayId.Equals(OpposingWay) && current.RelativelyInside(vs.Front))
                {
                    this.ClosestGoodLane = current;
                    break;
                }

                if (!current.Way.WayId.Equals(this.OpposingLane.Way.WayId))
                    current = current.LaneOnRight;
                else
                    current = current.LaneOnLeft;
            }
        }
コード例 #30
0
        /// <summary>
        /// Given vehicles and there locations determines if the lane adjacent to us is occupied adjacent to the vehicle
        /// </summary>
        /// <param name="lane"></param>
        /// <param name="state"></param>
        /// <returns></returns>
        public bool Occupied(ArbiterLane lane, VehicleState state)
        {
            // check stopwatch time for proper elapsed
            if (this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 10)
                this.Reset();

            if (TacticalDirector.VehicleAreas.ContainsKey(lane))
            {
                // vehicles in the lane
                List<VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[lane];

                // position of the center of our vehicle
                Coordinates center = state.Front - state.Heading.Normalize(TahoeParams.VL / 2.0);

                // cutoff for allowing vehicles outside this range
                double dCutOff = TahoeParams.VL * 1.5;

                // loop through vehicles
                foreach (VehicleAgent va in laneVehicles)
                {
                    // vehicle high level distance
                    double d = Math.Abs(lane.DistanceBetween(center, va.ClosestPosition));

                    // check less than distance cutoff
                    if (d < dCutOff)
                    {
                        ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " filled with vehicle: " + va.ToString());
                        this.CurrentVehicle = va;
                        this.Reset();
                        return true;
                    }
                }
            }

            // now check the lateral sensor for being occupied
            if (this.SideSickObstacleDetected(lane, state))
            {
                this.CurrentVehicle = new VehicleAgent(true, true);
                this.Reset();
                ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " SIDE OBSTACLE DETECTED");
                return true;
            }

            // none found
            this.CurrentVehicle = null;

            // if none found, timer not running start timer
            if (!this.LateralClearStopwatch.IsRunning)
            {
                this.CurrentVehicle = new VehicleAgent(true, true);
                this.Reset();
                this.LateralClearStopwatch.Start();
                ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
                return true;
            }
            // check enough time
            else if (this.LateralClearStopwatch.IsRunning && this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 0.5)
            {
                this.CurrentVehicle = null;
                ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
                return false;
            }
            // not enough time
            else
            {
                this.CurrentVehicle = new VehicleAgent(true, true);
                double timer = this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0;
                ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
                return true;
            }
        }
コード例 #31
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);
            }
        }
コード例 #32
0
        public bool VehiclePassableInPolygon(ArbiterLane al, VehicleAgent va, Polygon p, VehicleState ourState, Polygon circ)
        {
            Polygon vehiclePoly = va.GetAbsolutePolygon(ourState);
            vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
            List<Coordinates> pointsOutside = new List<Coordinates>();
            ArbiterLanePartition alp = al.GetClosestPartition(va.ClosestPosition);

            foreach(Coordinates c in vehiclePoly)
            {
                if (!p.IsInside(c))
                    pointsOutside.Add(c);
            }

            foreach (Coordinates m in pointsOutside)
            {
                foreach (Coordinates n in pointsOutside)
                {
                    if(!m.Equals(n))
                    {
                        if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) *
                            GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0)
                        {
                            return false;
                        }
                    }
                }
            }

            return true;
        }
コード例 #33
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
        }
コード例 #34
0
        /// <summary>
        /// Generates lane adjacency in the network
        /// </summary>
        /// <param name="arn"></param>
        private void GenerateLaneAdjacency(ArbiterRoadNetwork arn)
        {
            // loop over segments
            foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
            {
                // check if both ways exist for first algorithm
                if (asg.Way1.IsValid && asg.Way2.IsValid)
                {
                    // lanes of the segment
                    Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;

                    // get a sample lane from way 1
                    Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way1Enum = asg.Way1.Lanes.GetEnumerator();
                    way1Enum.MoveNext();
                    ArbiterLane way1Sample = way1Enum.Current.Value;

                    // get a sample lane from way 2
                    Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way2Enum = asg.Way2.Lanes.GetEnumerator();
                    way2Enum.MoveNext();
                    ArbiterLane way2Sample = way2Enum.Current.Value;

                    // direction, 1 means way1 has lower # lanes
                    int modifier = 1;

                    // check if way 2 has lower lane numbers
                    if (way1Sample.LaneId.Number > way2Sample.LaneId.Number)
                    {
                        // set modifier to -1 so count other way
                        modifier = -1;
                    }

                    // loop over lanes
                    foreach (ArbiterLane al in segLanes.Values)
                    {
                        // if not lane 1
                        if (al.LaneId.Number != 1)
                        {
                            // get lower # lane in way 1
                            ArbiterLaneId lowerNumWay1Id = new ArbiterLaneId(al.LaneId.Number - 1, asg.Way1.WayId);

                            // check if the segment contains this lane
                            if (segLanes.ContainsKey(lowerNumWay1Id))
                            {
                                // get lane
                                ArbiterLane lowerNumWay1 = segLanes[lowerNumWay1Id];

                                // check if current lane is also in way 1
                                if (lowerNumWay1.Way.WayId.Equals(al.Way.WayId))
                                {
                                    // check modifier for 1 => lower is to right
                                    if (modifier == 1)
                                    {
                                        al.LaneOnRight          = lowerNumWay1;
                                        lowerNumWay1.LaneOnLeft = al;
                                    }
                                    // otherwise -1 => lane is to left
                                    else
                                    {
                                        al.LaneOnLeft            = lowerNumWay1;
                                        lowerNumWay1.LaneOnRight = al;
                                    }
                                }
                                // otherwise the current lane is in a different way
                                else
                                {
                                    // the lane is to the left by default
                                    al.LaneOnLeft           = lowerNumWay1;
                                    lowerNumWay1.LaneOnLeft = al;
                                }
                            }
                            // otherwise the lowe lane is in way 2
                            else
                            {
                                // set lane
                                ArbiterLane lowerNumWay2 = segLanes[new ArbiterLaneId(al.LaneId.Number - 1, asg.Way2.WayId)];

                                // check if current lane is also in way 2
                                if (lowerNumWay2.Way.WayId.Equals(al.Way.WayId))
                                {
                                    // check modifier for 1 => lower is to left
                                    if (modifier == 1)
                                    {
                                        al.LaneOnLeft            = lowerNumWay2;
                                        lowerNumWay2.LaneOnRight = al;
                                    }
                                    // otherwise -1 => lane is to right
                                    else
                                    {
                                        al.LaneOnRight          = lowerNumWay2;
                                        lowerNumWay2.LaneOnLeft = al;
                                    }
                                }
                                // otherwise the current lane is in a different way
                                else
                                {
                                    // the lane is to the left by default
                                    al.LaneOnLeft           = lowerNumWay2;
                                    lowerNumWay2.LaneOnLeft = al;
                                }
                            }
                        }
                    }             // loop over lanes
                }                 // both ways valid
                // otherwise only a single way is valid
                else
                {
                    // lanes of the segment
                    Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes;

                    // make sure more than one lane exists
                    if (segLanes.Count > 1)
                    {
                        // loop over lanes
                        foreach (ArbiterLane al in segLanes.Values)
                        {
                            // get theoretical id of lane one number up
                            ArbiterLaneId ali = new ArbiterLaneId(al.LaneId.Number + 1, al.LaneId.WayId);

                            // check if lane one number up exists
                            if (segLanes.ContainsKey(ali))
                            {
                                // get lane one number up
                                ArbiterLane alu = segLanes[ali];

                                // check # waypoints
                                if (al.Waypoints.Count > 1 && alu.Waypoints.Count > 1)
                                {
                                    // get closest points on this lane and other lane
                                    PointOnPath p1;
                                    PointOnPath p2;
                                    double      distance;
                                    CreationTools.GetClosestPoints(al.PartitionPath, alu.PartitionPath, out p1, out p2, out distance);

                                    // get partition points of closest point on this lane
                                    Coordinates partitionStart = p1.segment.Start;
                                    Coordinates partitionEnd   = p1.segment.End;

                                    // get area of partition triangle
                                    double triangeArea = CreationTools.TriangleArea(partitionStart, p2.pt, partitionEnd);

                                    // determine if closest point on other lane is to the left or right of partition
                                    bool onLeft = true;
                                    if (triangeArea >= 0)
                                    {
                                        onLeft = false;
                                    }

                                    // set adjacency accordingly for both lanes
                                    if (onLeft)
                                    {
                                        al.LaneOnLeft   = alu;
                                        alu.LaneOnRight = al;
                                    }
                                    // otherwise on right
                                    else
                                    {
                                        al.LaneOnRight = alu;
                                        alu.LaneOnLeft = al;
                                    }
                                }
                            }
                        }
                    }
                }                 // end single way only valid

                // loop over lanes to print info on adjacency

                /*foreach (ArbiterLane al in asg.Lanes.Values)
                 * {
                 *      Console.Write(al.LaneId.ToString() + ": ");
                 *
                 *      if (al.LaneOnLeft != null)
                 *      {
                 *              Console.Write("Left-" + al.LaneOnLeft.LaneId.ToString());
                 *      }
                 *
                 *      if (al.LaneOnRight != null)
                 *      {
                 *              Console.Write("Right-" + al.LaneOnRight.LaneId.ToString());
                 *      }
                 *
                 *      Console.Write("\n");
                 * }
                 */
            } // segment loop
        }
コード例 #35
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);
        }
コード例 #36
0
        /// <summary>
        /// Helps with parameterizations for lateral reasoning
        /// </summary>
        /// <param name="referenceLane"></param>
        /// <param name="fqmLane"></param>
        /// <param name="goal"></param>
        /// <param name="vehicleFront"></param>
        /// <returns></returns>
        public TravelingParameters ParameterizationHelper(ArbiterLane referenceLane, ArbiterLane fqmLane,
            Coordinates goal, Coordinates vehicleFront, IState nextState, VehicleState state, VehicleAgent va)
        {
            // get traveling parameterized list
            List<TravelingParameters> tps = new List<TravelingParameters>();

            // get distance to the goal
            double goalDistance;
            double goalSpeed;
            this.StoppingParams(goal, referenceLane, vehicleFront, new double[] { }, out goalSpeed, out goalDistance);
            tps.Add(this.GetParameters(referenceLane, goalSpeed, goalDistance, state));

            // get next stop
            ArbiterWaypoint stopPoint;
            double stopSpeed;
            double stopDistance;
            StopType stopType;
            this.NextLaneStop(fqmLane, vehicleFront, new double[] { }, new List<ArbiterWaypoint>(),
                out stopPoint, out stopSpeed, out stopDistance, out stopType);
            this.StoppingParams(stopPoint.Position, referenceLane, vehicleFront, new double[] { },
                out stopSpeed, out stopDistance);
            tps.Add(this.GetParameters(referenceLane, stopSpeed, stopDistance, state));

            // get vehicle
            if (va != null)
            {
                VehicleAgent tmp = this.ForwardVehicle.CurrentVehicle;
                double tmpDist = this.ForwardVehicle.currentDistance;
                this.ForwardVehicle.CurrentVehicle = va;
                this.ForwardVehicle.currentDistance = referenceLane.DistanceBetween(state.Front, va.ClosestPosition);
                TravelingParameters tp = this.ForwardVehicle.Follow(referenceLane, state, new List<ArbiterWaypoint>());
                tps.Add(tp);
                this.ForwardVehicle.CurrentVehicle = tmp;
                this.ForwardVehicle.currentDistance = tmpDist;
            }

            // parameterize
            tps.Sort();
            return tps[0];
        }
コード例 #37
0
        /// <summary>
        /// Generates the xySegments into segments and inputs them into the input road network
        /// </summary>
        /// <param name="arn"></param>
        /// <returns></returns>
        public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn)
        {
            foreach (SimpleSegment ss in segments)
            {
                // seg
                ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id));
                ArbiterSegment   asg = new ArbiterSegment(asi);
                arn.ArbiterSegments.Add(asi, asg);
                asg.RoadNetwork = arn;
                asg.SpeedLimits = new ArbiterSpeedLimit();
                asg.SpeedLimits.MaximumSpeed = 13.4112;                 // 30mph max speed

                // way1
                ArbiterWayId awi1 = new ArbiterWayId(1, asi);
                ArbiterWay   aw1  = new ArbiterWay(awi1);
                aw1.Segment = asg;
                asg.Ways.Add(awi1, aw1);
                asg.Way1 = aw1;

                // way2
                ArbiterWayId awi2 = new ArbiterWayId(2, asi);
                ArbiterWay   aw2  = new ArbiterWay(awi2);
                aw2.Segment = asg;
                asg.Ways.Add(awi2, aw2);
                asg.Way2 = aw2;

                // make lanes
                foreach (SimpleLane sl in ss.Lanes)
                {
                    // lane
                    ArbiterLaneId ali;
                    ArbiterLane   al;

                    // get way of lane id
                    if (ss.Way1Lanes.Contains(sl))
                    {
                        ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1);
                        al  = new ArbiterLane(ali);
                        aw1.Lanes.Add(ali, al);
                        al.Way = aw1;
                    }
                    else
                    {
                        ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2);
                        al  = new ArbiterLane(ali);
                        aw2.Lanes.Add(ali, al);
                        al.Way = aw2;
                    }

                    // add to display
                    arn.DisplayObjects.Add(al);

                    // width
                    al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048;

                    if (sl.LaneWidth == 0)
                    {
                        Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m");
                    }

                    // lane boundaries
                    al.BoundaryLeft  = this.GenerateLaneBoundary(sl.LeftBound);
                    al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound);

                    // add lane to seg
                    asg.Lanes.Add(ali, al);

                    // waypoints
                    List <ArbiterWaypoint> waypointList = new List <ArbiterWaypoint>();

                    // generate waypoints
                    foreach (SimpleWaypoint sw in sl.Waypoints)
                    {
                        // waypoint
                        ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali);
                        ArbiterWaypoint   aw  = new ArbiterWaypoint(sw.Position, awi);
                        aw.Lane = al;

                        // stop
                        if (sl.Stops.Contains(sw.ID))
                        {
                            aw.IsStop = true;
                        }

                        // checkpoint
                        foreach (SimpleCheckpoint sc in sl.Checkpoints)
                        {
                            if (sw.ID == sc.WaypointId)
                            {
                                aw.IsCheckpoint = true;
                                aw.CheckpointId = int.Parse(sc.CheckpointId);
                                arn.Checkpoints.Add(aw.CheckpointId, aw);
                            }
                        }

                        // add
                        asg.Waypoints.Add(awi, aw);
                        arn.ArbiterWaypoints.Add(awi, aw);
                        al.Waypoints.Add(awi, aw);
                        waypointList.Add(aw);
                        arn.DisplayObjects.Add(aw);
                        arn.LegacyWaypointLookup.Add(sw.ID, aw);
                    }

                    al.WaypointList = waypointList;

                    // lane partitions
                    List <ArbiterLanePartition> alps = new List <ArbiterLanePartition>();
                    al.Partitions = alps;

                    // generate lane partitions
                    for (int i = 0; i < waypointList.Count - 1; i++)
                    {
                        // create lane partition
                        ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali);
                        ArbiterLanePartition   alp  = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i + 1], asg);
                        alp.Lane = al;
                        waypointList[i].NextPartition         = alp;
                        waypointList[i + 1].PreviousPartition = alp;
                        alps.Add(alp);
                        arn.DisplayObjects.Add(alp);

                        // crete initial user partition
                        ArbiterUserPartitionId      aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId);
                        ArbiterUserPartition        aup  = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]);
                        List <ArbiterUserPartition> aups = new List <ArbiterUserPartition>();
                        aups.Add(aup);
                        alp.UserPartitions = aups;
                        alp.SetDefaultSparsePolygon();
                        arn.DisplayObjects.Add(aup);
                    }

                    // path segments of lane
                    List <IPathSegment> ips          = new List <IPathSegment>();
                    List <Coordinates>  pathSegments = new List <Coordinates>();
                    pathSegments.Add(alps[0].Initial.Position);

                    // loop
                    foreach (ArbiterLanePartition alPar in alps)
                    {
                        ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
                        // make new segment
                        pathSegments.Add(alPar.Final.Position);
                    }

                    // generate lane partition path
                    LinePath partitionPath = new LinePath(pathSegments);
                    al.SetLanePath(partitionPath);
                    al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected);

                    // safeto zones
                    foreach (ArbiterWaypoint aw in al.Waypoints.Values)
                    {
                        if (aw.IsStop)
                        {
                            LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
                            double dist = -30;
                            LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
                            if (dist != 0)
                            {
                                begin = al.LanePath().StartPoint;
                            }
                            ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
                            asz.isExit = true;
                            asz.Exit   = aw;
                            al.SafetyZones.Add(asz);
                            arn.DisplayObjects.Add(asz);
                            arn.ArbiterSafetyZones.Add(asz);
                        }
                    }
                }
            }

            return(arn);
        }