Exemplo n.º 1
0
        /// <summary>
        /// Pupulates all the priority monitors given the information we know right at this moment
        /// </summary>
        public void PopulateMonitor(IArbiterWaypoint ourExit)
        {
            // 1. Generate dependencies for each lane

            // 2. Assign stopped vehicles at stops to stop lanes

            // 3. Given dependencies populate the stop queues for each stopped vehicle

            // 4. Assign stopped vehicles close to exits in priority lanes to their priority monitor

            // 5. given our exit assign our dependencies
        }
        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="initial"></param>
        /// <param name="final"></param>
        public ArbiterInterconnect(IArbiterWaypoint initial, IArbiterWaypoint final) :
            base(false, null, false, null, new List <IConnectAreaWaypoints>(), initial, final)
        {
            this.initialWaypoint = initial;
            this.finalWaypoint   = final;

            this.InterconnectId = new ArbiterInterconnectId(initialWaypoint.AreaSubtypeWaypointId, finalWaypoint.AreaSubtypeWaypointId);

            // create a path of the partition and get the closest
            List <Coordinates> ips = new List <Coordinates>();

            ips.Add(initial.Position);
            ips.Add(final.Position);
            this.InterconnectPath = new LinePath(ips);

            // nav edge stuff
            this.Contained.Add(this);
            this.blockage = new NavigationBlockage(0.0);

            this.TurnPolygon      = this.DefaultPoly();
            this.InnerCoordinates = new List <Coordinates>();
        }
        /// <summary>
        /// Get a road plan while setting partition costs very high
        /// </summary>
        /// <param name="partition"></param>
        /// <param name="goal"></param>
        /// <param name="blockAdjacent"></param>
        /// <param name="c"></param>
        /// <returns></returns>
        public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay)
        {
            KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;

            this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
            RoadPlan rp = null;

            if (!blockAdjacent)
            {
                NavigationBlockage nb  = partition.Blockage;
                NavigationBlockage tmp = new NavigationBlockage(double.MaxValue);
                partition.Blockage = tmp;
                rp = this.PlanNavigableArea(partition.Lane, c, goal, new List <ArbiterWaypoint>());
                partition.Blockage = nb;
            }
            else
            {
                // save
                List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> > savedBlockages = new List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> >();

                // set
                savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(partition, partition.Blockage));
                // create new
                NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue);
                anewerBlockage.BlockageExists = true;
                partition.Blockage            = anewerBlockage;

                foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions)
                {
                    if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way))))
                    {
                        savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(alp, alp.Blockage));

                        // create new
                        NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
                        newerBlockage.BlockageExists = true;
                        alp.Blockage = newerBlockage;
                    }
                }

                // plan
                rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List <ArbiterWaypoint>());

                // restore
                foreach (KeyValuePair <ArbiterLanePartition, NavigationBlockage> saved in savedBlockages)
                {
                    saved.Key.Blockage = saved.Value;
                }
            }

            // restore
            this.currentTimes = tmpCurrentTimes;

            // return
            return(rp);
        }
        /// <summary>
        /// Try to plan the intersection heavily penalizing the interconnect
        /// </summary>
        /// <param name="iTraversableWaypoint"></param>
        /// <param name="iArbiterWaypoint"></param>
        /// <param name="arbiterInterconnect"></param>
        /// <returns></returns>
        public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect, bool blockAllRelated)
        {
            ITraversableWaypoint entry = (ITraversableWaypoint)interconnect.FinalGeneric;

            if (!blockAllRelated)
            {
                return(this.PlanIntersectionWithoutInterconnect(exit, goal, interconnect));
            }
            else
            {
                Dictionary <IConnectAreaWaypoints, NavigationBlockage> saved = new Dictionary <IConnectAreaWaypoints, NavigationBlockage>();
                if (entry.IsEntry)
                {
                    foreach (ArbiterInterconnect ai in entry.Entries)
                    {
                        saved.Add(ai, ai.Blockage);

                        // create new
                        NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
                        newerBlockage.BlockageExists = true;
                        ai.Blockage = newerBlockage;
                    }
                }

                if (entry is ArbiterWaypoint && ((ArbiterWaypoint)entry).PreviousPartition != null)
                {
                    ArbiterLanePartition alp = ((ArbiterWaypoint)entry).PreviousPartition;
                    saved.Add(alp, alp.Blockage);

                    // create new
                    NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
                    newerBlockage.BlockageExists = true;
                    alp.Blockage = newerBlockage;
                }

                KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;
                this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();

                // plan
                IntersectionPlan ip = this.PlanIntersection(exit, goal);

                this.currentTimes = tmpCurrentTimes;

                // reset the blockages
                foreach (KeyValuePair <IConnectAreaWaypoints, NavigationBlockage> savedPair in saved)
                {
                    savedPair.Key.Blockage = savedPair.Value;
                }

                // return plan
                return(ip);
            }
        }
        /// <summary>
        /// Try to plan the intersection heavily penalizing the interconnect
        /// </summary>
        /// <param name="iTraversableWaypoint"></param>
        /// <param name="iArbiterWaypoint"></param>
        /// <param name="arbiterInterconnect"></param>
        /// <returns></returns>
        public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect)
        {
            // save old blockage
            NavigationBlockage tmpBlockage = interconnect.Blockage;

            // create new
            NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);

            newerBlockage.BlockageExists = true;
            interconnect.Blockage        = newerBlockage;

            KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;

            this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();

            // plan
            IntersectionPlan ip = this.PlanIntersection(exit, goal);

            this.currentTimes = tmpCurrentTimes;

            // reset interconnect blockage
            interconnect.Blockage = tmpBlockage;

            // return plan
            return(ip);
        }
        public static INavigableNode FilterGoal(VehicleState state)
        {
            // get goal
            INavigableNode goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
                                  CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;

            if (waitRemoveLastGoal && CoreCommon.Mission.MissionCheckpoints.Count != 1)
            {
                waitRemoveLastGoal = false;
            }

            // id
            IArbiterWaypoint goalWp = null;

            if (goal != null)
            {
                goalWp = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId];
            }

            // check lane change or opposing
            if (goal != null &&
                (CoreCommon.CorePlanningState is OpposingLanesState && ((OpposingLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId)) ||
                (CoreCommon.CorePlanningState is ChangeLanesState && ((ChangeLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId)))
            {
                if (CoreCommon.Mission.MissionCheckpoints.Count == 1)
                {
                    waitRemoveLastGoal = true;
                    ArbiterOutput.Output("Waiting to remove last Checkpoint: " + goal.ToString());
                }
                else
                {
                    // set hit
                    ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString());
                    CoreCommon.Mission.MissionCheckpoints.Dequeue();

                    // update goal
                    goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
                           CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
                }
            }
            else if (goal != null && CoreCommon.Mission.MissionCheckpoints.Count == 1 && waitRemoveLastGoal &&
                     (CoreCommon.CorePlanningState is StayInLaneState || CoreCommon.CorePlanningState is StayInSupraLaneState))
            {
                // set hit
                ArbiterOutput.Output("Wait over, Reached Checkpoint: " + goal.ToString());
                CoreCommon.Mission.MissionCheckpoints.Dequeue();

                // update goal
                goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
                       CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
            }
            // TODO implement full version of hit test
            // check if we have hit the goal (either by being in opposing lane or going to opposing and next to it or in lane and pass over it
            else if (goal != null)
            {
                bool reachedCp = false;

                if (CoreCommon.CorePlanningState is StayInLaneState)
                {
                    StayInLaneState sils = (StayInLaneState)CoreCommon.CorePlanningState;
                    if (goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(sils.Lane))
                    {
                        if (CoreCommon.Mission.MissionCheckpoints.Count != 1)
                        {
                            double distanceAlong = sils.Lane.DistanceBetween(state.Front, goal.Position);
                            if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0)
                            {
                                reachedCp = true;
                            }
                        }
                        else
                        {
                            double distanceAlong  = sils.Lane.DistanceBetween(state.Front, goal.Position);
                            double distanceAlong2 = sils.Lane.DistanceBetween(state.Position, goal.Position);
                            if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.005 && Math.Abs(distanceAlong) < 0.3 ||
                                CoreCommon.Communications.GetVehicleState().VehiclePolygon.IsInside(goal.Position) ||
                                (distanceAlong <= 0.0 && distanceAlong2 >= 0))
                            {
                                reachedCp = true;
                            }
                        }
                    }
                }
                else if (CoreCommon.CorePlanningState is ChangeLanesState)
                {
                    ChangeLanesState cls = (ChangeLanesState)CoreCommon.CorePlanningState;
                    if (cls.Parameters.Initial.Way.Equals(cls.Parameters.Target.Way) &&
                        goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(cls.Parameters.Target))
                    {
                        double distanceAlong = cls.Parameters.Target.DistanceBetween(state.Front, goal.Position);
                        if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0)
                        {
                            reachedCp = true;
                            ArbiterOutput.Output("Removed goal changing lanes");
                        }
                    }
                }

                if (reachedCp)
                {
                    // set hit
                    ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString());
                    CoreCommon.Mission.MissionCheckpoints.Dequeue();

                    // update goal
                    goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
                           CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
                }
            }

            // set goal info
            CoreCommon.CurrentInformation.RouteCheckpoint   = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? goal.ToString() : "NONE";
            CoreCommon.CurrentInformation.GoalsRemaining    = CoreCommon.Mission.MissionCheckpoints.Count.ToString();
            CoreCommon.CurrentInformation.RouteCheckpointId = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() : "NONE";

            // return current
            return(goal);
        }
