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")); } }