private void HandleControlComm(int controlID, string message)
{
// Show the message
_logger(GetPaddedString(ClientType.C, controlID) + "::<<<<::" + message + "::" + DateTime.Now.ToString("G"));
// Check message validness
if (ControlTranslator.CheckMessageValidnessFrom(message))
{
// Translate the message
ControlMessageResultClient messageResult = ControlTranslator.DecodeFrom(message);
// Pass the received command to the right robot
switch (messageResult.Type)
{
case ControlMessageTypesClient.Path:
{
try
{
SendMsg(ClientType.R, messageResult.RobotID, ConvertToRobCmd(messageResult.RobotID, messageResult.WaitTime, messageResult.Path));
}
catch (Exception ex)
{
_logger("Alert! Error tranlating path: " + string.Join("-", messageResult.Path) + " - Message: " + ex.Message);
}
}
break;
case ControlMessageTypesClient.Pickup: { SendMsg(ClientType.R, messageResult.RobotID, RobotTranslator.EncodeToPickup()); } break;
case ControlMessageTypesClient.Setdown: { SendMsg(ClientType.R, messageResult.RobotID, RobotTranslator.EncodeToSetdown()); } break;
case ControlMessageTypesClient.Rest: { SendMsg(ClientType.R, messageResult.RobotID, RobotTranslator.EncodeToRest()); } break;
case ControlMessageTypesClient.GetItem: { SendMsg(ClientType.R, messageResult.RobotID, RobotTranslator.EncodeToGetItem()); } break;
case ControlMessageTypesClient.PutItem: { SendMsg(ClientType.R, messageResult.RobotID, RobotTranslator.EncodeToPutItem()); } break;
default:
break;
}
}
}
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));
}
/// <summary>
/// Handles incoming communication with a robot.
/// </summary>
/// <param name="robotID">The ID of the robot.</param>
/// <param name="message">The message received from the robot.</param>
private void HandleRobotComm(int robotID, string message)
{
// Check message validness
if (RobotTranslator.CheckMessageValidnessFrom(message))
{
// Translate the message
RobotMessageResultClient messageResult = RobotTranslator.DecodeFrom(message);
// Show the message (translate tag to ID if possible)
if (messageResult.Type == RobotMessageTypesClient.Pos && _waypointManager != null && _waypointManager.IsKnownTag(messageResult.WaypointTag))
{
_logger(
GetPaddedString(ClientType.R, robotID) + "::<<<<::" +
message.Replace(messageResult.WaypointTag, _waypointManager.Translate(_waypointManager.Translate(messageResult.WaypointTag)).ID.ToString()) + "::" +
DateTime.Now.ToString("G"));
}
else
{
_logger(
GetPaddedString(ClientType.R, robotID) + "::<<<<::" +
message + "::" +
DateTime.Now.ToString("G"));
}
// Init robot error handling mode if not set yet
if (!_botErrorCorrectionMode.ContainsKey(robotID))
{
_botErrorCorrectionMode[robotID] = false;
}
// Split the submessage
switch (messageResult.Type)
{
case RobotMessageTypesClient.Pos:
{
if (_waypointManager != null && _waypointManager.IsKnownTag(messageResult.WaypointTag))
{
// --> Keep track of previous position
if (_lastPositions[robotID] != _positions[robotID])
{
_lastPositions[robotID] = _positions[robotID];
}
// Translate new position
_positions[robotID] = _waypointManager.Translate(_waypointManager.Translate(messageResult.WaypointTag));
// --> Keep track of orientation
if (_lastPositions[robotID] != null && _positions[robotID] != null)
{
if (_waypointManager.IsConnected(_lastPositions[robotID], _positions[robotID]))
{
// Get orientation we have to turn towards
double xDelta = _positions[robotID].X - _lastPositions[robotID].X;
double yDelta = _positions[robotID].Y - _lastPositions[robotID].Y;
if (Math.Abs(xDelta) > Math.Abs(yDelta))
{
if (xDelta > 0)
{
_orientations[robotID] = RobotOrientation.East;
}
else
{
_orientations[robotID] = RobotOrientation.West;
}
}
else
if (yDelta > 0)
{
_orientations[robotID] = RobotOrientation.North;
}
else
{
_orientations[robotID] = RobotOrientation.South;
}
}
else
{
if (_lastPositions[robotID] != _positions[robotID])
{
_logger("Alert!:robot " + robotID + " lost its orientation");
}
}
}
// --> Error handling
// See whether the robot is off his path
if (_lastPaths.ContainsKey(robotID) && // See whether the robot has a path currently
(_botErrorCorrectionMode.ContainsKey(robotID) ? !_botErrorCorrectionMode[robotID] : true) && // See whether the robot is already in resolve mode
!_lastPaths[robotID].Contains(_positions[robotID].ID)) // See whether the new position is on the robots path
{
// We have a problem - simply abort the current actions and try to drive back to the beginning of the path
List <int> resolvingPath = _waypointManager.PlanPath(_positions[robotID], _waypointManager.Translate(_lastPaths[robotID][0])).Route.Select(s => s.ID).ToList();
_resolvingPaths[robotID] = resolvingPath;
_logger("Alert!:robot " + robotID + " went the wrong way - trying to get it back on track with resolving path " + string.Join(",", resolvingPath));
_botErrorCorrectionMode[robotID] = true;
_errorModeSetter(true);
// Abort actions and submit resolving path
SendMsg(ClientType.R, robotID, ConvertToRobCmd(robotID, 0, resolvingPath));
}
// Continue handling the error until it is fixed
if (_botErrorCorrectionMode[robotID])
{
if (_lastPaths[robotID][0] == _positions[robotID].ID)
{
// We hit the right waypoint - now resubmit the original path and quit resolve mode
_logger("Alert!:robot " + robotID + " successfully arrived at starting waypoint of original path - trying path again");
_botErrorCorrectionMode[robotID] = false;
_errorModeSetter(false);
SendMsg(ClientType.R, robotID, ConvertToRobCmd(robotID, 0, _lastPaths[robotID]));
}
else
{
// See whether the robot is on its resolving path
if (!_resolvingPaths[robotID].Contains(_positions[robotID].ID))
{
// Continue driving back until we hit the right waypoint
List <int> resolvingPath = _waypointManager.PlanPath(_positions[robotID], _waypointManager.Translate(_lastPaths[robotID][0])).Route.Select(s => s.ID).ToList();
_resolvingPaths[robotID] = resolvingPath;
_logger("Alert!:robot " + robotID + " again went the wrong way - trying to get it back on track by applying path " + string.Join(",", resolvingPath));
// Abort actions and submit new resolving path
SendMsg(ClientType.R, robotID, ConvertToRobCmd(robotID, 0, resolvingPath));
}
}
}
// --> Notify all listening controllers (if not currently trying to fix an error)
if (!_botErrorCorrectionMode[robotID])
{
foreach (var controllerID in _controlClients.Keys)
{
SendMsg(ClientType.C, controllerID, ControlTranslator.EncodeTo(robotID, _positions[robotID].ID, _orientations[robotID]));
}
}
// --> Notify the view
_locationUpdater(robotID, _positions[robotID].ID, _orientations[robotID]);
}
else
{
_logger("Alert!:robot " + robotID + " sent an unknown RFID-tag: " + messageResult.WaypointTag + " load correct Waypoints!");
}
}
break;
case RobotMessageTypesClient.Orient:
{
// Store new orientation information
_orientations[robotID] = messageResult.Orientation;
// Notify all listening controllers
foreach (var controllerID in _controlClients.Keys)
{
SendMsg(ClientType.C, controllerID, ControlTranslator.EncodeTo(robotID, _positions[robotID].ID, _orientations[robotID]));
}
// Notify the view
_locationUpdater(robotID, _positions[robotID].ID, _orientations[robotID]);
}
break;
case RobotMessageTypesClient.Pickup:
{
// Notify all listening controllers
foreach (var controllerID in _controlClients.Keys)
{
SendMsg(ClientType.C, controllerID, ControlTranslator.EncodeToPickup(robotID, messageResult.PickupSuccess));
}
}
break;
case RobotMessageTypesClient.Setdown:
{
// Notify all listening controllers
foreach (var controllerID in _controlClients.Keys)
{
SendMsg(ClientType.C, controllerID, ControlTranslator.EncodeToSetdown(robotID, messageResult.SetdownSuccess));
}
}
break;
default: throw new ArgumentException("Unknown message type: " + messageResult.Type);
}
}
else
{
// Show the message
_logger(GetPaddedString(ClientType.R, robotID) + "::<<<<::" + message + "::" + DateTime.Now.ToString("G"));
}
}