Exemplo n.º 7
0
        /// <summary>
        /// Generates interconnects into the road network
        /// </summary>
        /// <param name="arn"></param>
        /// <returns></returns>
        public ArbiterRoadNetwork GenerateInterconnects(ArbiterRoadNetwork arn)
        {
            // list of all exit entries in the xy rndf
            List <SimpleExitEntry> sees = new List <SimpleExitEntry>();

            // zones
            if (xyRndf.Zones != null)
            {
                // loop over zones
                foreach (SimpleZone sz in xyRndf.Zones)
                {
                    // add all ee's
                    sees.AddRange(sz.Perimeter.ExitEntries);
                }
            }

            // segments
            if (xyRndf.Segments != null)
            {
                // loop over segments
                foreach (SimpleSegment ss in xyRndf.Segments)
                {
                    // lanes
                    foreach (SimpleLane sl in ss.Lanes)
                    {
                        // add all ee's
                        sees.AddRange(sl.ExitEntries);
                    }
                }
            }

            // loop over ee's and create interconnects
            foreach (SimpleExitEntry see in sees)
            {
                IArbiterWaypoint    initial = arn.LegacyWaypointLookup[see.ExitId];
                IArbiterWaypoint    final   = arn.LegacyWaypointLookup[see.EntryId];
                ArbiterInterconnect ai      = new ArbiterInterconnect(initial, final);
                arn.ArbiterInterconnects.Add(ai.InterconnectId, ai);
                arn.DisplayObjects.Add(ai);

                if (initial is ITraversableWaypoint)
                {
                    ITraversableWaypoint initialWaypoint = (ITraversableWaypoint)initial;

                    initialWaypoint.IsExit = true;

                    if (initialWaypoint.Exits == null)
                    {
                        initialWaypoint.Exits = new List <ArbiterInterconnect>();
                    }

                    initialWaypoint.Exits.Add(ai);
                }
                else
                {
                    throw new Exception("initial wp of ee: " + see.ExitId + " is not ITraversableWaypoint");
                }

                if (final is ITraversableWaypoint)
                {
                    ITraversableWaypoint finalWaypoint = (ITraversableWaypoint)final;

                    finalWaypoint.IsEntry = true;

                    if (finalWaypoint.Entries == null)
                    {
                        finalWaypoint.Entries = new List <ArbiterInterconnect>();
                    }

                    finalWaypoint.Entries.Add(ai);
                }
                else
                {
                    throw new Exception("final wp of ee: " + see.EntryId + " is not ITraversableWaypoint");
                }

                // set the turn direction
                this.SetTurnDirection(ai);

                // interconnectp olygon stuff
                this.GenerateInterconnectPolygon(ai);
                if (ai.TurnPolygon.IsComplex)
                {
                    Console.WriteLine("Found complex polygon for interconnect: " + ai.ToString());
                    ai.TurnPolygon = ai.DefaultPoly();
                }
            }

            return(arn);
        }
        /// <summary>
        /// Pupulates all the priority monitors given the information we know right at this moment
        /// </summary>
        public void PopulateMonitor(IArbiterWaypoint ourExit)
        {
            // 1. Generate dependencies for each lane

            // 2. Assign stopped vehicles at stops to stop lanes

            // 3. Given dependencies populate the stop queues for each stopped vehicle

            // 4. Assign stopped vehicles close to exits in priority lanes to their priority monitor

            // 5. given our exit assign our dependencies
        }
        /// <summary>
        /// Get a road plan while setting partition costs very high
        /// </summary>
        /// <param name="partition"></param>
        /// <param name="goal"></param>
        /// <param name="blockAdjacent"></param>
        /// <param name="c"></param>
        /// <returns></returns>
        public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay)
        {
            KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes;
            this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>();
            RoadPlan rp = null;

            if (!blockAdjacent)
            {
                NavigationBlockage nb = partition.Blockage;
                NavigationBlockage tmp = new NavigationBlockage(double.MaxValue);
                partition.Blockage = tmp;
                rp = this.PlanNavigableArea(partition.Lane, c, goal, new List<ArbiterWaypoint>());
                partition.Blockage = nb;
            }
            else
            {
                // save
                List<KeyValuePair<ArbiterLanePartition, NavigationBlockage>> savedBlockages = new List<KeyValuePair<ArbiterLanePartition, NavigationBlockage>>();

                // set
                savedBlockages.Add(new KeyValuePair<ArbiterLanePartition,NavigationBlockage>(partition, partition.Blockage));
                // create new
                NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue);
                anewerBlockage.BlockageExists = true;
                partition.Blockage = anewerBlockage;

                foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions)
                {
                    if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way))))
                    {
                        savedBlockages.Add(new KeyValuePair<ArbiterLanePartition,NavigationBlockage>(alp, alp.Blockage));

                        // create new
                        NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
                        newerBlockage.BlockageExists = true;
                        alp.Blockage = newerBlockage;
                    }
                }

                // plan
                rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List<ArbiterWaypoint>());

                // restore
                foreach (KeyValuePair<ArbiterLanePartition, NavigationBlockage> saved in savedBlockages)
                {
                    saved.Key.Blockage = saved.Value;
                }
            }

            // restore
            this.currentTimes = tmpCurrentTimes;

            // return
            return rp;
        }
        /// <summary>
        /// Try to plan the intersection heavily penalizing the interconnect
        /// </summary>
        /// <param name="iTraversableWaypoint"></param>
        /// <param name="iArbiterWaypoint"></param>
        /// <param name="arbiterInterconnect"></param>
        /// <returns></returns>
        public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect, bool blockAllRelated)
        {
            ITraversableWaypoint entry = (ITraversableWaypoint)interconnect.FinalGeneric;

            if (!blockAllRelated)
                return this.PlanIntersectionWithoutInterconnect(exit, goal, interconnect);
            else
            {
                Dictionary<IConnectAreaWaypoints, NavigationBlockage> saved = new Dictionary<IConnectAreaWaypoints, NavigationBlockage>();
                if (entry.IsEntry)
                {
                    foreach (ArbiterInterconnect ai in entry.Entries)
                    {
                        saved.Add(ai, ai.Blockage);

                        // create new
                        NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
                        newerBlockage.BlockageExists = true;
                        ai.Blockage = newerBlockage;
                    }
                }

                if (entry is ArbiterWaypoint && ((ArbiterWaypoint)entry).PreviousPartition != null)
                {
                    ArbiterLanePartition alp = ((ArbiterWaypoint)entry).PreviousPartition;
                    saved.Add(alp, alp.Blockage);

                    // create new
                    NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
                    newerBlockage.BlockageExists = true;
                    alp.Blockage = newerBlockage;
                }

                KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes;
                this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>();

                // plan
                IntersectionPlan ip = this.PlanIntersection(exit, goal);

                this.currentTimes = tmpCurrentTimes;

                // reset the blockages
                foreach (KeyValuePair<IConnectAreaWaypoints, NavigationBlockage> savedPair in saved)
                    savedPair.Key.Blockage = savedPair.Value;

                // return plan
                return ip;
            }
        }
        /// <summary>
        /// Try to plan the intersection heavily penalizing the interconnect
        /// </summary>
        /// <param name="iTraversableWaypoint"></param>
        /// <param name="iArbiterWaypoint"></param>
        /// <param name="arbiterInterconnect"></param>
        /// <returns></returns>
        public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect)
        {
            // save old blockage
            NavigationBlockage tmpBlockage = interconnect.Blockage;

            // create new
            NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
            newerBlockage.BlockageExists = true;
            interconnect.Blockage = newerBlockage;

            KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes;
            this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>();

            // plan
            IntersectionPlan ip = this.PlanIntersection(exit, goal);

            this.currentTimes = tmpCurrentTimes;

            // reset interconnect blockage
            interconnect.Blockage = tmpBlockage;

            // return plan
            return ip;
        }
