示例#1
0
    private ReverseResumableAStar getRRA(Cell startCell, Cell destinationCell)
    {
        // if the RRA destination cell is too far from the current cell (8 cells), then it is restarted

        if (rraDict.ContainsKey(destinationCell))
        {
            var rra = rraDict[destinationCell];
            if (rra.HasCell(startCell) || Cell.ManhanttanDistance(rra.DestinationCell, startCell) <= 8)
            {
                return(rra);
            }
        }

        var newRRA = new ReverseResumableAStar(destinationCell, startCell);

        rraDict[destinationCell] = newRRA;
        return(newRRA);
    }
示例#2
0
        /// <summary>
        /// Solves the specified node.
        /// </summary>
        /// <param name="node">The node.</param>
        /// <param name="currentTime">The current time.</param>
        /// <param name="agent">The agent.</param>
        /// <param name="obstacleNodes">The obstacle nodes.</param>
        /// <param name="lockedNodes">The locked nodes.</param>
        /// <returns></returns>
        private bool Solve(ConflictTree.Node node, double currentTime, Agent agent)
        {
            //clear reservation table
            _reservationTable.Clear();

            //add constraints (agentId = -1 => Intervals from the tree)
            foreach (var constraint in node.getConstraints(agent.ID))
            {
                _reservationTable.Add(constraint.IntervalConstraint);
            }

            //drive to next node must be possible - otherwise it is not a valid node
            foreach (var reservation in agent.ReservationsToNextNode)
            {
                if (!_reservationTable.IntersectionFree(reservation))
                {
                    return(false);
                }
            }

            //We can use WHCA Star here in a low level approach.
            //Window = Infinitively long
            var rraStar = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
            var aStar   = new SpaceTimeAStar(Graph, LengthOfAWaitStep, double.PositiveInfinity, _reservationTable, agent, rraStar);

            //execute
            var found = aStar.Search();

            //+ WHCA* Nodes
            List <ReservationTable.Interval> reservations;
            Path path = new Path();

            if (found)
            {
                //add all WHCA Nodes
                aStar.GetPathAndReservations(ref path, out reservations);

#if DEBUG
                foreach (var reservation in reservations)
                {
                    Debug.Assert(_reservationTable.IntersectionFree(reservation));
                }
#endif

                //add the next node again
                if (path.Count == 0 || path.NextAction.Node != agent.NextNode || path.NextAction.StopAtNode == false)
                {
                    path.AddFirst(agent.NextNode, true, 0);
                }

                node.setSolution(agent.ID, path, reservations);

                //found
                return(true);
            }
            else
            {
                //not found
                return(false);
            }
        }
示例#3
0
 /// <summary>
 /// Initializes a new instance of the <see cref="SpaceTimeAStar"/> class.
 /// </summary>
 public SpaceTimeAStarSteper(Graph graph, double lengthOfAWaitStep, Agent agent, ReverseResumableAStar rraStar)
     : base(graph, lengthOfAWaitStep, 0, null, agent, rraStar, false)
 {
     //we don't need the AStar Base things
     Q      = null;
     Open   = null;
     Closed = null;
 }
示例#4
0
 /// <summary>
 /// Initializes a new instance of the <see cref="PASActionGenerator"/> class.
 /// </summary>
 /// <param name="graph">The graph.</param>
 /// <param name="lengthOfAWaitStep">The length of a wait step.</param>
 /// <param name="agent">The agent.</param>
 /// <param name="obstacles">The obstacles.</param>
 /// <param name="rraStar">The rra star.</param>
 public PASActionGenerator(Graph graph, double lengthOfAWaitStep, Agent agent, ReverseResumableAStar rraStar)
     : base(graph, lengthOfAWaitStep, double.PositiveInfinity, null, agent, rraStar)
 {
 }
