public string ConvertToRobCmd(int robot, double waitTime, List <int> path)
{
// Get current position and orientation of the bot
RobotOrientation currentOrientation = _orientations[robot];
DTOWaypoint currentWaypoint = _waypointManager.Translate(path.First());
if (currentWaypoint != _positions[robot])
{
throw new InvalidOperationException("Path is not starting with the current position of the robot!");
}
// Store path just in case the robot does not turn towards the right direction - we can then re-execute it after backtracking
if (!_botErrorCorrectionMode.ContainsKey(robot) || !_botErrorCorrectionMode[robot])
{
_botErrorCorrectionMode[robot] = false;
_lastPaths[robot] = path;
}
// Init command list
List <RobotActions> commands = new List <RobotActions>();
// Build all commands necessary to follow the path starting with the second waypoint of the list
foreach (var wpID in path.Skip(1))
{
// Get next waypoint
DTOWaypoint nextWaypoint = _waypointManager.Translate(wpID);
// Check whether the waypoints are connected at all
if (!_waypointManager.IsConnected(currentWaypoint, nextWaypoint))
{
throw new InvalidOperationException("Cannot go to a waypoint that is not connected with this one!");
}
// Get orientation we have to turn towards
RobotOrientation currentGoalOrientation;
double xDelta = nextWaypoint.X - currentWaypoint.X; double yDelta = nextWaypoint.Y - currentWaypoint.Y;
if (Math.Abs(xDelta) == 0 && Math.Abs(yDelta) == 0)
{
throw new InvalidOperationException("Cannot go from a waypoint to itself.");
}
if (Math.Abs(xDelta) > Math.Abs(yDelta))
{
if (xDelta > 0)
{
currentGoalOrientation = RobotOrientation.East;
}
else
{
currentGoalOrientation = RobotOrientation.West;
}
}
else
if (yDelta > 0)
{
currentGoalOrientation = RobotOrientation.North;
}
else
{
currentGoalOrientation = RobotOrientation.South;
}
// See whether we have to turn
switch (currentOrientation)
{
case RobotOrientation.North:
switch (currentGoalOrientation)
{
case RobotOrientation.North: /* Nothing to do */ break;
case RobotOrientation.West: commands.Add(RobotActions.TurnLeft); break;
case RobotOrientation.South: commands.Add(RobotActions.TurnLeft); commands.Add(RobotActions.TurnLeft); break;
case RobotOrientation.East: commands.Add(RobotActions.TurnRight); break;
default: throw new ArgumentException("Unknown orientation: " + currentGoalOrientation);
}
break;
case RobotOrientation.West:
switch (currentGoalOrientation)
{
case RobotOrientation.North: commands.Add(RobotActions.TurnRight); break;
case RobotOrientation.West: /* Nothing to do */ break;
case RobotOrientation.South: commands.Add(RobotActions.TurnLeft); break;
case RobotOrientation.East: commands.Add(RobotActions.TurnLeft); commands.Add(RobotActions.TurnLeft); break;
default: throw new ArgumentException("Unknown orientation: " + currentGoalOrientation);
}
break;
case RobotOrientation.South:
switch (currentGoalOrientation)
{
case RobotOrientation.North: commands.Add(RobotActions.TurnLeft); commands.Add(RobotActions.TurnLeft); break;
case RobotOrientation.West: commands.Add(RobotActions.TurnRight); break;
case RobotOrientation.South: /* Nothing to do */ break;
case RobotOrientation.East: commands.Add(RobotActions.TurnLeft); break;
default: throw new ArgumentException("Unknown orientation: " + currentGoalOrientation);
}
break;
case RobotOrientation.East:
switch (currentGoalOrientation)
{
case RobotOrientation.North: commands.Add(RobotActions.TurnLeft); break;
case RobotOrientation.West: commands.Add(RobotActions.TurnLeft); commands.Add(RobotActions.TurnLeft); break;
case RobotOrientation.South: commands.Add(RobotActions.TurnRight); break;
case RobotOrientation.East: /* Nothing to do */ break;
default: throw new ArgumentException("Unknown orientation: " + currentGoalOrientation);
}
break;
default: throw new ArgumentException("Unknown orientation: " + currentOrientation);
}
// Add the go command
commands.Add(RobotActions.Forward);
// Update markers of current situation
currentWaypoint = nextWaypoint;
currentOrientation = currentGoalOrientation;
}
// Build and return the command string
return(RobotTranslator.EncodeTo(waitTime, commands));
}
public WaypointSearchData(double distanceTraveled, double distanceToGoal, DTOWaypoint waypoint, WaypointSearchData parentMove, int depth)
{
DistanceTraveled = distanceTraveled; DistanceToGoal = distanceToGoal; Waypoint = waypoint; ParentMove = parentMove; Depth = depth;
}
/// <summary>
/// Indicates whether there is a connection from the first to the second waypoint.
/// </summary>
/// <param name="first">The first waypoint the connection is outgoing from.</param>
/// <param name="second">The second waypoint the connection is incoming towards.</param>
/// <returns><code>true</code> if the connection exists, <code>false</code> otherwise.</returns>
public bool IsConnected(DTOWaypoint first, DTOWaypoint second)
{
return(_connections[first].Contains(second));
}
public WaypointSearchResult PlanPath(DTOWaypoint startNode, DTOWaypoint destinationNode)
{
Dictionary <DTOWaypoint, WaypointSearchData> openLocations = new Dictionary <DTOWaypoint, WaypointSearchData>();
Dictionary <DTOWaypoint, WaypointSearchData> closedLocations = new Dictionary <DTOWaypoint, WaypointSearchData>();
openLocations[startNode] = new WaypointSearchData(0.0, GetDistance(startNode, destinationNode), startNode, null, 0);
// Don't move if already at destination
if (startNode == destinationNode)
{
return(null);
}
// Maximum number of waypoints to look at in search
int maxNumIterations = 3000;
int numIterations = 0;
// Loop until end is found
while (true)
{
// Find lowest cost waypoint in openLocations
DTOWaypoint currentNode = null;
double lowestCost = double.PositiveInfinity;
foreach (var w in openLocations.Keys)
{
if (openLocations[w].DistanceTraveled + openLocations[w].DistanceToGoal < lowestCost)
{
currentNode = w;
lowestCost = openLocations[w].DistanceTraveled + openLocations[w].DistanceToGoal;
}
}
// Something wrong happened -can't find the end
if (currentNode == null)
{
return(null);
}
// Grab the details about the current waypoint
WaypointSearchData currentNodeData = openLocations[currentNode];
// If the closest is also the destination or out of iterations
if (currentNode == destinationNode || numIterations++ == maxNumIterations)
{
// Init result
WaypointSearchResult result = new WaypointSearchResult();
// Found it on the first move
if (currentNodeData.ParentMove == null)
{
return(null);
}
// Go back to the first move made
while (currentNodeData != null)
{
result.AddStep(currentNodeData.Waypoint);
currentNodeData = currentNodeData.ParentMove;
}
return(result);
}
// Transfer closest from open to closed list
closedLocations[currentNode] = openLocations[currentNode];
openLocations.Remove(currentNode);
// Expand all the moves
foreach (var successorNode in currentNode.Paths.Select(p => _waypoints[p]))
{
// Check whether the node is already on closed
if (closedLocations.ContainsKey(successorNode))
{
// Don't deal with anything already on the closed list (don't want loops)
continue;
}
// If it's not in the open list, add it
if (!openLocations.ContainsKey(successorNode))
{
openLocations[successorNode] =
new WaypointSearchData(
currentNodeData.DistanceTraveled + GetDistance(currentNode, successorNode), // The distance already traveled
GetDistance(successorNode, destinationNode), // The approximate distance to the goal
successorNode, // The node itself
currentNodeData, // Parent data
currentNodeData.Depth + 1); // The current depth
}
else
{
// It's already in the open list, but see if this new path is better
WaypointSearchData oldPath = openLocations[successorNode];
// Replace it with the new one
if (oldPath.DistanceTraveled + oldPath.DistanceToGoal > currentNodeData.DistanceTraveled + GetDistance(currentNode, successorNode))
{
openLocations[successorNode] =
new WaypointSearchData(
currentNodeData.DistanceTraveled + GetDistance(currentNode, successorNode), // The distance already traveled
GetDistance(successorNode, destinationNode), // The approximate distance to the goal
successorNode, // The node itself
currentNodeData, // Parent data
currentNodeData.Depth + 1); // The current depth
}
}
}
}
}
private double GetDistance(DTOWaypoint from, DTOWaypoint to)
{
return(Math.Sqrt((from.X - to.X) * (from.X - to.X) + (from.Y - to.Y) * (from.Y - to.Y)));
}
/// <summary>
/// Stores another step to this route. The steps have to be added in backwards order.
/// </summary>
/// <param name="waypoint">The waypoint of the route to add.</param>
public void AddStep(DTOWaypoint waypoint)
{
Route.AddFirst(waypoint);
}