Exemplo n.º 12
0
        /// <summary>
        /// Gets priotiy lane determination
        /// </summary>
        /// <param name="ii"></param>
        /// <param name="path"></param>
        /// <param name="end"></param>
        /// <returns></returns>
        private Coordinates?LaneIntersectsPath(IntersectionInvolved ii, LinePath path, IArbiterWaypoint end)
        {
            ArbiterLane al = (ArbiterLane)ii.Area;

            LinePath.PointOnPath current = path.StartPoint;
            bool go = true;

            while (go)            // && !(current.Location.DistanceTo(path.EndPoint.Location) < 0.1))
            {
                Coordinates alClose = al.LanePath().GetClosestPoint(current.Location).Location;
                double      alDist  = alClose.DistanceTo(current.Location);
                if (alDist <= 0.05)
                {
                    return(al.LanePath().GetClosestPoint(current.Location).Location);
                }

                if (current.Location.Equals(path.EndPoint.Location))
                {
                    go = false;
                }

                current = path.AdvancePoint(current, 0.1);
            }

            /*if (ii.Exit != null)
             * {
             *      ITraversableWaypoint laneExit = ii.Exit;
             *      foreach (ArbiterInterconnect tmpAi in laneExit.OutgoingConnections)
             *      {
             *              if (tmpAi.FinalGeneric.Equals(end))
             *              {
             *                      return tmpAi.FinalGeneric.Position;
             *              }
             *      }
             * }*/

            return(null);
        }