示例#5
0
        /// <summary>
        /// Find the path for all the agents.
        /// </summary>
        /// <param name="currentTime">The current time.</param>
        /// <param name="agents">agents</param>
        /// <param name="obstacleNodes">The way points of the obstacles.</param>
        /// <param name="lockedNodes">The locked nodes.</param>
        /// <param name="nextReoptimization">The next re-optimization time.</param>
        /// <param name="runtimeLimit">The runtime limit.</param>
        public override void FindPaths(double currentTime, List <Agent> agents)
        {
            Stopwatch.Restart();

            //Reservation Table
            _reservationTable.Clear();
            var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);

            //panic times initialization
            foreach (var agent in agents)
            {
                if (!_waitTime.ContainsKey(agent.ID))
                {
                    _waitTime.Add(agent.ID, double.NegativeInfinity);
                }
                if (!_moveTime.ContainsKey(agent.ID))
                {
                    _moveTime.Add(agent.ID, currentTime);
                }
                if (!_es2evadedFrom.ContainsKey(agent.ID))
                {
                    _es2evadedFrom.Add(agent.ID, new HashSet <int>());
                }
            }

            //set fixed blockage
            try
            {
                foreach (var agent in agents)
                {
                    //all intervals from now to the moment of stop
                    foreach (var interval in fixedBlockage[agent])
                    {
                        _reservationTable.Add(interval, agent.ID);
                    }

                    //reservation of next node
                    int collisionAgent;
                    if (_reservationTable.IntersectionFree(agent.NextNode, agent.ArrivalTimeAtNextNode, double.PositiveInfinity, out collisionAgent))
                    {
                        _reservationTable.Add(agent.NextNode, agent.ArrivalTimeAtNextNode, double.PositiveInfinity, agent.ID);
                    }
                    else
                    {
                        Debug.Assert(collisionAgent == agent.ID);
                    }
                }
            }
            catch (RAWSimO.MultiAgentPathFinding.DataStructures.DisjointIntervalTree.IntervalIntersectionException) { }

            //deadlock handling
            if (UseDeadlockHandler)
            {
                _deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
                _deadlockHandler.MaximumWaitTime   = 30;
                _deadlockHandler.Update(agents, currentTime);
            }

            //optimize Path
            bool       deadlockBreakingManeuver;
            var        reservationOwnerAgentId = -1;
            int        reservationOwnerNodeId  = -1;
            List <int> nextHopNodes;

            foreach (Agent a in agents)
            {
                if (a.Path != null && !a.Path.IsConsistent)
                {
                    throw new Exception("fs ex");
                }
            }

            foreach (var agent in agents.Where(a => !a.FixedPosition && a.NextNode != a.DestinationNode && a.ArrivalTimeAtNextNode <= currentTime && a.RequestReoptimization))
            {
                //runtime exceeded
                if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
                {
                    Communicator.SignalTimeout();
                    return;
                }

                deadlockBreakingManeuver = false;

                //agent is allowed to move?
                if (currentTime < _waitTime[agent.ID])
                {
                    continue;
                }

                //remove blocking next node
                _reservationTable.RemoveIntersectionWithTime(agent.NextNode, agent.ArrivalTimeAtNextNode);

                //Create RRA* search if necessary.
                //Necessary if the agent has none or the agents destination has changed
                ReverseResumableAStar rraStar;
                if (!_rraStar.TryGetValue(agent.ID, out rraStar) || rraStar == null || rraStar.StartNode != agent.DestinationNode)
                {
                    //new search
                    rraStar             = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet <int>());
                    _rraStar[agent.ID]  = rraStar;
                    _moveTime[agent.ID] = currentTime;
                }

                //already found in RRA*?
                var found = rraStar.Closed.Contains(agent.NextNode);

                //If the search is old, the path may me blocked now
                if (found && rraStar.PathContains(agent.NextNode))
                {
                    //new search
                    rraStar            = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet <int>());
                    _rraStar[agent.ID] = rraStar;
                    found = false;
                }

                //Is a search processing necessary
                if (!found)
                {
                    found = rraStar.Search(agent.NextNode);
                }

                //the search ended with no result => just wait a moment
                if (!found)
                {
                    //new search
                    _rraStar[agent.ID] = null;

                    //still not found? Then wait!
                    if (_waitTime[agent.ID] < currentTime - LengthOfAWaitStep * 2f)
                    {
                        waitStep(agent, agent.ID, currentTime);
                        continue;
                    }
                    else
                    {
                        deadlockBreakingManeuver = true;
                    }
                }

                if (!deadlockBreakingManeuver)
                {
                    //set the next step of the path
                    nextHopNodes = _getNextHopNodes(currentTime, agent, out reservationOwnerAgentId, out reservationOwnerNodeId);

                    //avoid going back
                    if (Es2BackEvadingAvoidance && nextHopNodes.Count > 1 && _es2evadedFrom[agent.ID].Contains(nextHopNodes[1]))
                    {
                        deadlockBreakingManeuver = true;
                    }
                    else if (Es2BackEvadingAvoidance)
                    {
                        _es2evadedFrom[agent.ID].Clear();
                    }

                    //found a path => set it
                    if (!deadlockBreakingManeuver && nextHopNodes.Count > 1)
                    {
                        _setPath(nextHopNodes, agent);
                        _moveTime[agent.ID] = currentTime;
                        continue;
                    }
                }

                //deadlock breaking maneuver due to wait time
                if (!deadlockBreakingManeuver)
                {
                    deadlockBreakingManeuver = currentTime - _moveTime[agent.ID] > MaximumWaitTime;
                }

                //deadlock breaking maneuver due to wait for relation circle
                if (!deadlockBreakingManeuver)
                {
                    //transitive closure of wait for relation
                    HashSet <int> waitForSet = new HashSet <int>();
                    var           waitForID  = agent.ID;
                    while (!deadlockBreakingManeuver && _waitFor.ContainsKey(waitForID))
                    {
                        if (waitForSet.Contains(waitForID))
                        {
                            deadlockBreakingManeuver = true;
                        }
                        waitForSet.Add(waitForID);
                        waitForID = _waitFor[waitForID];
                    }
                }


                if (!deadlockBreakingManeuver)
                {
                    //wait a little while
                    waitStep(agent, reservationOwnerAgentId, currentTime);
                    continue;
                }
                else
                {
                    //deadlock breaking maneuver must be done!
                    switch (_evadingStragety)
                    {
                    case EvadingStrategy.EvadeByRerouting:

                        //obstacle free
                        if (!found)
                        {
                            rraStar = _rraStar[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode, new HashSet <int>());
                        }


                        for (int tries = 1; tries <= Es1MaximumNumberOfBreakingManeuverTries; tries++)
                        {
                            //deadlock breaking maneuver must be done!
                            if (tries >= Es1MaximumNumberOfBreakingManeuverTries)
                            {
                                //wait a little while => for myself
                                waitStep(agent, agent.ID, currentTime);
                                break;
                            }

                            //The agent can not move
                            _waitFor[agent.ID] = reservationOwnerAgentId;

                            //add an obstacle and restart the search
                            rraStar.AddCustomLock(reservationOwnerNodeId);
                            rraStar.Clear(agent.DestinationNode, agent.NextNode);
                            found = rraStar.Search(agent.NextNode);

                            //found => get hop nodes
                            if (!found)
                            {
                                //wait a little while => for myself
                                waitStep(agent, agent.ID, currentTime);
                                break;
                            }
                            else
                            {
                                nextHopNodes = _getNextHopNodes(currentTime, agent, out reservationOwnerAgentId, out reservationOwnerNodeId);

                                if (nextHopNodes.Count > 1)
                                {
                                    _setPath(nextHopNodes, agent);
                                    break;
                                }
                            }
                        }
                        break;

                    case EvadingStrategy.EvadeToNextNode:

                        //try to find a free hop
                        var foundBreakingManeuverEdge = false;
                        var possibleEdges             = new List <Edge>(Graph.Edges[agent.NextNode]);
                        shuffle <Edge>(possibleEdges, Randomizer);
                        foreach (var edge in possibleEdges.Where(e => !e.ToNodeInfo.IsLocked && (agent.CanGoThroughObstacles || !e.ToNodeInfo.IsObstacle) && !_es2evadedFrom[agent.ID].Contains(e.To)))
                        {
                            //create intervals
                            var intervals = _reservationTable.CreateIntervals(currentTime, currentTime, 0, agent.Physics, agent.NextNode, edge.To, true);
                            reservationOwnerNodeId  = -1;
                            reservationOwnerAgentId = -1;

                            //check if a reservation is possible
                            if (_reservationTable.IntersectionFree(intervals, out reservationOwnerNodeId, out reservationOwnerAgentId))
                            {
                                foundBreakingManeuverEdge = true;

                                //avoid going back
                                if (this.Es2BackEvadingAvoidance)
                                {
                                    _es2evadedFrom[agent.ID].Add(edge.To);
                                }

                                //create a path
                                agent.Path.Clear();
                                agent.Path.AddLast(edge.To, true, LengthOfAWaitStep);

                                break;
                            }
                        }

                        if (!foundBreakingManeuverEdge)
                        {
                            //Clear the nodes
                            _es2evadedFrom[agent.ID].Clear();

                            //just wait
                            waitStep(agent, agent.ID, currentTime);
                        }

                        break;
                    }
                }

                foreach (Agent a in agents)
                {
                    if (a.Path != null && !a.Path.IsConsistent)
                    {
                        throw new Exception("fs ex");
                    }
                }

                //deadlock?
                if (UseDeadlockHandler)
                {
                    if (_deadlockHandler.IsInDeadlock(agent, currentTime))
                    {
                        _deadlockHandler.RandomHop(agent);
                    }
                }
            }
        }