/* * Copyright (C) 2018 dimercur * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "commonitor.h" #include #include #include #include #include #include #include #include #include /* * @brief Constants used for sending commands to monitor */ const string LABEL_MONITOR_ANSWER_ACK = "AACK"; const string LABEL_MONITOR_ANSWER_NACK = "ANAK"; const string LABEL_MONITOR_ANSWER_COM_ERROR = "ACER"; const string LABEL_MONITOR_ANSWER_TIMEOUT = "ATIM"; const string LABEL_MONITOR_ANSWER_CMD_REJECTED = "ACRJ"; const string LABEL_MONITOR_MESSAGE = "MSSG"; const string LABEL_MONITOR_CAMERA_OPEN = "COPN"; const string LABEL_MONITOR_CAMERA_CLOSE = "CCLS"; const string LABEL_MONITOR_CAMERA_IMAGE = "CIMG"; const string LABEL_MONITOR_CAMERA_ARENA_ASK = "CASA"; const string LABEL_MONITOR_CAMERA_ARENA_INFIRME = "CAIN"; const string LABEL_MONITOR_CAMERA_ARENA_CONFIRM = "CACO"; const string LABEL_MONITOR_CAMERA_POSITION_COMPUTE = "CPCO"; const string LABEL_MONITOR_CAMERA_POSITION_STOP = "CPST"; const string LABEL_MONITOR_CAMERA_POSITION = "CPOS"; const string LABEL_MONITOR_ROBOT_COM_OPEN = "ROPN"; const string LABEL_MONITOR_ROBOT_COM_CLOSE = "RCLS"; const string LABEL_MONITOR_ROBOT_PING = "RPIN"; const string LABEL_MONITOR_ROBOT_RESET = "RRST"; const string LABEL_MONITOR_ROBOT_START_WITHOUT_WD = "RSOW"; const string LABEL_MONITOR_ROBOT_START_WITH_WD = "RSWW"; const string LABEL_MONITOR_ROBOT_RELOAD_WD = "RLDW"; const string LABEL_MONITOR_ROBOT_MOVE = "RMOV"; const string LABEL_MONITOR_ROBOT_TURN = "RTRN"; const string LABEL_MONITOR_ROBOT_GO_FORWARD = "RGFW"; const string LABEL_MONITOR_ROBOT_GO_BACKWARD = "RGBW"; const string LABEL_MONITOR_ROBOT_GO_LEFT = "RGLF"; const string LABEL_MONITOR_ROBOT_GO_RIGHT = "RGRI"; const string LABEL_MONITOR_ROBOT_STOP = "RSTP"; const string LABEL_MONITOR_ROBOT_POWEROFF = "RPOF"; const string LABEL_MONITOR_ROBOT_BATTERY_LEVEL = "RBLV"; const string LABEL_MONITOR_ROBOT_GET_BATTERY = "RGBT"; const string LABEL_MONITOR_ROBOT_GET_STATE = "RGST"; const string LABEL_MONITOR_ROBOT_CURRENT_STATE = "RCST"; const string LABEL_SEPARATOR_CHAR = ":"; /** * Create a server and open a socket over TCP * * @param port Port used for communication * @return Socket number * @throw std::runtime_error if it fails */ int ComMonitor::Open(int port) { struct sockaddr_in server; socketFD = socket(AF_INET, SOCK_STREAM, 0); if (socketFD < 0) { throw std::runtime_error{"Can not create socket"}; } server.sin_addr.s_addr = INADDR_ANY; server.sin_family = AF_INET; server.sin_port = htons(port); if (bind(socketFD, (struct sockaddr *) &server, sizeof (server)) < 0) { cerr<<"["<<__PRETTY_FUNCTION__<<"] Can not bind socket ("<c_str() << endl; write(clientID, str.c_str(), str.length()); delete(&msg); // Call user method after write Write_Post(); } /** * Receive a message from monitor * * @return Message received from monitor * @attention Message provided is produced by the method. You must delete it when you are done using it * @warning Read is not thread safe : check that multiple tasks can't access this method simultaneously */ Message *ComMonitor::Read() { char length = 0; string s; char data; bool endReception = false; Message *msg; // Call user method before read Read_Pre(); if (clientID > 0) { while (!endReception) { if ((length = recv(clientID, (void*) &data, 1, MSG_WAITALL)) > 0) { if (data != '\n') { s += data; } else endReception = true; } } if (length <= 0) msg = new Message(MESSAGE_MONITOR_LOST); else { msg = StringToMessage(s); } } // Call user method after read Read_Post(); return msg; } /** * Method used internally to convert a message content to a string that can be sent over TCP * @param msg Message to be converted * @return A string, image of the message */ string ComMonitor::MessageToString(Message &msg) { int id; string str; Message *localMsg = &msg; Position pos; id = msg.GetID(); switch (id) { case MESSAGE_ANSWER_ACK : str.append(LABEL_MONITOR_ANSWER_ACK); break; case MESSAGE_ANSWER_NACK: str.append(LABEL_MONITOR_ANSWER_NACK); break; case MESSAGE_ANSWER_ROBOT_TIMEOUT: str.append(LABEL_MONITOR_ANSWER_TIMEOUT); break; case MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND: str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); break; case MESSAGE_ANSWER_ROBOT_ERROR: str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); break; case MESSAGE_ANSWER_COM_ERROR: str.append(LABEL_MONITOR_ANSWER_COM_ERROR); break; case MESSAGE_CAM_POSITION: pos = ((MessagePosition*) & msg)->GetPosition(); str.append(LABEL_MONITOR_CAMERA_POSITION + LABEL_SEPARATOR_CHAR + to_string(pos.robotId) + ";" + to_string(pos.angle) + ";" + to_string(pos.center.x) + ";" + to_string(pos.center.y) + ";" + to_string(pos.direction.x) + ";" + to_string(pos.direction.y)); break; case MESSAGE_CAM_IMAGE: str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) & msg)->GetImage()->ToBase64()); break; case MESSAGE_ROBOT_BATTERY_LEVEL: str.append(LABEL_MONITOR_ROBOT_BATTERY_LEVEL + LABEL_SEPARATOR_CHAR + to_string(((MessageBattery*) & msg)->GetLevel())); break; case MESSAGE_ROBOT_STATE_BUSY: str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "1"); break; case MESSAGE_ROBOT_STATE_NOT_BUSY: str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "0"); break; case MESSAGE_LOG: str.append(LABEL_MONITOR_MESSAGE + LABEL_SEPARATOR_CHAR + ((MessageString*) & msg)->GetString()); break; case MESSAGE_EMPTY: str.append(""); //empty string break; default: cerr<<"["<<__PRETTY_FUNCTION__<<"] (from ComMonitor::Write): Invalid message to send ("<