public override byte[] GetBytes()
{
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
Node ignore = f.RearNode;
ForkliftPath fp = new ForkliftPath(GetPath(), ignore);
return Encoding.ASCII.GetBytes(fp.getDirections());
}
public void TestForkliftPathGraphSixNodesFromAToBBackToB()
{
Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;
Path p = g.ShortestPath(g.getNode("a"), g.getNode("b"), g.getNode("b"));
ForkliftPath fp = new ForkliftPath(p, g.getNode("b"));
string actual = fp.getDirections();
string expected = "NNLSNSNNLNL";
Assert.IsTrue(actual == expected);
}
// Consumes one package
private void ConsumePackage(byte[] package)
{
// Allocates data from the package
byte[] data = new byte[package.Length - 3];
// Finds the type of pakcage
byte byteType = package[1];
// Fills the data array with the corresponding data
for (int i = 2, j = 0; i < package.Length - 1; i++, j++)
{
data[j] = package[i];
}
// Convert the data to a string
string dataString = Encoding.UTF8.GetString(data, 0, data.Length);
// Find the correct method to consume the package
switch (byteType)
{
case TYPE_UPDATE_COLOR:
// Tell the user that the color is being fetched
// dataString contains the id of the color
ConsoleHandler.AddMessage(MessageType.BLUETOOTH, "Sending color #" + dataString + "...");
// Get the color from the database
Color requestedColor = Database.Instance.Data.Colors.FirstOrDefault(i => i.Value.Identifier == int.Parse(dataString)).Value;
// Convert the RGB-values of the color to byte array
byte[] returnData = requestedColor.ToRGBBytes();
// Send the package back to the NXT
SendPackageBT(TYPE_FETCHED_COLOR, returnData);
break;
case TYPE_SAVE_COLOR:
// Split the data into to values that needs to be saved
int colorId = int.Parse(dataString.Substring(0,3)); // Use the first char as color identifer
int red = int.Parse(dataString.Substring(3,3)); // Use the first char as red value
int green = int.Parse(dataString.Substring(6,3)); // Use the first char as green value
int blue = int.Parse(dataString.Substring(9,3)); // Use the first char as blue value
// Tell the user that the color is being saved
ConsoleHandler.AddMessage(MessageType.BLUETOOTH, "Received color #" + colorId + ". (" + red + ", " + green + ", " + blue + ")");
// Find the old color
Color color = Database.Instance.Data.Colors.FirstOrDefault(i => i.Value.Identifier == colorId).Value;
// Update the values
color.Red = red;
color.Green = green;
color.Blue = blue;
// Save the updated color
Database.Instance.Data.AddColor(color);
break;
case TYPE_REPORT_OBSTACLE:
ConsoleHandler.AddMessage(MessageType.ERROR, "Obstacle encountered!");
int directionsIndex = int.Parse(dataString);
if (!(CurrentJob is TurnJob) && !(CurrentJob is DebugJob))
{
if (CurrentJob != null)
{
Path currentPath = CurrentJob.GetPath();
String directions = new ForkliftPath(currentPath, forklift.RearNode).getDirections();
if (CurrentJob is PalletJob)
{
PalletJob pj = (PalletJob)CurrentJob;
if (pj.Type == PalletJobType.fetch)
{
directions = directions.Insert(0, "TUND");
}
else if (pj.Type == PalletJobType.deliver)
{
directions = directions.Insert(0, "TUBDN");
}
}
int max = directions.Length;
int i = directionsIndex;
if (directions[directions.Length - 1] == 'L' || directions[directions.Length - 1] == 'R')
{
max--;
}
double nodeRearIndex = (max - 1.0 - i) / 2.0;
if (Math.Abs(nodeRearIndex % 1 - 0.5) < 0.4)
{
ConsoleHandler.AddMessage(MessageType.ERROR, "Currently inside intersection - aborting all jobs");
forklift.FrontNode = null;
forklift.FrontNode = null;
// Inform the user to update the position
ConsoleHandler.AddMessage(MessageType.ERROR, "Please update the position of the forklift...");
while (forklift.FrontNode == null || forklift.RearNode == null)
{
;
}
}
else
{
Node newRearNode = null;
Node newFrontNode = null;
int roundedIndex = (int)nodeRearIndex;
if (roundedIndex == 0)
{
newFrontNode = forklift.FrontNode;
newRearNode = forklift.RearNode;
}
else
{
newFrontNode = currentPath.Nodes[roundedIndex];
newRearNode = currentPath.Nodes[roundedIndex - 1];
}
// Update the visitied count on edges
Node previousNode = null;
foreach (Node n in currentPath.Nodes)
{
if (previousNode == null)
{
previousNode = n;
continue;
}
// Find edge to the previous node and increment the visitied count
KeyValuePair<Node, Edge> nodeEdgePair = previousNode.Neighbours.Single(x => x.Key != null && x.Key.Equals(n));
nodeEdgePair.Value.Visited++;
if (n.Equals(newFrontNode))
{
break;
}
previousNode = n;
}
// Update forklift nodes
forklift.FrontNode = newFrontNode;
forklift.RearNode = newRearNode;
// Update edge the NXT is standing on
Database.Instance.Data.Graphs.FirstOrDefault().Value.BlockEdge(newFrontNode, newRearNode);
}
}
else
{
Commands.PrintError("No current job - Continuing...");
}
}
else if(CurrentJob is TurnJob)
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Already performing turn-job... (ignoreing)");
}
else if (CurrentJob is DebugJob)
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Performing debug-job... (ignoreing debugjobs)");
}
break;
// Check if the NXT updated its status
case TYPE_UPDATE_STATUS:
{
if (dataString[0] != GetStatusByte(forklift))
{
// Tell the user what the status the NXT updated to
ConsoleHandler.AddMessage(MessageType.BLUETOOTH, "Received " + ConsoleHandler.DNS + "-status: " + dataString);
}
// Check what status the NXT told us
switch (dataString[0])
{
// The NXT was idle
case STATUS_IDLE:
// Check if the nxt just completed a job
if (forklift.Status == Status.BUSY && CurrentJob != null)
{
Path p = CurrentJob.GetPath();
// Update the visited count on traversed edges
Node previousNode = null;
foreach (Node n in p.Nodes)
{
if (previousNode == null)
{
previousNode = n;
continue;
}
// Find edge to the previous node and increment the visitied count
KeyValuePair<Node, Edge> nodeEdgePair = previousNode.Neighbours.Single(x => x.Key != null && x.Key.Equals(n));
nodeEdgePair.Value.Visited++;
previousNode = n;
}
// Update the position of the forklift
if (p.Nodes.Count >= 2)
{
Node frontNode = p.Nodes.ElementAt(p.Nodes.Count - 1);
Node rearNode = p.Nodes.ElementAt(p.Nodes.Count - 2);
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
f.UpdateNodes(frontNode, rearNode);
}
if (!(CurrentJob is TurnJob))
{
Graph g = Database.Instance.Data.Graphs.FirstOrDefault().Value;
// Unblock all edges
for (int i = g.BlockedEdges.Count - 1; i >= 0; i--)
{
Tuple<Node, Node> edge = g.BlockedEdges[i];
g.UnblockEdge(edge.Item1, edge.Item2);
}
// Decay the graph
g.Decay(0.90);
}
if (CurrentJob is PalletJob)
{
PalletJob job = (PalletJob) CurrentJob;
// Check if the forklift just finished a deliver pallet job
if (job.Type == PalletJobType.deliver)
{
Pallet pallet = forklift.Payload;
// Update the location of the pallet
pallet.Location = p.Nodes.Last();
// Update the payload of the forklift
forklift.Payload = null;
}
else if (job.Type == PalletJobType.fetch)
{
Node n = p.Nodes.Last();
// Update the payload (Which will also update the location of the pallet)
forklift.Payload = n.Pallet;
}
// Swap front and rear node
forklift.UpdateNodes(forklift.RearNode, forklift.FrontNode);
}
CurrentJob = null;
}
// Update the internal status
forklift.Status = Status.IDLE;
// Check if PALL-E should get another job
if (CurrentJob == null && forklift.Status == Status.IDLE)
{
Job nextJob = null;
while ((nextJob == null || nextJob.GetPath() != null) && JobList.Count > 0)
{
// Get the next job (job with lowest id)
nextJob = JobList.Aggregate((l, r) => l.Key < r.Key ? l : r).Value;
try
{
nextJob.GetPath();
break;
}
catch (PathException e)
{
Commands.PrintError("Remove job #" + nextJob.ID() + ": '" + e.Message + "'");
}
catch (JobException e)
{
Commands.PrintError("Remove job #" + nextJob.ID() + ": '" + e.Message + "'");
}
Database.Instance.Data.RemoveJob(nextJob);
nextJob = null;
}
// We removed all the jobs, nothing left to do!
if (JobList.Count < 1)
{
break;
}
try
{
// Test if the path can be generated
nextJob.GetPath();
// Tell the user what job was sent
Commands.PrintSuccess("Sending Job #" + nextJob.ID() + " to PALL-E. " + (JobList.Count - 1) + " jobs left");
// Send the job to the NXT
SendPackageBT(nextJob.GetJobTypeBytes(), nextJob.GetBytes());
}
catch (JobException e)
{
Commands.PrintError("Job #" + nextJob.ID() + " cancelled: '" + e.Message + "'");
Database.Instance.Data.RemoveJob(nextJob);
}
}
break;
// The NXT was busy
case STATUS_BUSY:
// If the NXT was just idle, you know you have given it a job
if (forklift.Status == Status.IDLE)
{
// Debug jobs has highe priority
if(JobList.Count > 0)
{
// Remove the job that is being executed currently
CurrentJob = JobList.Aggregate((l, r) => l.Key < r.Key ? l : r).Value;
Database.Instance.Data.Jobs.Remove(CurrentJob.Identifier);
}
}
// Update the internal status
forklift.Status = Status.BUSY;
break;
case STATUS_OBSTACLE:
if (CurrentJob != null)
{
if (!(CurrentJob is DebugJob))
{
if (!(CurrentJob is TurnJob))
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Calculating alternative path...");
Database.Instance.Data.AddJob(CurrentJob);
CurrentJob = new TurnJob(Database.Instance.Data.Jobs.Keys.Min() - 1);
Node oldFrontNode = forklift.FrontNode;
Node oldRearNode = forklift.RearNode;
forklift.FrontNode = oldRearNode;
forklift.RearNode = oldFrontNode;
ConsoleHandler.AddMessage(MessageType.REGULAR, "Sending turn-job to PALL-E...");
}
else
{
ConsoleHandler.AddMessage(MessageType.REGULAR, "Resending turn-job to " + ConsoleHandler.DNS + "...");
}
// Send turn job
SendPackageBT(CurrentJob.GetJobTypeBytes(), CurrentJob.GetBytes());
}
else
{
// Send empty package (so that obstacle status will be avoided)
DebugJob dj = new DebugJob(-1, "");
SendPackageBT(dj.GetJobTypeBytes(), dj.GetBytes());
}
}
else
{
ConsoleHandler.AddMessage(MessageType.ERROR, "Tried to recover from obstacle, but there are no jobs left");
// throw new Exception("No current jobs");
}
// The status should not be updated if the current job is a debugjob
if (!(CurrentJob is DebugJob))
{
// Update the internal status
forklift.Status = Status.OBSTACLE;
}
break;
// The NXT encoutered an error
case STATUS_ERROR:
// Update the internal status
forklift.Status = Status.ERROR;
// Tell the user that the NXT encountered an error
ConsoleHandler.AddMessage(MessageType.ERROR, ConsoleHandler.DNS + " reported an error");
break;
default:
forklift.Status = Status.UNKNOWN;
break;
}
break;
}
}
return;
}
public override byte[] GetBytes()
{
Forklift f = Database.Instance.Data.Forklifts.FirstOrDefault().Value;
Node ignore = f.RearNode;
Path p = GetPath();
if (p == null)
{
throw new NullReferenceException("Path is empty (either at destination, or impossible)");
}
ForkliftPath fp = new ForkliftPath(p, ignore);
string directions = fp.getDirections();
// We add some special instructions to handle the different job types
// D - forklift down
// N - move to next red tape
// U - forklift up
// T - Turn around
// B - Back off small step
// The instructions is inserted in reversed order
if (Type == PalletJobType.fetch)
{
// Removes the extra N
directions = directions.Substring(1, directions.Length - 1);
// Add directions to fetch the pallet
directions = directions.Insert(0, "TUNDR");
}
else if (Type == PalletJobType.deliver)
{
// Removes the extra N
directions = directions.Substring(1, directions.Length - 1);
// Add directions to deliver the pallet
directions = directions.Insert(0, "TUBDNR");
}
return Encoding.ASCII.GetBytes(directions);
}
