예제 #1
0
 public override void FindPaths(double currentTime, List <Agent> agents)
 {
     //randomHop
     _deadlockHandler.LengthOfAWaitStep = LengthOfAWaitStep;
     foreach (var agent in agents.Where(a => !a.FixedPosition))
     {
         _deadlockHandler.RandomHop(agent);
     }
 }
예제 #2
0
        /// <summary>
        /// Find the path for all the agents.
        /// </summary>
        /// <param name="agents">agents</param>
        /// <param name="queues">The queues for the destination, starting with the destination point.</param>
        /// <param name="nextReoptimization">The next re-optimization time.</param>
        /// <returns>
        /// paths
        /// </returns>
        /// <exception cref="System.Exception">Here I have to do something</exception>
        public override void FindPaths(double currentTime, List <Agent> agents)
        {
            Stopwatch.Restart();

            //Reservation Table
            _reservationTable.Reorganize(currentTime);

            //remove all agents that are not affected by this search
            foreach (var missingAgentId in _calculatedReservations.Keys.Where(id => !agents.Any(a => a.ID == id)))
            {
                _reservationTable.Remove(_calculatedReservations[missingAgentId]);
            }

            //sort Agents
            agents = agents.OrderBy(a => a.CanGoThroughObstacles ? 1 : 0).ThenBy(a => Graph.getDistance(a.NextNode, a.DestinationNode)).ToList();

            Dictionary <int, double> bias = new Dictionary <int, double>();

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

            //optimize Path
            if (UseBias)
            {
                foreach (var agent in agents.Where(a => a.RequestReoptimization && !a.FixedPosition && a.NextNode != a.DestinationNode))
                {
                    //Create RRA* search if necessary.
                    //Necessary if the agent has none or the agents destination has changed
                    ReverseResumableAStar rraStar;
                    if (!rraStars.TryGetValue(agent.ID, out rraStar) || rraStar == null || rraStar.StartNode != agent.DestinationNode ||
                        UseDeadlockHandler && _deadlockHandler.IsInDeadlock(agent, currentTime)) // TODO this last expression is used to set back the state of the RRA* in case of a deadlock - this is only a hotfix
                    {
                        rraStars[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
                    }

                    if (rraStars[agent.ID].Closed.Contains(agent.NextNode) || rraStars[agent.ID].Search(agent.NextNode))
                    {
                        var nodes = rraStars[agent.ID].getPathAsNodeList(agent.NextNode);
                        nodes.Add(agent.NextNode);
                        foreach (var node in nodes)
                        {
                            if (!bias.ContainsKey(node))
                            {
                                bias.Add(node, 0.0);
                            }
                            bias[node] += LengthOfAWaitStep * 1.0001;
                        }
                    }
                }
            }

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

                //remove existing reservations of previous calculations that are not a victim of the reorganization
                _reservationTable.Remove(_calculatedReservations[agent.ID]);

                //all reservations deleted
                _calculatedReservations[agent.ID].Clear();

                if (!UseBias)
                {
                    //Create RRA* search if necessary.
                    //Necessary if the agent has none or the agents destination has changed
                    ReverseResumableAStar rraStar;
                    if (!rraStars.TryGetValue(agent.ID, out rraStar) || rraStar == null || rraStar.StartNode != agent.DestinationNode ||
                        UseDeadlockHandler && _deadlockHandler.IsInDeadlock(agent, currentTime)) // TODO this last expression is used to set back the state of the RRA* in case of a deadlock - this is only a hotfix
                    {
                        rraStars[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
                    }
                }

                //search my path to the goal
                var aStar = new SpaceTimeAStar(Graph, LengthOfAWaitStep, currentTime + LengthOfAWindow, _reservationTable, agent, rraStars[agent.ID]);
                aStar.FinalReservation = true;

                List <int> nodes = null;
                if (UseBias)
                {
                    aStar.BiasedCost = bias;

                    if (rraStars[agent.ID].Closed.Contains(agent.NextNode) || rraStars[agent.ID].Search(agent.NextNode))
                    {
                        //subtract my own cost
                        nodes = rraStars[agent.ID].getPathAsNodeList(agent.NextNode);
                        nodes.Add(agent.NextNode);
                        foreach (var node in nodes)
                        {
                            bias[node] -= LengthOfAWaitStep * 1.0001;
                        }
                    }
                }


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

                if (UseBias && nodes != null)
                {
                    //re add my own cost
                    foreach (var node in nodes)
                    {
                        bias[node] += LengthOfAWaitStep * 1.0001;
                    }
                }

                if (!found)
                {
                    //set a fresh reservation for my current node
                    _reservationTable.Clear(agent.NextNode);
                    _calculatedReservations[agent.ID] = new List <ReservationTable.Interval>(new ReservationTable.Interval[] { new ReservationTable.Interval(agent.NextNode, 0, double.PositiveInfinity) });
                    _reservationTable.Add(_calculatedReservations[agent.ID]);
                    agent.Path = new Path();

                    //clear all reservations of other agents => they will not calculate a path over this node anymore
                    foreach (var otherAgent in _calculatedReservations.Keys.Where(id => id != agent.ID))
                    {
                        _calculatedReservations[otherAgent].RemoveAll(r => r.Node == agent.NextNode);
                    }

                    //add wait step
                    agent.Path.AddFirst(agent.NextNode, true, LengthOfAWaitStep);

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

                //just wait where you are and until the search time reaches currentTime + LenghtOfAWindow is always a solution
                Debug.Assert(found);

                //+ WHCA* Nodes
                agent.Path = new Path();

                List <ReservationTable.Interval> reservations;
                aStar.GetPathAndReservations(ref agent.Path, out reservations);
                _calculatedReservations[agent.ID] = reservations;

                //add to reservation table
                _reservationTable.Add(_calculatedReservations[agent.ID]);


                //add reservation to infinity
                var lastNode = (_calculatedReservations[agent.ID].Count > 0) ? _calculatedReservations[agent.ID][_calculatedReservations[agent.ID].Count - 1].Node : agent.NextNode;
                var lastTime = (_calculatedReservations[agent.ID].Count > 0) ? _calculatedReservations[agent.ID][_calculatedReservations[agent.ID].Count - 1].End : currentTime;
                _calculatedReservations[agent.ID].Add(new ReservationTable.Interval(lastNode, lastTime, double.PositiveInfinity));
                try
                {
                    _reservationTable.Add(lastNode, lastTime, double.PositiveInfinity);
                }
                catch (DisjointIntervalTree.IntervalIntersectionException)
                {
                    //This could technically fail => especially when they come from a station
                }

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

                //next time ready?
                if (agent.Path.Count == 0)
                {
                    rraStars[agent.ID] = null;
                }

                //deadlock?
                if (UseDeadlockHandler)
                {
                    if (_deadlockHandler.IsInDeadlock(agent, currentTime))
                    {
                        _deadlockHandler.RandomHop(agent);
                    }
                }
            }
        }
예제 #3
0
        /// <summary>
        /// Find the path for all the agents.
        /// </summary>
        /// <param name="currentTime">The current Time.</param>
        /// <param name="agents">agents</param>
        public override void FindPaths(double currentTime, List <Agent> agents)
        {
            Stopwatch.Restart();

            //initialization data structures
            var conflictTree   = new ConflictTree();
            var Open           = new FibonacciHeap <double, ConflictTree.Node>();
            var solvable       = true;
            var generatedNodes = 0;

            ConflictTree.Node bestNode = null;
            double            bestTime = 0.0;

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

            //simply blocked
            foreach (var agent in agents.Where(a => a.FixedPosition))
            {
                Graph.NodeInfo[agent.NextNode].IsLocked = true;
            }

            // TODO this only works as long as a possible solution is guaranteed - maybe instead ignore paths to plan for agents with no possible solution and hope that it clears by others moving on?
            //first node initialization
            List <Agent> unsolvableAgents = null;

            foreach (var agent in agents.Where(a => !a.FixedPosition))
            {
                bool agentSolved = Solve(conflictTree.Root, currentTime, agent);
                if (!agentSolved)
                {
                    if (unsolvableAgents == null)
                    {
                        unsolvableAgents = new List <Agent>()
                        {
                            agent
                        }
                    }
                    ;
                    else
                    {
                        unsolvableAgents.Add(agent);
                    }
                }
                solvable = solvable && agentSolved;
            }

            //node selection strategy (Queue will pick the node with minimum value
            NodeSelectionExpression nodeObjectiveSelector = node =>
            {
                switch (SearchMethod)
                {
                case CBSSearchMethod.BestFirst:
                    return(node.SolutionCost);

                case CBSSearchMethod.BreathFirst:
                    return(node.Depth);

                case CBSSearchMethod.DepthFirst:
                    return((-1) * node.Depth);

                default:
                    return(0);
                }
            };

            //Enqueue first node
            if (solvable)
            {
                Open.Enqueue(conflictTree.Root.SolutionCost, conflictTree.Root);
            }
            else
            {
                Communicator.LogDefault("WARNING! Aborting CBS - could not obtain an initial solution for the following agents: " +
                                        string.Join(",", unsolvableAgents.Select(a => "Agent" + a.ID.ToString() + "(" + a.NextNode.ToString() + "->" + a.DestinationNode.ToString() + ")")));
            }
            bestNode = conflictTree.Root;

            //search loop
            ConflictTree.Node p = conflictTree.Root;
            while (Open.Count > 0)
            {
                //local variables
                int agentId1;
                int agentId2;
                ReservationTable.Interval interval;

                //pop out best node
                p = Open.Dequeue().Value;

                //check the path
                var hasNoConflicts = ValidatePath(p, agents, out agentId1, out agentId2, out interval);

                //has no conflicts?
                if (hasNoConflicts)
                {
                    bestNode = p;
                    break;
                }

                // time up? => return the best solution
                if (Stopwatch.ElapsedMilliseconds / 1000.0 > RuntimeLimitPerAgent * agents.Count * 0.9 || Stopwatch.ElapsedMilliseconds / 1000.0 > RunTimeLimitOverall)
                {
                    Communicator.SignalTimeout();
                    break;
                }

                //save best node
                if (bestNode == null || interval.Start > bestTime)
                {
                    bestTime = interval.Start;
                    bestNode = p;
                }

                //append child 1
                var node1 = new ConflictTree.Node(agentId1, interval, p);
                solvable = Solve(node1, currentTime, agents.First(a => a.ID == agentId1));
                if (solvable)
                {
                    Open.Enqueue(node1.SolutionCost, node1);
                }

                //append child 2
                var node2 = new ConflictTree.Node(agentId2, interval, p);
                solvable = Solve(node2, currentTime, agents.First(a => a.ID == agentId2));
                if (solvable)
                {
                    Open.Enqueue(node2.SolutionCost, node2);
                }

                generatedNodes += 2;
            }

            //return the solution => suboptimal
            foreach (var agent in agents)
            {
                agent.Path = bestNode.getSolution(agent.ID);
                if (_deadlockHandler.IsInDeadlock(agent, currentTime))
                {
                    _deadlockHandler.RandomHop(agent);
                }
            }
        }
예제 #4
0
        public override void FindPaths(double currentTime, List <Agent> agents)
        {
            Stopwatch.Restart();

            var fixedBlockage = AgentInfoExtractor.getStartBlockage(agents, currentTime);

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

            //initiate biased costs
            _biasedCost.Clear();
            foreach (var agent in agents)
            {
                _biasedCost.Add(agent.ID, new Dictionary <int, double>());
            }

            while (true)
            {
                //clear reservation table
                _reservationTable.Clear();

                //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
                        _reservationTable.Add(agent.NextNode, agent.ArrivalTimeAtNextNode, double.PositiveInfinity, agent.ID);
                    }
                }
                catch (RAWSimO.MultiAgentPathFinding.DataStructures.DisjointIntervalTree.IntervalIntersectionException) { }

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

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


                    if (_deadlockHandler.IsInDeadlock(agent, currentTime))
                    {
                        _deadlockHandler.RandomHop(agent, _reservationTable, currentTime, true, true);
                        continue;
                    }

                    //Create A* search if necessary.
                    //Necessary if the agent has none or the agents destination has changed
                    var aStar = new SpaceAStar(Graph, agent.NextNode, agent.DestinationNode, agent.OrientationAtNextNode, agent.Physics, agent, _biasedCost[agent.ID]);

                    var found = aStar.Search();

                    //the search ended with no result => just wait a moment
                    if (!found)
                    {
                        //Can not find a path. Maybe the agent has to wait until the blocked nodes moved
                        waitStep(agent);
                        continue;
                    }

                    //get the path
                    List <ReservationTable.Interval> reservations;
                    agent.Path.Clear();
                    aStar.getReservationsAndPath(agent.ArrivalTimeAtNextNode, ref agent.Path, out reservations);
                    reservations.Last().End = double.PositiveInfinity;

                    //check whether the agent collides with an other agent on its way
                    int collidesWithAgent;
                    int collidesOnNode;
                    if (!_reservationTable.IntersectionFree(reservations, out collidesOnNode, out collidesWithAgent))
                    {
                        agent.Path.SetStopBeforeNode(collidesOnNode);

                        //collision => add biased cost for the current agent
                        //the other agent remains with the old cost. He has the higher priority
                        if (_biasedCost[agent.ID].ContainsKey(collidesOnNode))
                        {
                            _biasedCost[agent.ID][collidesOnNode] += BiasedCostAmount;
                        }
                        else
                        {
                            _biasedCost[agent.ID][collidesOnNode] = BiasedCostAmount;
                        }

                        collisionFound = true;
                    }
                    else
                    {
                        _reservationTable.Add(reservations, agent.ID);
                    }
                }

                if (!collisionFound)
                {
                    return;
                }
            }
        }
