Exemplo n.º 1
0
        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;
                }
            }
        }
Exemplo n.º 2
0
        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));
        }
Exemplo n.º 3
0
        /// <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"));
            }
        }