/// <summary>
/// Ons the bot join queue.
/// </summary>
/// <param name="bot">The bot.</param>
internal void onBotJoinQueue(BotNormal bot)
{
if (!_managedBots.Contains(bot))
{
_managedBots.Add(bot);
}
}
/// <summary>
/// Gets the place in queue.
/// </summary>
/// <param name="bot">The bot.</param>
/// <returns></returns>
public Waypoint getPlaceInQueue(BotNormal bot)
{
if (_botsInQueue.Contains(bot))
{
return(null);
}
else
{
return(_placeInQueue[bot]);
}
}
/// <summary>
/// Notifies the path manager when the bot has a new destination.
/// </summary>
/// <param name="bot">The bot.</param>
/// <exception cref="System.NotImplementedException"></exception>
internal void notifyBotNewDestination(BotNormal bot)
{
if (bot.DestinationWaypoint == null)
{
return;
}
if (_queueManagers.ContainsKey(bot.DestinationWaypoint))
{
_queueManagers[bot.DestinationWaypoint].onBotJoinQueue(bot);
}
}
/// <summary>
/// Updates this instance.
/// </summary>
public void Update()
{
//no need for managing
if (_managedBots.Count == 0)
{
return;
}
//get locked way points
LockedWaypoints.Clear();
_botsInQueue.Clear();
//is elevator and elevator is in use => lock the destination waypoint
if (QueueWaypoint.Elevator != null && QueueWaypoint.Elevator.InUse)
{
LockedWaypoints.Add(QueueWaypoint, null);
}
//check bot states
for (int i = 0; i < _managedBots.Count; i++)
{
BotNormal bot = _managedBots[i];
var nextWaypoint = bot.NextWaypoint != null ? bot.NextWaypoint : bot.CurrentWaypoint;
var currentWaypointInQueue = Queue.Contains(bot.CurrentWaypoint);
var nextWaypointInQueue = Queue.Contains(nextWaypoint);
var locksCurrentWaypoint = currentWaypointInQueue && bot.CurrentWaypoint.GetDistance(bot) <= _maxEdgeLength[bot.CurrentWaypoint];
// Check whether bot is leaving the queue (only possible at the queue waypoint!)
if (// Check whether the bot has a new destination and its current waypoint is the end of the queue
(bot.DestinationWaypoint != QueueWaypoint && bot.CurrentWaypoint == QueueWaypoint && !locksCurrentWaypoint) ||
// Check whether the bot is already outside the queue area and has a different destination than the queue waypoint
(!currentWaypointInQueue && !nextWaypointInQueue && bot.DestinationWaypoint != QueueWaypoint))
{
//bot leaves elevator?
if (QueueWaypoint.Elevator != null && bot == QueueWaypoint.Elevator.usedBy)
{
QueueWaypoint.Elevator.InUse = false;
QueueWaypoint.Elevator.usedBy = null;
}
// Not in queue anymore - remove it
_managedBots.RemoveAt(i);
// Update index (we removed one)
i--;
// Mark queueing inactive (this is redundant - see below)
bot.IsQueueing = false;
// Proceed to next bot
continue;
}
//bot in Queue
if (currentWaypointInQueue)
{
_botsInQueue.Add(bot);
// Indicate queueing
bot.IsQueueing = true;
if (bot.CurrentWaypoint != QueueWaypoint)
{
bot.RequestReoptimization = false; //this bot will be managed by this manager
}
//add locks
if (locksCurrentWaypoint && !LockedWaypoints.ContainsKey(bot.CurrentWaypoint))
{
LockedWaypoints.Add(bot.CurrentWaypoint, bot);
}
if (nextWaypointInQueue && !LockedWaypoints.ContainsKey(nextWaypoint))
{
LockedWaypoints.Add(nextWaypoint, bot);
}
}
//bot reached end of the queue - no active queueing anymore
if (_queueWaypointIndices.ContainsKey(bot.CurrentWaypoint) && _queueWaypointIndices[bot.CurrentWaypoint] == 0)
{
// Mark queueing inactive
bot.IsQueueing = false;
_botsInQueue.Remove(bot);
}
}
//if this is an elevator queue, the first way point is locked, when the elevator is in use
if (QueueWaypoint.Elevator != null && QueueWaypoint.Elevator.InUse && !LockedWaypoints.ContainsKey(QueueWaypoint))
{
LockedWaypoints[QueueWaypoint] = null;
}
// Remove bots that finished their cruise
foreach (var bot in _queueCruisePaths.Where(kvp => kvp.Key.CurrentWaypoint == kvp.Value.Last()).Select(kvp => kvp.Key).ToArray())
{
_queueCruisePaths.Remove(bot);
}
// Reset places
_placeInQueue.Clear();
// Manage locks for cruising bots
HashSet <BotNormal> failedCruiseBots = null;
foreach (var bot in _queueCruisePaths.Keys)
{
// Assert that the bot is in the queue
Debug.Assert(_queueAreaXMin <= bot.X && bot.X <= _queueAreaXMax && _queueAreaYMin <= bot.Y && bot.Y <= _queueAreaYMax);
// Fetch the waypoint the bot is currently at
var realWP = GetNearestQueueWaypoint(bot.X, bot.Y);
int currentQueueIndex = _queueWaypointIndices[realWP];
// Lock waypoints that are left for the cruise
foreach (var cruiseWP in _queueCruisePaths[bot].Where(wp => _queueWaypointIndices[wp] <= currentQueueIndex))
{
// If bot is not moving and next waypoint is already blocked by another bot, something went wrong - discard the cruise and lineup regularly
if (LockedWaypoints.ContainsKey(cruiseWP) && LockedWaypoints[cruiseWP] != bot)
{
// Cruise failed - mark for removal
if (failedCruiseBots == null)
{
failedCruiseBots = new HashSet <BotNormal>()
{
bot
}
}
;
else
{
failedCruiseBots.Add(bot);
}
}
else
{
// Lock waypoint for cruise
LockedWaypoints[cruiseWP] = bot;
}
}
}
// Cancel failed cruises
if (failedCruiseBots != null)
{
foreach (var failedBot in failedCruiseBots)
{
_queueCruisePaths.Remove(failedBot);
}
}
// Assign already moving bots
foreach (var bot in _botsInQueue.Where(bot => bot.CurrentWaypoint != bot.NextWaypoint && bot.NextWaypoint != null))
{
_placeInQueue.Add(bot, bot.NextWaypoint);
}
//assign standing bots
foreach (var bot in _botsInQueue.Where(bot => !_placeInQueue.ContainsKey(bot)))
{
var queueIndex = _queueWaypointIndices[bot.CurrentWaypoint];
var newQueueIndex = -1;
if (queueIndex == 0 || LockedWaypoints.ContainsKey(Queue[queueIndex - 1]))
{
//locked => stay where you are
_placeInQueue.Add(bot, bot.CurrentWaypoint);
newQueueIndex = queueIndex;
}
else
{
//if almost there, just move one up
Path path = new Path();
if (queueIndex == 1)
{
LockedWaypoints.Add(Queue[queueIndex - 1], bot);
path.AddFirst(_pathManager.GetNodeIdByWaypoint(Queue[queueIndex - 1]), true, 0.0);
newQueueIndex = queueIndex - 1;
}
else
{
//go as far as you can
IEnumerable <Waypoint> lockedPredecessorWaypoints = LockedWaypoints.Keys.Where(q => _queueWaypointIndices[q] < queueIndex);
int targetQueueIndex = lockedPredecessorWaypoints.Any() ? lockedPredecessorWaypoints.Max(w => _queueWaypointIndices[w]) + 1 : QueueWaypoint.Elevator != null ? 1 : 0;
List <Waypoint> cruisePath = new List <Waypoint>();
int nextIndex = -1;
// Build cruise path through queue
for (int currentIndex = queueIndex; currentIndex >= targetQueueIndex;)
{
// Determine next waypoint in queue to go to after this one (consider shortcuts)
IEnumerable <Waypoint> shortCuts = Queue[currentIndex].Paths.Where(p => _queueWaypointIndices.ContainsKey(p) && _queueWaypointIndices[p] < currentIndex && targetQueueIndex <= _queueWaypointIndices[p]);
nextIndex = currentIndex > targetQueueIndex && shortCuts.Any() ? shortCuts.Min(p => _queueWaypointIndices[p]) : currentIndex - 1;
// Check whether a stop is required at the
double inboundOrientation = cruisePath.Count >= 1 && currentIndex > targetQueueIndex?Circle.GetOrientation(cruisePath[cruisePath.Count - 1].X, cruisePath[cruisePath.Count - 1].Y, Queue[currentIndex].X, Queue[currentIndex].Y) : double.NaN;
double outboundOrientation = cruisePath.Count >= 1 && currentIndex > targetQueueIndex?Circle.GetOrientation(Queue[currentIndex].X, Queue[currentIndex].Y, Queue[nextIndex].X, Queue[nextIndex].Y) : double.NaN;
bool stopRequired = !double.IsNaN(inboundOrientation) && Math.Abs(Circle.GetOrientationDifference(inboundOrientation, outboundOrientation)) >= bot.Instance.StraightOrientationTolerance;
// Add connection to the overall path
LockedWaypoints[Queue[currentIndex]] = bot;
bool stopAtNode = currentIndex == queueIndex || currentIndex == targetQueueIndex || stopRequired;
path.AddLast(
// The next waypoint to go to
_pathManager.GetNodeIdByWaypoint(Queue[currentIndex]),
// See whether we need to stop at the waypoint (either because it is the last one or the angles do not match - using 10 degrees in radians here, which should be in line with the Graph class of path planning)
stopAtNode,
// Don't wait at all
0.0);
// Add the step to the cruise path
cruisePath.Add(Queue[currentIndex]);
// Update to next index
currentIndex = nextIndex;
}
// Prepare for next node in queue
path.NextNodeToPrepareFor = queueIndex != nextIndex && nextIndex >= 0 ? _pathManager.GetNodeIdByWaypoint(Queue[nextIndex]) : -1;
// The new index in queue is the targeted one
newQueueIndex = targetQueueIndex;
// Save path for overwatch
_queueCruisePaths[bot] = cruisePath;
// Check path
for (int i = 0; i < cruisePath.Count - 1; i++)
{
if (!cruisePath[i].ContainsPath(cruisePath[i + 1]))
{
throw new InvalidOperationException();
}
}
}
// Set path
bot.Path = path;
_placeInQueue.Add(bot, Queue[newQueueIndex]);
//if the next place is an elevator set it
if (_placeInQueue[bot].Elevator != null)
{
QueueWaypoint.Elevator.InUse = true;
QueueWaypoint.Elevator.usedBy = bot;
}
}
}
//assign bots that are not in the queue
//search for the first free node in the queue
int firstFreeNode = Queue.Count - 1;
while (firstFreeNode > 0 && !LockedWaypoints.ContainsKey(Queue[firstFreeNode - 1]))
{
firstFreeNode--;
}
//if this is a queue for an elevator, then do not assign the elevator directly, because others might wait in a queue on a different tier
if (firstFreeNode == 0 && QueueWaypoint.Elevator != null)
{
firstFreeNode = 1;
}
//botsLeftToQueue
var botsLeftToQueue = _managedBots.Where(bot => !_placeInQueue.ContainsKey(bot)).ToList();
//while a bot exists with no place in queue
while (botsLeftToQueue.Count > 0)
{
var nearestBot = botsLeftToQueue[0];
var distance = Queue[firstFreeNode].GetDistance(nearestBot);
for (int i = 1; i < botsLeftToQueue.Count; i++)
{
if (Queue[firstFreeNode].GetDistance(botsLeftToQueue[i]) < distance)
{
nearestBot = botsLeftToQueue[i];
distance = Queue[firstFreeNode].GetDistance(nearestBot);
}
}
botsLeftToQueue.Remove(nearestBot);
_placeInQueue.Add(nearestBot, Queue[firstFreeNode]);
firstFreeNode = Math.Min(firstFreeNode + 1, Queue.Count - 1);
}
//Last Node should never be blocked
if (LockedWaypoints.ContainsKey(Queue.Last()))
{
LockedWaypoints.Remove(Queue.Last());
}
}
/// <summary>
/// Checks weather the bot can go to the next way point without collisions.
/// </summary>
/// <param name="botNormal">The bot.</param>
/// <param name="currentTime">The current time.</param>
/// <param name="waypointStart">The way point start.</param>
/// <param name="waypointEnd">The way point end.</param>
/// <param name="blockCurrentWaypointUntil">Block duration.</param>
/// <param name="rotationDuration">Rotation duration.</param>
/// <returns></returns>
public bool RegisterNextWaypoint(BotNormal botNormal, double currentTime, double blockCurrentWaypointUntil, double rotationDuration, Waypoint waypointStart, Waypoint waypointEnd)
{
//get checkpoints
var tmpReservations = _reservationTable.CreateIntervals(currentTime, blockCurrentWaypointUntil + rotationDuration, 0.0, botNormal.Physics, _waypointIds[waypointStart], _waypointIds[waypointEnd], true);
if (tmpReservations == null)
{
return(false); //no valid way point
}
//if the last node or way point is an elevator way point than add all connected nodes
if (tmpReservations.Count >= 2)
{
var lastReservation = tmpReservations[tmpReservations.Count - 1];
var elevatorWaypoint = _waypointIds[lastReservation.Node];
if (elevatorWaypoint.Elevator != null)
{
var prelastReservation = tmpReservations[tmpReservations.Count - 2];
foreach (var waypoint in elevatorWaypoint.Elevator.ConnectedPoints.Where(w => w != elevatorWaypoint))
{
tmpReservations.Add(new ReservationTable.Interval(_waypointIds[waypoint], prelastReservation.Start, prelastReservation.End));
tmpReservations.Add(new ReservationTable.Interval(_waypointIds[waypoint], lastReservation.Start, lastReservation.End));
}
}
}
//remove current
_reservationTable.Remove(_reservations[botNormal]);
//check if free
var free = _reservationTable.IntersectionFree(tmpReservations);
//check if a pod collision can occur
if (botNormal.Pod != null)
{
//checker if there is a static pod
foreach (var interval in tmpReservations)
{
if (_waypointIds[interval.Node].Pod != null)
{
//there exists a way point with a pod on it
free = false;
break;
}
}
}
if (free)
{
_reservations[botNormal] = tmpReservations;
//#RealWorldIntegration.start
if (botNormal.Instance.SettingConfig.RealWorldIntegrationCommandOutput && botNormal.Instance.SettingConfig.LogAction != null)
{
//Log the wait command
var sb = new StringBuilder();
sb.Append("#RealWorldIntegration => Bot ").Append(botNormal.ID).Append(" Drive: ");
if (blockCurrentWaypointUntil - currentTime > 0)
{
sb.Append("(wait: ").Append(blockCurrentWaypointUntil - currentTime).Append("s)");
}
for (var i = 1; i < _reservations[botNormal].Count - 1; i++)
{
sb.Append(_waypointIds[_reservations[botNormal][i].Node].ID).Append(";");
}
botNormal.Instance.SettingConfig.LogAction(sb.ToString());
// Issue the path command
Instance.RemoteController.RobotSubmitPath(
botNormal.ID, // The ID of the robot the path is send to
blockCurrentWaypointUntil - currentTime, // The time the robot shall wait before executing the path
_reservations[botNormal].Take(_reservations[botNormal].Count - 1).Select(r => _waypointIds[r.Node].ID).ToList()); // The path to execute
}
//#RealWorldIntegration.end
}
//Debug
//Log the wait command
/*
* var tmp = new StringBuilder();
* tmp.Append("Bot ").Append(botNormal.ID).Append(" Drive: ");
*
* if (blockCurrentWaypointUntil - currentTime > 0)
* tmp.Append("(wait: ").Append(blockCurrentWaypointUntil - currentTime).Append("s)");
*
* for (var i = 1; i < _reservations[botNormal].Count - 1; i++)
* tmp.Append(_waypointIds[_reservations[botNormal][i].Node].ID).Append(";");
* botNormal.Instance.Configuration.LogAction(tmp.ToString());
* if (tmp.ToString().Equals(_lastWaitLog))
* botNormal.Instance.Configuration.LogAction("Same Log(for Debugging)");
* _lastWaitLog = tmp.ToString();
*/
//(re)add intervals
_reservationTable.Add(_reservations[botNormal]);
return(free);
}
/// <summary>
/// Creates a bot with the given characteristics.
/// </summary>
/// <param name="id">The ID of the bot.</param>
/// <param name="tier">The initial position (tier).</param>
/// <param name="x">The initial position (x-coordinate).</param>
/// <param name="y">The initial position (y-coordinate).</param>
/// <param name="radius">The radius of the bot.</param>
/// <param name="orientation">The initial orientation.</param>
/// <param name="podTransferTime">The time for picking up and setting down a pod.</param>
/// <param name="maxAcceleration">The maximal acceleration in m/s^2.</param>
/// <param name="maxDeceleration">The maximal deceleration in m/s^2.</param>
/// <param name="maxVelocity">The maximal velocity in m/s.</param>
/// <param name="turnSpeed">The time it takes the bot to take a full turn in s.</param>
/// <param name="collisionPenaltyTime">The penalty time for a collision in s.</param>
/// <returns>The newly created bot.</returns>
public Bot CreateBot(int id, Tier tier, double x, double y, double radius, double orientation, double podTransferTime, double maxAcceleration, double maxDeceleration, double maxVelocity, double turnSpeed, double collisionPenaltyTime, bool createMovableStation = false)
{
// Consider override values
if (SettingConfig.OverrideConfig != null && SettingConfig.OverrideConfig.OverrideBotPodTransferTime)
{
podTransferTime = SettingConfig.OverrideConfig.OverrideBotPodTransferTimeValue;
}
if (SettingConfig.OverrideConfig != null && SettingConfig.OverrideConfig.OverrideBotMaxAcceleration)
{
maxAcceleration = SettingConfig.OverrideConfig.OverrideBotMaxAccelerationValue;
}
if (SettingConfig.OverrideConfig != null && SettingConfig.OverrideConfig.OverrideBotMaxDeceleration)
{
maxDeceleration = SettingConfig.OverrideConfig.OverrideBotMaxDecelerationValue;
}
if (SettingConfig.OverrideConfig != null && SettingConfig.OverrideConfig.OverrideBotMaxVelocity)
{
maxVelocity = SettingConfig.OverrideConfig.OverrideBotMaxVelocityValue;
}
if (SettingConfig.OverrideConfig != null && SettingConfig.OverrideConfig.OverrideBotTurnSpeed)
{
turnSpeed = SettingConfig.OverrideConfig.OverrideBotTurnSpeedValue;
}
// Init
Bot bot = null;
MovableStation ms = null;
if (createMovableStation)
{
ms = new MovableStation(id, this, radius, maxAcceleration, maxDeceleration, maxVelocity, turnSpeed, collisionPenaltyTime, x, y);
bot = ms;
}
else
{
switch (ControllerConfig.PathPlanningConfig.GetMethodType())
{
case PathPlanningMethodType.Simple:
bot = new BotHazard(this, ControllerConfig.PathPlanningConfig as SimplePathPlanningConfiguration);
break;
case PathPlanningMethodType.Dummy:
case PathPlanningMethodType.WHCAvStar:
case PathPlanningMethodType.WHCAnStar:
case PathPlanningMethodType.FAR:
case PathPlanningMethodType.BCP:
case PathPlanningMethodType.OD_ID:
case PathPlanningMethodType.CBS:
case PathPlanningMethodType.PAS:
bot = new BotNormal(id, this, radius, podTransferTime, maxAcceleration, maxDeceleration, maxVelocity, turnSpeed, collisionPenaltyTime, x, y);
break;
default: throw new ArgumentException("Unknown path planning engine: " + ControllerConfig.PathPlanningConfig.GetMethodType());
}
}
// Set values
bot.ID = id;
bot.Tier = tier;
bot.Instance = this;
bot.Radius = radius;
bot.X = x;
bot.Y = y;
bot.PodTransferTime = podTransferTime;
bot.MaxAcceleration = maxAcceleration;
bot.MaxDeceleration = maxDeceleration;
bot.MaxVelocity = maxVelocity;
bot.TurnSpeed = turnSpeed;
bot.CollisionPenaltyTime = collisionPenaltyTime;
bot.Orientation = orientation;
if (bot is BotHazard)
{
((BotHazard)bot).EvadeDistance = 2.3 * radius;
((BotHazard)bot).SetTargetOrientation(orientation);
}
// Add bot
if (ms != null)
{
ms.Capacity = 1000;
Bots.Add(ms);
//bot was referencing only bot-part of movable station and those values were updated
MovableStations.Add(ms);
}
else
{
Bots.Add(bot);
}
tier.AddBot(bot);
_idToBots[bot.ID] = bot;
// Determine volatile ID
int volatileID = 0;
while (_volatileBotIDs.Contains(volatileID))
{
volatileID++;
}
bot.VolatileID = volatileID;
_volatileBotIDs.Add(bot.VolatileID);
// Return it
return(bot);
}