예제 #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);
                    }
                }
            }
        }
예제 #6
0
        /// <summary>
        /// Find the path for all the agents.
        /// </summary>
        /// <param name="agents">agents</param>
        /// <param name="obstacleWaypoints">The way points of the obstacles.</param>
        /// <param name="queues">The queues for the destination, starting with the destination point.</param>
        /// <param name="nextReoptimization">The next re-optimization time.</param>
        /// <returns>
        /// paths
        /// </returns>
        /// <exception cref="System.Exception">Here I have to do something</exception>
        private bool _findPaths(double currentTime, List <Agent> agents, Dictionary <int, int> agentPrios, bool lastRun, double runtimeLimit)
        {
            var conflictFree = true;

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

            SortAgents(ref agents, agentPrios);

            //set fixed blockage
            foreach (var interval in fixedBlockage.Values.SelectMany(d => d))
            {
                _reservationTable.Add(interval);
            }

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

            //optimize Path
            foreach (var agent in agents.Where(a => !a.FixedPosition))
            {
                if (Stopwatch.ElapsedMilliseconds / 1000.0 > runtimeLimit * 0.9)
                {
                    Communicator.SignalTimeout();
                    return(true);
                }

                //Create RRA* search if necessary.
                //Necessary if the agent has none or the agents destination has changed
                ReverseResumableAStar rraStar;
                if (!rraStars.TryGetValue(agent.ID, out rraStar) || rraStar.StartNode != agent.DestinationNode ||
                    UseDeadlockHandler && _deadlockHandler.IsInDeadlock(agent, currentTime)) // TODO this last expression is used to set back the state of the RRA* in case of a deadlock - this is only a hotfix
                {
                    rraStars[agent.ID] = new ReverseResumableAStar(Graph, agent, agent.Physics, agent.DestinationNode);
                }

                //search my path to the goal
                var aStar = new SpaceTimeAStar(Graph, LengthOfAWaitStep, currentTime + LengthOfAWindow, _reservationTable, agent, rraStars[agent.ID]);

                //the agent with a higher priority has to wait so that the others can go out of the way
                aStar.WaitStepsBeforeStart = (int)(Math.Pow(2, agentPrios[agent.ID]) / 2.0);

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

                if (!found)
                {
                    conflictFree = false;

                    //fall back => ignore the collisions
                    agentPrios[agent.ID]++;
                    if (!lastRun)
                    {
                        if (!AbortAtFirstConflict)
                        {
                            continue;
                        }
                        else
                        {
                            return(false);
                        }
                    }
                }

                //+ WHCA* Nodes
                List <ReservationTable.Interval> reservations;
                if (found)
                {
                    aStar.GetPathAndReservations(ref agent.Path, out reservations);

                    foreach (var reservation in reservations)
                    {
                        _reservationTable.Add(reservation);
                    }
                }

                //+ RRA* Nodes
                if (aStar.GoalNode >= 0)
                {
                    rraStars[agent.ID].addPath(agent.Path, aStar.NodeTo2D(aStar.GoalNode));
                }

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

                //next time ready?
                if (agent.Path.Count == 0)
                {
                    rraStars[agent.ID] = null;
                }

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

            return(conflictFree);
        }