From 34d1cb6bd810d4dab8017c2d5826c3a2cd161f9e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20DI=20MERCURIO?= Date: Wed, 19 Dec 2018 09:15:42 +0100 Subject: [PATCH] passage au tout objet en cours --- .../superviseur-robot/lib/comgui.cpp | 189 ------- .../superviseur-robot/lib/commonitor.cpp | 336 +++++++++++++ .../lib/{comgui.h => commonitor.h} | 52 +- .../superviseur-robot/lib/comrobot.cpp | 334 +++++++++++++ .../superviseur-robot/lib/comrobot.h | 143 ++++++ .../raspberry/superviseur-robot/lib/img.cpp | 17 +- .../raspberry/superviseur-robot/lib/img.h | 19 +- .../superviseur-robot/lib/messages.cpp | 469 ++++++++++++++++-- .../superviseur-robot/lib/messages.h | 142 ++---- .../nbproject/Makefile-Debug.mk | 20 +- .../nbproject/Makefile-Debug__Pthread_.mk | 131 +++++ .../nbproject/Makefile-Debug__RPI_.mk | 20 +- .../nbproject/Makefile-Release.mk | 20 +- .../nbproject/Makefile-impl.mk | 2 +- .../nbproject/Makefile-variables.mk | 8 + .../nbproject/Package-Debug__Pthread_.bash | 76 +++ .../nbproject/configurations.xml | 179 +++++++ .../nbproject/private/Makefile-variables.mk | 1 + .../nbproject/private/configurations.xml | 37 ++ .../nbproject/private/private.xml | 8 +- .../superviseur-robot/nbproject/project.xml | 8 +- software/raspberry/superviseur-robot/tasks.h | 4 +- .../superviseur-robot/tasks_pthread.cpp | 263 ++++++++++ .../superviseur-robot/tasks_pthread.h | 104 ++++ 24 files changed, 2212 insertions(+), 370 deletions(-) delete mode 100644 software/raspberry/superviseur-robot/lib/comgui.cpp create mode 100644 software/raspberry/superviseur-robot/lib/commonitor.cpp rename software/raspberry/superviseur-robot/lib/{comgui.h => commonitor.h} (64%) create mode 100644 software/raspberry/superviseur-robot/lib/comrobot.cpp create mode 100644 software/raspberry/superviseur-robot/lib/comrobot.h create mode 100644 software/raspberry/superviseur-robot/nbproject/Makefile-Debug__Pthread_.mk create mode 100644 software/raspberry/superviseur-robot/nbproject/Package-Debug__Pthread_.bash create mode 100644 software/raspberry/superviseur-robot/tasks_pthread.cpp create mode 100644 software/raspberry/superviseur-robot/tasks_pthread.h diff --git a/software/raspberry/superviseur-robot/lib/comgui.cpp b/software/raspberry/superviseur-robot/lib/comgui.cpp deleted file mode 100644 index 0ad3141..0000000 --- a/software/raspberry/superviseur-robot/lib/comgui.cpp +++ /dev/null @@ -1,189 +0,0 @@ -/* - * 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 "comgui.h" -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -/* - * Constants used for sending commands to gui - */ -const string LABEL_GUI_ANGULAR_POSITION = "AngularPosition"; -const string LABEL_GUI_ANGULAR_SPEED = "AngularSpeed"; -const string LABEL_GUI_BATTERY_LEVEL = "Battery"; -const string LABEL_GUI_LINEAR_SPEED = "LinearSpeed"; -const string LABEL_GUI_USER_PRESENCE = "User"; -const string LABEL_GUI_BETA_ANGLE = "Beta"; -const string LABEL_GUI_TORQUE = "Torque"; -const string LABEL_GUI_EMERGENCY_STOP = "Emergency"; -const string LABEL_GUI_LOG = "Log"; - -/** - * 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 ComGui::Open(int port) { - struct sockaddr_in server; - - socketFD = socket(AF_INET, SOCK_STREAM, 0); - if (socketFD < 0) { - throw std::runtime_error{"ComGui::Open : 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) { - throw std::runtime_error{"ComGui::Open : Can not bind socket on port " + std::to_string(port)}; - } - - listen(socketFD, 1); - - return socketFD; -} - -/** - * Close socket and server - */ -void ComGui::Close() { - close(socketFD); - - socketFD = -1; -} - -/** - * Wait for a client to connect - * @return Client number - * @throw std::runtime_error if it fails - */ -int ComGui::AcceptClient() { - struct sockaddr_in client; - int c = sizeof (struct sockaddr_in); - - clientID = accept(socketFD, (struct sockaddr *) &client, (socklen_t*) & c); - - if (clientID < 0) - throw std::runtime_error { - "ComGui::AcceptClient : Accept failed" - }; - - return clientID; -} - -/** - * Send a message to GUI - * - * @param msg Message to send to GUI - * @attention Message given in parameter will be destroyed (delete) after being sent. No need for user to delete message after that. - * @warning Write is not thread safe : check that multiple tasks can't access this method simultaneously - */ -void ComGui::Write(Message* msg) { - string *str; - - // Call user method before Write - Write_Pre(); - - /* Convert message to string to send to GUI */ - str = MessageToString(msg); - - //cout << "Message sent to GUI: " << str->c_str() << endl; - write(clientID, str->c_str(), str->length()); - - delete(str); - - // Call user method after write - Write_Post(); -} - -/** - * 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 *ComGui::MessageToString(Message *msg) { - int id; - string *str; - - if (msg != NULL) { - id = msg->GetID(); - - switch (id) { - case MESSAGE_ANGLE_POSITION: - str = new string(LABEL_GUI_ANGULAR_POSITION + "=" + to_string(((MessageFloat*) msg)->GetValue()) + "\n"); - replace(str->begin(), str->end(), '.', ','); // Mono C# require float to have a , instead of a . - break; - case MESSAGE_ANGULAR_SPEED: - str = new string(LABEL_GUI_ANGULAR_SPEED + "=" + to_string(((MessageFloat*) msg)->GetValue()) + "\n"); - replace(str->begin(), str->end(), '.', ','); // Mono C# require float to have a , instead of a . - break; - case MESSAGE_BATTERY: - str = new string(LABEL_GUI_BATTERY_LEVEL + "=" + to_string(((MessageFloat*) msg)->GetValue()) + "\n"); - replace(str->begin(), str->end(), '.', ','); // Mono C# require float to have a , instead of a . - break; - case MESSAGE_BETA: - str = new string(LABEL_GUI_BETA_ANGLE + "=" + to_string(((MessageFloat*) msg)->GetValue()) + "\n"); - replace(str->begin(), str->end(), '.', ','); // Mono C# require float to have a , instead of a . - break; - case MESSAGE_LINEAR_SPEED: - str = new string(LABEL_GUI_LINEAR_SPEED + "=" + to_string(((MessageFloat*) msg)->GetValue()) + "\n"); - replace(str->begin(), str->end(), '.', ','); // Mono C# require float to have a , instead of a . - break; - case MESSAGE_TORQUE: - str = new string(LABEL_GUI_TORQUE + "=" + to_string(((MessageFloat*) msg)->GetValue()) + "\n"); - replace(str->begin(), str->end(), '.', ','); // Mono C# require float to have a , instead of a . - break; - case MESSAGE_EMERGENCY_STOP: - str = new string(LABEL_GUI_EMERGENCY_STOP + "="); - if (((MessageBool*) msg)->GetState()) - str->append("True\n"); - else - str->append("False\n"); - break; - case MESSAGE_USER_PRESENCE: - str = new string(LABEL_GUI_USER_PRESENCE + "="); - if (((MessageBool*) msg)->GetState()) - str->append("True\n"); - else - str->append("False\n"); - break; - case MESSAGE_EMPTY: - str = new string(""); //empty string - break; - case MESSAGE_LOG: - str = new string(LABEL_GUI_LOG + "=" + ((MessageString*) msg)->GetString() + "\n"); - break; - default: - str = new string(""); //empty string - break; - } - } - - return str; -} diff --git a/software/raspberry/superviseur-robot/lib/commonitor.cpp b/software/raspberry/superviseur-robot/lib/commonitor.cpp new file mode 100644 index 0000000..9bb0d96 --- /dev/null +++ b/software/raspberry/superviseur-robot/lib/commonitor.cpp @@ -0,0 +1,336 @@ +/* + * 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_LOST_DMB= "ATIM"; +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{"ComMonitor::Open : 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) { + throw std::runtime_error{"ComMonitor::Open : Can not bind socket on port " + std::to_string(port)}; + } + + listen(socketFD, 1); + + return socketFD; +} + +/** + * Close socket and server + */ +void ComMonitor::Close() { + close(socketFD); + + socketFD = -1; +} + +/** + * Wait for a client to connect + * @return Client number + * @throw std::runtime_error if it fails + */ +int ComMonitor::AcceptClient() { + struct sockaddr_in client; + int c = sizeof (struct sockaddr_in); + + clientID = accept(socketFD, (struct sockaddr *) &client, (socklen_t*) & c); + + if (clientID < 0) + throw std::runtime_error { + "ComMonitor::AcceptClient : Accept failed" + }; + + return clientID; +} + +/** + * Send a message to monitor + * + * @param msg Message to send to monitor + * @attention Message given in parameter will be destroyed (delete) after being sent. No need for user to delete message after that. + * @warning Write is not thread safe : check that multiple tasks can't access this method simultaneously + */ +void ComMonitor::Write(Message &msg) { + string str; + + // Call user method before Write + Write_Pre(); + + /* Convert message to string to send to monitor */ + str = MessageToString(msg); + + //cout << "Message sent to monitor: " << str->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: + switch (((MessageAnswer*)localMsg)->GetAnswer()) { + case ANSWER_ACK: + str.append(LABEL_MONITOR_ANSWER_ACK); + break; + case ANSWER_NACK: + str.append(LABEL_MONITOR_ANSWER_NACK); + break; + case ANSWER_LOST_ROBOT: + str.append(LABEL_MONITOR_ANSWER_LOST_DMB); + break; + case ANSWER_ROBOT_TIMEOUT: + str.append(LABEL_MONITOR_ANSWER_TIMEOUT); + break; + case ANSWER_ROBOT_UNKNOWN_COMMAND: + str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); + break; + case ANSWER_ROBOT_ERROR: + str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); + break; + default: + str.append(LABEL_MONITOR_ANSWER_NACK); + }; + + break; + case MESSAGE_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_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_CURRENT_STATE: + str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + to_string(((MessageState*) &msg)->GetState())); + 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: + throw std::runtime_error + { + "ComMonitor::MessageToString (from ComMonitor::Write): Invalid message to send (" + msg.ToString() + }; + } + + str.append("\n"); + + return str; +} + +/** + * Method used internally to convert a string received over TCP to a message + * @param s String containing message + * @return A message, image of the string + */ +Message *ComMonitor::StringToMessage(string &s) { + Message *msg; + size_t pos; + string org =s; + string tokenCmd; + string tokenData; + + /* Separate command from data if string contains a ':' */ + if ((pos=org.find(LABEL_SEPARATOR_CHAR)) != string::npos) { + tokenCmd = org.substr(0,pos); + org.erase(0,pos+1); + tokenData=org; + } else tokenCmd=org; + + /* Convert command to message */ + if (tokenCmd.find(LABEL_MONITOR_ROBOT_MOVE)!= string::npos) { + msg = new MessageInt(MESSAGE_ROBOT_MOVE,stoi(tokenData)); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_TURN)!= string::npos) { + msg = new MessageInt(MESSAGE_ROBOT_TURN,stoi(tokenData)); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITHOUT_WD)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_START_WITHOUT_WD); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITH_WD)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_START_WITH_WD); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RELOAD_WD)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_RELOAD_WD); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_PING)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_PING); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RESET)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_RESET); + } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_ASK)!= string::npos) { + msg = new Message(MESSAGE_ASK_ARENA); + } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_CONFIRM)!= string::npos) { + msg = new Message(MESSAGE_ARENA_CONFIRM); + } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_INFIRME)!= string::npos) { + msg = new Message(MESSAGE_ARENA_INFIRM); + } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_CLOSE)!= string::npos) { + msg = new Message(MESSAGE_CAM_CLOSE); + } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_OPEN)!= string::npos) { + msg = new Message(MESSAGE_CAM_OPEN); + } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_COMPUTE)!= string::npos) { + msg = new Message(MESSAGE_COMPUTE_POSITION); + } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_STOP)!= string::npos) { + msg = new Message(MESSAGE_STOP_COMPUTE_POSITION); + } else if (tokenCmd.find(LABEL_MONITOR_MESSAGE)!= string::npos) { + msg = new MessageString(MESSAGE_LOG,tokenData); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_CLOSE)!= string::npos) { + msg = new Message(MESSAGE_CLOSE_COM); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_OPEN)!= string::npos) { + msg = new Message(MESSAGE_OPEN_COM); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_BATTERY)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_GET_BATTERY); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_STATE)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_GET_STATE); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_FORWARD)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_GO_FORWARD); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_BACKWARD)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_GO_BACK); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_LEFT)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_GO_LEFT); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_RIGHT)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_GO_RIGHT); + } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_POWEROFF)!= string::npos) { + msg = new Message(MESSAGE_ROBOT_POWEROFF); + } else { + msg = new Message(MESSAGE_EMPTY); + } + + return msg; +} diff --git a/software/raspberry/superviseur-robot/lib/comgui.h b/software/raspberry/superviseur-robot/lib/commonitor.h similarity index 64% rename from software/raspberry/superviseur-robot/lib/comgui.h rename to software/raspberry/superviseur-robot/lib/commonitor.h index 351a0d7..9af1a5d 100644 --- a/software/raspberry/superviseur-robot/lib/comgui.h +++ b/software/raspberry/superviseur-robot/lib/commonitor.h @@ -15,8 +15,8 @@ * along with this program. If not, see . */ -#ifndef __COMGUI_H__ -#define __COMGUI_H__ +#ifndef __COMMONITOR_H__ +#define __COMMONITOR_H__ #include "messages.h" #include @@ -24,22 +24,22 @@ using namespace std; /** - * Class used for generating a server and communicating through it with GUI + * Class used for generating a server and communicating through it with monitor * - * @brief Communication class with GUI (server) + * @brief Communication class with monitor (server) * */ -class ComGui { +class ComMonitor { public: /** * Constructor */ - ComGui() {} + ComMonitor() {} /** * Destructor */ - virtual ~ComGui() {} + virtual ~ComMonitor() {} /** * Create a server and open a socket over TCP @@ -63,13 +63,13 @@ public: int AcceptClient(); /** - * Send a message to GUI + * Send a message to monitor * - * @param msg Message to send to GUI + * @param msg Message to send to monitor * @attention Message given in parameter will be destroyed (delete) after being sent. No need for user to delete message after that. * @warning Write is not thread safe : check that multiple tasks can't access this method simultaneously */ - void Write(Message* msg); + void Write(Message &msg); /** * Function called at beginning of Write method @@ -82,6 +82,27 @@ public: * Use it to do some synchronization (release of mutex, for example) */ virtual void 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 *Read(); + + /** + * Function called at beginning of Read method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Read_Pre() {} + + /** + * Function called at end of Read method + * Use it to do some synchronization (release of mutex, for example) + */ + virtual void Read_Post() {} protected: /** * Socket descriptor @@ -98,7 +119,14 @@ protected: * @param msg Message to be converted * @return A string, image of the message */ - string *MessageToString(Message *msg); + string MessageToString(Message &msg); + + /** + * Method used internally to convert a string received over TCP to a message + * @param s String containing message + * @return A message, image of the string + */ + Message *StringToMessage(string &s); }; -#endif /* __COMGUI_H__ */ +#endif /* __COMMONITOR_H__ */ diff --git a/software/raspberry/superviseur-robot/lib/comrobot.cpp b/software/raspberry/superviseur-robot/lib/comrobot.cpp new file mode 100644 index 0000000..347a248 --- /dev/null +++ b/software/raspberry/superviseur-robot/lib/comrobot.cpp @@ -0,0 +1,334 @@ +/* + * 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 "comrobot.h" + +#include +#include +#include +#include +#include + +#include +#include + +#ifdef __FOR_PC__ +#define USART_FILENAME "/dev/ttyUSB0" +#else +#define USART_FILENAME "/dev/ttyS0" +#endif /* __FOR_PC__ */ + +/* + * Constants to be used for communicating with robot. Contains command tag + */ +typedef enum { + LABEL_ANGLE_POSITION = 'p', + LABEL_ANGULAR_SPEED = 's', + LABEL_BATTERY_LEVEL = 'b', + LABEL_BETA_ANGLE = 'v', + LABEL_USER_PRESENCE = 'u', + + LABEL_TORQUE = 'c', + LABEL_EMERGENCY_STOP = 'a' +} LabelRobot; + +/** + * Open serial link with robot + * @return File descriptor + * @throw std::runtime_error if it fails + */ +int ComRobot::Open() { + fd = open(USART_FILENAME, O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode + if (fd == -1) { + //ERROR - CAN'T OPEN SERIAL PORT + throw std::runtime_error{"Error - Unable to open UART " + string(USART_FILENAME) + ". Ensure it is not in use by another application"}; + exit(EXIT_FAILURE); + } + + //Configuration of the serial port 115 520 Bauds + struct termios options; + tcgetattr(fd, &options); + options.c_cflag = B115200 | CS8 | CLOCAL | CREAD; //fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) + //printf ("W=%02X ", receivedChar); + + if (rxLength <= -1) { + this->lostCom = true; + printf("Warning: communication lost in ComStm32::Read\n"); + msg = new Message(); + + return msg; + } else if (rxLength == 0) { + // nothing to do + } else if (receivedChar == '<') { // start of frame received + i = 0; + + do { + rxLength = read(this->fd, (void*) &rxBuffer[i], 6 - i); //Filestream, buffer to store in, number of bytes to read (max) + + if (rxLength >= 0) + i = i + rxLength; + else { + printf("Error while reading (%i)", rxLength); + + return NULL; + } + } while (i < 6); + + if (rxBuffer[5] == '\n') { + messageComplete = true; + } + } + } + + /* Treatment of received message */ + msg = CharToMessage(rxBuffer); + + /* Call Post method for read */ + Read_Post(); + + return msg; +} + +/** + * Convert an array of char to its message representation (when receiving data from stm32) + * @param bytes Array of char + * @return Message corresponding to received array of char + */ +Message* ComRobot::CharToMessage(unsigned char *bytes) { + Message *msg = __null; + MessageFloat *msgf; + MessageBool *msgb; + + switch (bytes[0]) { + case LABEL_ANGLE_POSITION: + msgf = new MessageFloat(); + msgf->SetID(MESSAGE_ANGLE_POSITION); + msgf->SetValue(CharToFloat(&bytes[1])); + msg = (Message*) msgf; + + break; + case LABEL_ANGULAR_SPEED: + msgf = new MessageFloat(); + msgf->SetID(MESSAGE_ANGULAR_SPEED); + msgf->SetValue(CharToFloat(&bytes[1])); + msg = (Message*) msgf; + + break; + case LABEL_BATTERY_LEVEL: + msgf = new MessageFloat(); + msgf->SetID(MESSAGE_BATTERY); + msgf->SetValue(CharToFloat(&bytes[1])); + msg = (Message*) msgf; + + break; + case LABEL_BETA_ANGLE: + msgf = new MessageFloat(); + msgf->SetID(MESSAGE_BETA); + msgf->SetValue(CharToFloat(&bytes[1])); + msg = (Message*) msgf; + + break; + case LABEL_USER_PRESENCE: + msgb = new MessageBool(); + msgb->SetID(MESSAGE_USER_PRESENCE); + msgb->SetState(CharToBool(&bytes[1])); + msg = (Message*) msgb; + + break; + default: + printf("Unknown message received from robot (%i)\n", bytes[0]); + fflush(stdout); + msg = new Message(); + } + + if (msg == NULL) { + printf("Message is null (%02X)\n", bytes[0]); + fflush(stdout); + msg = new Message(); + } + + return msg; +} + +/** + * Convert an array of char to float + * @param bytes Array of char, containing a binary image of a float + * @return Float value + */ +float ComRobot::CharToFloat(unsigned char *bytes) { + unsigned long value; + + union { + unsigned char buffer[4]; + float f; + } convert; + + convert.buffer[0] = bytes[0]; + convert.buffer[1] = bytes[1]; + convert.buffer[2] = bytes[2]; + convert.buffer[3] = bytes[3]; + + //value = (bytes[3] << 24) | (bytes[2] << 16) | (bytes[1] << 8) | (bytes[0]); + + return convert.f; +} + +/** + * Convert an array of char to integer + * @param bytes Array of char, containing a binary image of an integer + * @return Integer value + */ +unsigned int ComRobot::CharToInt(unsigned char *bytes) { + unsigned long value; + + value = (bytes[3] << 24) | (bytes[2] << 16) | (bytes[1] << 8) | (bytes[0]); + + return (unsigned int) value; +} + +/** + * Convert an array of char to boolean + * @param bytes Array of char, containing a binary image of a boolean + * @return Boolean value + */ +bool ComRobot::CharToBool(unsigned char *bytes) { + unsigned long value; + + value = (bytes[3] << 24) | (bytes[2] << 16) | (bytes[1] << 8) | (bytes[0]); + + if (value == 0) return false; + + else return true; +} + +/** + * Send a message to robot + * @param msg Message to send to robot + * @return 1 if success, 0 otherwise + * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself + * @attention Write is blocking until message is written into buffer (linux side) + * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously + */ +int ComRobot::Write(Message* msg) { + unsigned char buffer[7]; + int ret_val = 0; + + MessageToChar(msg, buffer); + + Write_Pre(); + + if (this->fd != -1) { + int count = write(this->fd, &buffer[0], 7); //Filestream, bytes to write, number of bytes to write + if (count < 0) { + printf("Warning: UART TX error in ComStm32::Write\n"); + } else { + ret_val = 1; + } + } + + // deallocation of msg + delete(msg); + + Write_Post(); + + return ret_val; +} + +/** + * Convert a message to its array of char representation (for sending command to stm32) + * @param msg Message to be sent to robot + * @param buffer Array of char, image of message to send + */ +void ComRobot::MessageToChar(Message *msg, unsigned char *buffer) { + float val_f; + int val_i; + unsigned char *b; + + buffer[0] = '<'; + buffer[6] = '\n'; + + switch (msg->GetID()) { + case MESSAGE_TORQUE: + buffer[1] = LABEL_TORQUE; + val_f = ((MessageFloat*) msg)->GetValue(); + b = (unsigned char *) &val_f; + + break; + case MESSAGE_EMERGENCY_STOP: + buffer[1] = LABEL_EMERGENCY_STOP; + if (((MessageBool*) msg)->GetState()) + val_i = 1; + else + val_i = 0; + b = (unsigned char *) &val_i; + + break; + default: + printf("Invalid message to send"); + val_i = 0; + b = (unsigned char *) &val_i; + } + + buffer[2] = b[0]; + buffer[3] = b[1]; + buffer[4] = b[2]; + buffer[5] = b[3]; +} + diff --git a/software/raspberry/superviseur-robot/lib/comrobot.h b/software/raspberry/superviseur-robot/lib/comrobot.h new file mode 100644 index 0000000..96cae04 --- /dev/null +++ b/software/raspberry/superviseur-robot/lib/comrobot.h @@ -0,0 +1,143 @@ +/* + * 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 . + */ + +#ifndef __COMROBOT_H__ +#define __COMROBOT_H__ + +#include "messages.h" + +using namespace std; + +/** + * Class used for communicating with robot over serial + * + * @brief Communication class with robot + * + */ +class ComRobot { +public: + /** + * Constructor + */ + ComRobot() {} + + /** + * Destructor + */ + virtual ~ComRobot() {} + + /** + * Open serial link with robot + * @return File descriptor + * @throw std::runtime_error if it fails + */ + int Open(); + + /** + * Close serial link + * @return Success if above 0, failure if below 0 + */ + int Close(); + + /** + * Get a message from robot + * @return Message currently received + * @attention A message object is created (new) when receiving data from robot. You MUST remember to destroy is (delete) after use + * @attention Read method is blocking until a message is received + * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously + */ + Message* Read(); + + /** + * Send a message to robot + * @param msg Message to send to robot + * @return 1 if success, 0 otherwise + * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself + * @attention Write is blocking until message is written into buffer (linux side) + * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously + */ + int Write(Message* msg); + + /** + * Function called at beginning of Read method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Read_Pre() {} + + /** + * Function called at end of Read method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Read_Post() {} + + /** + * Function called at beginning of Write method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Write_Pre() {} + + /** + * Function called at end of Write method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Write_Post() {} + + static Message *Ping(); + +protected: + /** + * Serial link file descriptor + */ + int fd; + + /** + * Convert an array of char to float + * @param bytes Array of char, containing a binary image of a float + * @return Float value + */ + float CharToFloat(unsigned char *bytes); + + /** + * Convert an array of char to boolean + * @param bytes Array of char, containing a binary image of a boolean + * @return Boolean value + */ + bool CharToBool(unsigned char *bytes); + + /** + * Convert an array of char to integer + * @param bytes Array of char, containing a binary image of an integer + * @return Integer value + */ + unsigned int CharToInt(unsigned char *bytes); + + /** + * Convert an array of char to its message representation (when receiving data from stm32) + * @param bytes Array of char + * @return Message corresponding to received array of char + */ + Message* CharToMessage(unsigned char *bytes); + + /** + * Convert a message to its array of char representation (for sending command to stm32) + * @param msg Message to be sent to robot + * @param buffer Array of char, image of message to send + */ + void MessageToChar(Message *msg, unsigned char *buffer); +}; + +#endif /* __COMROBOT_H__ */ diff --git a/software/raspberry/superviseur-robot/lib/img.cpp b/software/raspberry/superviseur-robot/lib/img.cpp index 15d0bbf..4b7ae3a 100644 --- a/software/raspberry/superviseur-robot/lib/img.cpp +++ b/software/raspberry/superviseur-robot/lib/img.cpp @@ -18,7 +18,6 @@ #include "img.h" bool Arene::empty() { - if ((this->arene.height==0) || (this->arene.width==0)) return true; else return false; } @@ -27,6 +26,14 @@ Img::Img(ImageMat imgMatrice) { this->img = imgMatrice.clone(); } +string Img::ToString() { + return "Image size: "+this->img.cols+"x"this->img.rows+" (dim="+this->img.dims+")"; +} + +Img* Img::Copy() { + return new Img(this->img); +} + float Img::calculAngle(Position robot) { float a = robot.direction.x - robot.center.x; float b = robot.direction.y - robot.center.y ; @@ -93,6 +100,14 @@ Jpg Img::toJpg() { return imgJpg; } +string Img::ToBase64() { + string imgBase64; + Jpg imgJpg = toJpg(); + + /* faire la convertion Jpg vers base 64 */ + return imgBase64; +} + std::list Img::search_robot(Arene monArene) { std::list robotsFind; diff --git a/software/raspberry/superviseur-robot/lib/img.h b/software/raspberry/superviseur-robot/lib/img.h index 1ddde5a..4e98ea0 100644 --- a/software/raspberry/superviseur-robot/lib/img.h +++ b/software/raspberry/superviseur-robot/lib/img.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -33,16 +34,18 @@ #define ARENA_NOT_DETECTED -1 +using namespace std; + typedef cv::Mat ImageMat; -typedef std::vector Jpg; +typedef vector Jpg; -struct Position { +typedef struct { cv::Point2f center; cv::Point2f direction; float angle; int robotId; -}; +} Position; class Arene { public: @@ -55,7 +58,12 @@ public: class Img { public: Img(ImageMat imgMatrice); + + string ToString(); + Img* Copy(); + Jpg toJpg(); + string ToBase64(); Arene search_arena(); int draw_robot(Position robot); @@ -63,11 +71,12 @@ public: int draw_arena(Arene areneToDraw); std::list search_robot(Arene monArene); + #ifdef __WITH_ARUCO__ - std::list search_aruco(Arene monArene = NULL); + list search_aruco(Arene monArene = NULL); #endif // __WITH_ARUCO__ private: - cv::Mat img; + ImageMat img; #ifdef __WITH_ARUCO__ Ptr dictionary; diff --git a/software/raspberry/superviseur-robot/lib/messages.cpp b/software/raspberry/superviseur-robot/lib/messages.cpp index ede1297..8957c62 100644 --- a/software/raspberry/superviseur-robot/lib/messages.cpp +++ b/software/raspberry/superviseur-robot/lib/messages.cpp @@ -21,13 +21,14 @@ #include /* - * Constants used with ToString method, for printing message id + * @brief Constants used with ToString method, for printing message id */ const string MESSAGE_ID_STRING[] = { "Empty", "Log", "Answer", + "Monitor connection lost", "Open serial com", "Close serial com", "Open camera", @@ -37,7 +38,7 @@ const string MESSAGE_ID_STRING[] = { "Arena infirmed", "Compute position", "Stop compute position", - "Position, + "Position", "Image", "Robot ping", "Robot reset", @@ -56,7 +57,19 @@ const string MESSAGE_ID_STRING[] = { "Robot battery level", "Robot get state", "Robot current state" +}; +/* + * @brief Constants used with ToString method, for printing answer id + */ +const string ANSWER_ID_STRING[] = { + "Acknowledge", + "Not Acknowledge", + "Robot lost", + "Timeout error", + "Unknown command", + "Invalid or refused command", + "Checksum error" }; /** @@ -66,12 +79,29 @@ Message::Message() { this->messageID = MESSAGE_EMPTY; } +/** + * Create a new, empty message + */ +Message::Message(MessageID id) { + SetID(id); +} + /** * Destroy message */ Message::~Message() { } +/** + * Set message ID + * @param id Message ID + */ +void Message::SetID(MessageID id) { + if (CheckID(id)) { + this->messageID = id; + } else throw std::runtime_error {"Invalid message id for Message"}; +} + /** * Translate content of message into a string that can be displayed * @return A string describing message contents @@ -99,6 +129,7 @@ Message* Message::Copy() { */ bool Message::CheckID(MessageID id) { if ((id != MESSAGE_EMPTY) && + (id != MESSAGE_MONITOR_LOST) && (id != MESSAGE_ARENA_CONFIRM) && (id != MESSAGE_ARENA_INFIRM) && (id != MESSAGE_ASK_ARENA) && @@ -125,23 +156,23 @@ bool Message::CheckID(MessageID id) { } else return true; } -/* MessageFloat */ +/* MessageInt */ /** - * Create a new, empty float message + * Create a new, empty int message */ -MessageFloat::MessageFloat() { +MessageInt::MessageInt() { value = 0.0; } /** - * Create a new float message, with given ID and value + * Create a new int message, with given ID and value * @param id Message ID * @param val Message value - * @throw std::runtime_error if message ID is incompatible with float data + * @throw std::runtime_error if message ID is incompatible with int data */ -MessageFloat::MessageFloat(MessageID id, float val) { - MessageFloat::SetID(id); +MessageInt::MessageInt(MessageID id, int val) { + MessageInt::SetID(id); value = val; } @@ -151,12 +182,12 @@ MessageFloat::MessageFloat(MessageID id, float val) { * @param id Message ID * @throw std::runtime_error if message ID is incompatible with float data */ -void MessageFloat::SetID(MessageID id) { +void MessageInt::SetID(MessageID id) { if (CheckID(id)) messageID = id; else throw std::runtime_error { - "Invalid message id for MessageFloat" + "Invalid message id for MessageInt" }; } @@ -164,7 +195,7 @@ void MessageFloat::SetID(MessageID id) { * Translate content of message into a string that can be displayed * @return A string describing message contents */ -string MessageFloat::ToString() { +string MessageInt::ToString() { if (CheckID(this->messageID)) return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nValue: " + to_string(this->value); else @@ -172,11 +203,11 @@ string MessageFloat::ToString() { } /** - * Allocate a new mesage and copy contents of current message + * Allocate a new message and copy contents of current message * @return A message, copy of current */ -Message* MessageFloat::Copy() { - return new MessageFloat(this->messageID, this->value); +Message* MessageInt::Copy() { + return new MessageInt(this->messageID, this->value); } /** @@ -184,13 +215,9 @@ Message* MessageFloat::Copy() { * @param id Message ID * @return true, if message ID is acceptable, false otherwise */ -bool MessageFloat::CheckID(MessageID id) { - if ((id != MESSAGE_ANGLE_POSITION) && - (id != MESSAGE_ANGULAR_SPEED) && - (id != MESSAGE_BATTERY) && - (id != MESSAGE_BETA) && - (id != MESSAGE_LINEAR_SPEED) && - (id != MESSAGE_TORQUE)) { +bool MessageInt::CheckID(MessageID id) { + if ((id != MESSAGE_ROBOT_TURN) && + (id != MESSAGE_ROBOT_MOVE)) { return false; } else return true; } @@ -260,48 +287,62 @@ bool MessageString::CheckID(MessageID id) { } else return true; } -/* class MessageBool */ +/* class MessageImg */ /** - * Create a new, empty boolean message + * Create a new, empty image message */ -MessageBool::MessageBool() { - state = false; +MessageImg::MessageImg() { + image = NULL; } /** - * Create a new boolean message, with given ID and boolean value + * Create a new image message, with given ID and image * @param id Message ID - * @param state Boolean value - * @throw std::runtime_error if message ID is incompatible with boolean data + * @param image Image + * @throw std::runtime_error if message ID is incompatible with image */ -MessageBool::MessageBool(MessageID id, bool state) { - MessageBool::SetID(id); +MessageImg::MessageImg(MessageID id, Img* image) { + MessageImg::SetID(id); + MessageImg::SetImage(image); +} - this->state = state; +/** + * Destroy Image message + */ +MessageImg::~MessageImg() { + delete (this->image); } /** * Set message ID * @param id Message ID - * @throw std::runtime_error if message ID is incompatible with boolean data + * @throw std::runtime_error if message ID is incompatible with image */ -void MessageBool::SetID(MessageID id) { +void MessageImg::SetID(MessageID id) { if (CheckID(id)) messageID = id; else throw std::runtime_error { - "Invalid message id for MessageBool" + "Invalid message id for MessageImg" }; } +/** + * Set message image + * @param image Reference to image object + */ +void MessageImg::SetImage(Img* image) { + this->image = image->Copy(); +} + /** * Translate content of message into a string that can be displayed * @return A string describing message contents */ -string MessageBool::ToString() { +string MessageImg::ToString() { if (CheckID(this->messageID)) - return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nState: \"" + to_string(this->state) + "\""; + return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\n" + this->image->ToString(); else return "Invalid message"; } @@ -310,8 +351,9 @@ string MessageBool::ToString() { * Allocate a new message and copy contents of current message * @return A message, copy of current */ -Message* MessageBool::Copy() { - return new MessageBool(this->messageID, this->state); +Message* MessageImg::Copy() { + + return new MessageImg(this->messageID, this->image->Copy()); } /** @@ -319,9 +361,352 @@ Message* MessageBool::Copy() { * @param id Message ID * @return true, if message ID is acceptable, false otherwise */ -bool MessageBool::CheckID(MessageID id) { - if ((id != MESSAGE_EMERGENCY_STOP) && - (id != MESSAGE_USER_PRESENCE)) { +bool MessageImg::CheckID(MessageID id) { + if (id != MESSAGE_IMAGE) { return false; } else return true; } + +/* class MessageAnswer*/ + +/** + * Create a new, empty answer message + */ +MessageAnswer::MessageAnswer() { + answer=ANSWER_ACK; +} + +/** + * Create a new answer message, with given ID and answer + * @param id Message ID + * @param ans Answer ID + * @throw std::runtime_error if message ID is incompatible with string data + */ +MessageAnswer::MessageAnswer(MessageID id, AnswerID ans) { + MessageAnswer::SetID(id); + MessageAnswer::SetAnswer(ans); +} + +/** + * Set message ID + * @param id Message ID + * @throw std::runtime_error if message ID is incompatible with answer message + */ +void MessageAnswer::SetID(MessageID id) { + if (CheckID(id)) + messageID = id; + else + throw std::runtime_error { + "Invalid message id for MessageAnswer" + }; +} + +/** + * Set message answer + * @param ans Answer ID + * @throw std::runtime_error if answer ID is incompatible with answer data + */ +void MessageAnswer::SetAnswer(AnswerID ans) { + if ((ans != ANSWER_ACK) && + (ans != ANSWER_NACK) && + (ans != ANSWER_LOST_ROBOT) && + (ans != ANSWER_ROBOT_CHECKSUM) && + (ans != ANSWER_ROBOT_ERROR) && + (ans != ANSWER_ROBOT_TIMEOUT) && + (ans != ANSWER_ROBOT_UNKNOWN_COMMAND)) { + this->answer = answer; + } else { + throw std::runtime_error{ + "Invalid answer for MessageAnswer"}; + } +} + +/** + * Translate content of message into a string that can be displayed + * @return A string describing message contents + */ +string MessageAnswer::ToString() { + if (CheckID(this->messageID)) + return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nAnswer: \"" + ANSWER_ID_STRING[this->answer] + "\""; + else + return "Invalid message"; +} + +/** + * Allocate a new message and copy contents of current message + * @return A message, copy of current + */ +Message* MessageAnswer::Copy() { + return new MessageAnswer(this->messageID, this->answer); +} + +/** + * Verify if message ID is compatible with current message type + * @param id Message ID + * @return true, if message ID is acceptable, false otherwise + */ +bool MessageAnswer::CheckID(MessageID id) { + if ((id != MESSAGE_ANSWER)) { + return false; + } else return true; +} + +/* class MessageBattery */ + +/** + * Create a new, empty battery message + */ +MessageBattery::MessageBattery() { + this->level = BATTERY_UNKNOWN; +} + +/** + * Create a new battery message, with given ID and battery level + * @param id Message ID + * @param level Battery level + * @throw std::runtime_error if message ID is incompatible with battery + */ +MessageBattery::MessageBattery(MessageID id, BatteryLevel level) { + MessageBattery::SetID(id); + MessageBattery::SetLevel(level); +} + +/** + * Set message ID + * @param id Message ID + * @throw std::runtime_error if message ID is incompatible with battery + */ +void MessageBattery::SetID(MessageID id) { + if (CheckID(id)) + messageID = id; + else + throw std::runtime_error { + "Invalid message id for MessageBattery" + }; +} + +/** + * Set battery level + * @param level Battery level + */ +void MessageBattery::SetLevel(BatteryLevel level) { + if ((level < BATTERY_UNKNOWN) || (level > BATTERY_FULL)) { + throw std::runtime_error{ + "Invalid battery level for MessageBattery"}; + } else { + this->level = level; + } +} + +/** + * Translate content of message into a string that can be displayed + * @return A string describing message contents + */ +string MessageBattery::ToString() { + string levelString; + + switch (this->level) { + case BATTERY_UNKNOWN: + levelString="Unknown"; + break; + case BATTERY_EMPTY: + levelString="Empty"; + break; + case BATTERY_LOW: + levelString="Low"; + break; + case BATTERY_FULL: + levelString="Full"; + break; + default: + levelString="Invalid"; + } + + if (CheckID(this->messageID)) + return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nBattery level: \"" + levelString + "\""; + else + return "Invalid message"; +} + +/** + * Allocate a new message and copy contents of current message + * @return A message, copy of current + */ +Message* MessageBattery::Copy() { + return new MessageBattery(this->messageID, this->level); +} + +/** + * Verify if message ID is compatible with current message type + * @param id Message ID + * @return true, if message ID is acceptable, false otherwise + */ +bool MessageBattery::CheckID(MessageID id) { + if ((id != MESSAGE_ROBOT_BATTERY_LEVEL)) { + return false; + } else return true; +} + +/* class MessagePosition */ + +/** + * Create a new, empty string message + */ +MessagePosition::MessagePosition() { + this->pos.angle = 0.0; + this->pos.robotId = 0; + this->pos.center.x=0.0; + this->pos.center.y=0.0; + this->pos.direction.x=0.0; + this->pos.direction.y=0.0; +} + +/** + * Create a new string message, with given ID and string + * @param id Message ID + * @param s Message string + * @throw std::runtime_error if message ID is incompatible with string data + */ +MessagePosition::MessagePosition(MessageID id, Position& pos) { + MessagePosition::SetID(id); + MessagePosition::SetPosition(pos); +} + +/** + * Set message ID + * @param id Message ID + * @throw std::runtime_error if message ID is incompatible with string data + */ +void MessagePosition::SetID(MessageID id) { + if (CheckID(id)) + messageID = id; + else + throw std::runtime_error { + "Invalid message id for MessagePosition" + }; +} + +/** + * Set position + * @param pos Reference to position + */ +void MessagePosition::SetPosition(Position& pos) { + this->pos.angle = pos.angle; + this->pos.robotId = pos.robotId; + this->pos.center = pos.center; + this->pos.direction = pos.direction; +} + +/** + * Translate content of message into a string that can be displayed + * @return A string describing message contents + */ +string MessagePosition::ToString() { + if (CheckID(this->messageID)) + return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nPosition: \"" + to_string(this->pos.center.x) + ";" + to_string(this->pos.center.y) + "\""; + else + return "Invalid message"; +} + +/** + * Allocate a new message and copy contents of current message + * @return A message, copy of current + */ +Message* MessagePosition::Copy() { + return new MessagePosition(this->messageID, this->pos); +} + +/** + * Verify if message ID is compatible with current message type + * @param id Message ID + * @return true, if message ID is acceptable, false otherwise + */ +bool MessagePosition::CheckID(MessageID id) { + if ((id != MESSAGE_POSITION)) { + return false; + } else return true; +} + + +/* class MessageState */ + +/** + * Create a new, empty state message + */ +MessageState::MessageState() { + state = ROBOT_NOT_BUSY; +} + +/** + * Create a new string message, with given ID and string + * @param id Message ID + * @param s Message string + * @throw std::runtime_error if message ID is incompatible with string data + */ +MessageState::MessageState(MessageID id, RobotState state) { + MessageState::SetID(id); + MessageState::SetState(state); +} + +/** + * Set message ID + * @param id Message ID + * @throw std::runtime_error if message ID is incompatible with robot state + */ +void MessageState::SetID(MessageID id) { + if (CheckID(id)) + messageID = id; + else + throw std::runtime_error { + "Invalid message id for MessageState" + }; +} + +/** + * Set robot state + * @param state Robot state + */ +void MessageState::SetState(RobotState state) { + if ((state != ROBOT_NOT_BUSY) && (state != ROBOT_BUSY)) { + throw std::runtime_error{ + "Invalid state for MessageState"}; + } else { + this->state = state; + } +} + +/** + * Translate content of message into a string that can be displayed + * @return A string describing message contents + */ +string MessageState::ToString() { + string stateString; + + if (this->state == ROBOT_NOT_BUSY) stateString="Not busy"; + else if (this->state == ROBOT_BUSY) stateString="Busy"; + else stateString="Invalid state"; + + if (CheckID(this->messageID)) + return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nState: \"" + stateString + "\""; + else + return "Invalid message"; +} + +/** + * Allocate a new message and copy contents of current message + * @return A message, copy of current + */ +Message* MessageState::Copy() { + return new MessageState(this->messageID, this->state); +} + +/** + * Verify if message ID is compatible with current message type + * @param id Message ID + * @return true, if message ID is acceptable, false otherwise + */ +bool MessageState::CheckID(MessageID id) { + if ((id != MESSAGE_ROBOT_CURRENT_STATE)) { + return false; + } else return true; +} \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/lib/messages.h b/software/raspberry/superviseur-robot/lib/messages.h index 1d8b8a8..d545220 100644 --- a/software/raspberry/superviseur-robot/lib/messages.h +++ b/software/raspberry/superviseur-robot/lib/messages.h @@ -38,7 +38,10 @@ typedef enum { // messages for serial communication with robot MESSAGE_OPEN_COM, MESSAGE_CLOSE_COM, - + + // Messages specific to server + MESSAGE_MONITOR_LOST, + // Messages for camera MESSAGE_CAM_OPEN, MESSAGE_CAM_CLOSE, @@ -75,12 +78,13 @@ typedef enum { ANSWER_NACK, ANSWER_LOST_ROBOT, ANSWER_ROBOT_TIMEOUT, - ANSWER_ROBOT_UNKNWON_COMMAND, + ANSWER_ROBOT_UNKNOWN_COMMAND, ANSWER_ROBOT_ERROR, ANSWER_ROBOT_CHECKSUM } AnswerID; typedef enum { + BATTERY_UNKNOWN=-1, BATTERY_EMPTY=0, BATTERY_LOW, BATTERY_FULL @@ -106,6 +110,11 @@ public: */ Message(); + /** + * Create a new, empty message + */ + Message(MessageID id); + /** * Destroy message */ @@ -135,8 +144,7 @@ public: * Set message ID * @param id Message ID */ - virtual void SetID(MessageID id) { - } + virtual void SetID(MessageID id); /** * Comparison operator @@ -259,93 +267,6 @@ protected: bool CheckID(MessageID id); }; -/** - * Message class for holding float value, based on Message class - * - * @brief Float message class - * - */ -class MessageFloat : public Message { -public: - /** - * Create a new, empty float message - */ - MessageFloat(); - - /** - * Create a new float message, with given ID and value - * @param id Message ID - * @param val Message value - * @throw std::runtime_error if message ID is incompatible with float data - */ - MessageFloat(MessageID id, float val); - - /** - * Set message ID - * @param id Message ID - * @throw std::runtime_error if message ID is incompatible with float data - */ - void SetID(MessageID id); - - /** - * Get message value (float) - * @return Float value - */ - float GetValue() { - return value; - } - - /** - * Set message value (float) - * @param val Float value to store in message - */ - void SetValue(float val) { - this->value = val; - } - - /** - * Translate content of message into a string that can be displayed - * @return A string describing message contents - */ - string ToString(); - - /** - * Allocate a new mesage and copy contents of current message - * @return A message, copy of current - */ - Message* Copy(); - - /** - * Comparison operator - * @param msg Message to be compared - * @return true if message are equal, false otherwise - */ - virtual bool operator==(const MessageFloat& msg) { - return ((messageID == msg.messageID) && (value == msg.value)); - } - - /** - * Difference operator - * @param msg Message to be compared - * @return true if message are different, false otherwise - */ - virtual bool operator!=(const MessageFloat& msg) { - return !((messageID == msg.messageID) && (value == msg.value)); - } -protected: - /** - * Message float value - */ - float value; - - /** - * Verify if message ID is compatible with current message type - * @param id Message ID - * @return true, if message ID is acceptable, false otherwise - */ - bool CheckID(MessageID id); -}; - /** * Message class for holding string value, based on Message class * @@ -454,6 +375,11 @@ public: */ MessageImg(MessageID id, Img* image); + /** + * Destroy Image message + */ + virtual ~MessageImg(); + /** * Set message ID * @param id Message ID @@ -473,9 +399,7 @@ public: * Set message image * @param image Pointer to image object */ - void SetImage(Img* image) { - this->image = image; - } + void SetImage(Img* image); /** * Translate content of message into a string that can be displayed @@ -519,10 +443,10 @@ public: /** * Create a new image message, with given ID and boolean value * @param id Message ID - * @param image Pointer to image + * @param pos Position * @throw std::runtime_error if message ID is incompatible with image message */ - MessagePosition(MessageID id, Position pos); + MessagePosition(MessageID id, Position& pos); /** * Set message ID @@ -543,9 +467,7 @@ public: * Set message image * @param image Pointer to image object */ - void SetPosition(Position pos) { - this->pos = pos; - } + void SetPosition(Position& pos); /** * Translate content of message into a string that can be displayed @@ -613,9 +535,7 @@ public: * Set message image * @param image Pointer to image object */ - void SetLevel(BatteryLevel level) { - this->level = level; - } + void SetLevel(BatteryLevel level); /** * Translate content of message into a string that can be displayed @@ -659,10 +579,10 @@ public: /** * Create a new image message, with given ID and boolean value * @param id Message ID - * @param image Pointer to image + * @param ans Answer ID * @throw std::runtime_error if message ID is incompatible with image message */ - MessageAnswer(MessageID id, AnswerID answer); + MessageAnswer(MessageID id, AnswerID ans); /** * Set message ID @@ -680,12 +600,10 @@ public: } /** - * Set message image - * @param image Pointer to image object + * Set message answer + * @param ans Answer ID */ - void SetAnswer(AnswerID answer) { - this->answer = answer; - } + void SetAnswer(AnswerID ans); /** * Translate content of message into a string that can be displayed @@ -753,9 +671,7 @@ public: * Set message image * @param image Pointer to image object */ - void SetState(RobotState state) { - this->state = state; - } + void SetState(RobotState state); /** * Translate content of message into a string that can be displayed @@ -771,7 +687,7 @@ public: protected: /** - * Message answer + * Robot state */ RobotState state; diff --git a/software/raspberry/superviseur-robot/nbproject/Makefile-Debug.mk b/software/raspberry/superviseur-robot/nbproject/Makefile-Debug.mk index ce4f453..03affc7 100644 --- a/software/raspberry/superviseur-robot/nbproject/Makefile-Debug.mk +++ b/software/raspberry/superviseur-robot/nbproject/Makefile-Debug.mk @@ -41,7 +41,10 @@ OBJECTFILES= \ ${OBJECTDIR}/lib/robot.o \ ${OBJECTDIR}/lib/server.o \ ${OBJECTDIR}/main.o \ - ${OBJECTDIR}/tasks.o + ${OBJECTDIR}/tasks.o \ + ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \ + ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \ + ${OBJECTDIR}/tasks_pthread.o # C Compiler Flags @@ -103,6 +106,21 @@ ${OBJECTDIR}/tasks.o: tasks.cpp ${RM} "$@.d" $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp +${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp + ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp + +${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp + ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp + +${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp + ${MKDIR} -p ${OBJECTDIR} + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks_pthread.o tasks_pthread.cpp + # Subprojects .build-subprojects: diff --git a/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__Pthread_.mk b/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__Pthread_.mk new file mode 100644 index 0000000..2b12c40 --- /dev/null +++ b/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__Pthread_.mk @@ -0,0 +1,131 @@ +# +# Generated Makefile - do not edit! +# +# Edit the Makefile in the project folder instead (../Makefile). Each target +# has a -pre and a -post target defined where you can add customized code. +# +# This makefile implements configuration specific macros and targets. + + +# Environment +MKDIR=mkdir +CP=cp +GREP=grep +NM=nm +CCADMIN=CCadmin +RANLIB=ranlib +CC=gcc +CCC=g++ +CXX=g++ +FC=gfortran +AS=as + +# Macros +CND_PLATFORM=GNU-Linux +CND_DLIB_EXT=so +CND_CONF=Debug__Pthread_ +CND_DISTDIR=dist +CND_BUILDDIR=build + +# Include project Makefile +include ./Makefile + +# Object Directory +OBJECTDIR=${CND_BUILDDIR}/${CND_CONF}/${CND_PLATFORM} + +# Object Files +OBJECTFILES= \ + ${OBJECTDIR}/lib/message.o \ + ${OBJECTDIR}/lib/messages.o \ + ${OBJECTDIR}/lib/monitor.o \ + ${OBJECTDIR}/lib/robot.o \ + ${OBJECTDIR}/lib/server.o \ + ${OBJECTDIR}/main.o \ + ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \ + ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \ + ${OBJECTDIR}/tasks_pthread.o + + +# C Compiler Flags +CFLAGS=-I/usr/xenomai/include/mercury -I/usr/xenomai/include -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -D__MERCURY__ -I/usr/xenomai/include/alchemy + +# CC Compiler Flags +CCFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables +CXXFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables + +# Fortran Compiler Flags +FFLAGS= + +# Assembler Flags +ASFLAGS= + +# Link Libraries and Options +LDLIBSOPTIONS=`pkg-config --libs opencv` + +# Build Targets +.build-conf: ${BUILD_SUBPROJECTS} + "${MAKE}" -f nbproject/Makefile-${CND_CONF}.mk ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot + +${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot: ${OBJECTFILES} + ${MKDIR} -p ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM} + ${LINK.cc} -o ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot ${OBJECTFILES} ${LDLIBSOPTIONS} -lpthread -lrt + +${OBJECTDIR}/lib/message.o: lib/message.cpp + ${MKDIR} -p ${OBJECTDIR}/lib + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/message.o lib/message.cpp + +${OBJECTDIR}/lib/messages.o: lib/messages.cpp + ${MKDIR} -p ${OBJECTDIR}/lib + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/messages.o lib/messages.cpp + +${OBJECTDIR}/lib/monitor.o: lib/monitor.cpp + ${MKDIR} -p ${OBJECTDIR}/lib + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/monitor.o lib/monitor.cpp + +${OBJECTDIR}/lib/robot.o: lib/robot.cpp + ${MKDIR} -p ${OBJECTDIR}/lib + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/robot.o lib/robot.cpp + +${OBJECTDIR}/lib/server.o: lib/server.cpp + ${MKDIR} -p ${OBJECTDIR}/lib + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/server.o lib/server.cpp + +${OBJECTDIR}/main.o: main.cpp + ${MKDIR} -p ${OBJECTDIR} + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/main.o main.cpp + +${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp + ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp + +${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp + ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp + +${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp + ${MKDIR} -p ${OBJECTDIR} + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks_pthread.o tasks_pthread.cpp + +# Subprojects +.build-subprojects: + +# Clean Targets +.clean-conf: ${CLEAN_SUBPROJECTS} + ${RM} -r ${CND_BUILDDIR}/${CND_CONF} + +# Subprojects +.clean-subprojects: + +# Enable dependency checking +.dep.inc: .depcheck-impl + +include .dep.inc diff --git a/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk b/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk index 702a517..c106fb3 100644 --- a/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk +++ b/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk @@ -42,7 +42,10 @@ OBJECTFILES= \ ${OBJECTDIR}/lib/robot.o \ ${OBJECTDIR}/lib/server.o \ ${OBJECTDIR}/main.o \ - ${OBJECTDIR}/tasks.o + ${OBJECTDIR}/tasks.o \ + ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \ + ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \ + ${OBJECTDIR}/tasks_pthread.o # C Compiler Flags @@ -109,6 +112,21 @@ ${OBJECTDIR}/tasks.o: tasks.cpp ${RM} "$@.d" $(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp +${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp + ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp + +${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp + ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp + +${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp + ${MKDIR} -p ${OBJECTDIR} + ${RM} "$@.d" + $(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks_pthread.o tasks_pthread.cpp + # Subprojects .build-subprojects: diff --git a/software/raspberry/superviseur-robot/nbproject/Makefile-Release.mk b/software/raspberry/superviseur-robot/nbproject/Makefile-Release.mk index fa26549..dd197c1 100644 --- a/software/raspberry/superviseur-robot/nbproject/Makefile-Release.mk +++ b/software/raspberry/superviseur-robot/nbproject/Makefile-Release.mk @@ -42,7 +42,10 @@ OBJECTFILES= \ ${OBJECTDIR}/lib/robot.o \ ${OBJECTDIR}/lib/server.o \ ${OBJECTDIR}/main.o \ - ${OBJECTDIR}/tasks.o + ${OBJECTDIR}/tasks.o \ + ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \ + ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \ + ${OBJECTDIR}/tasks_pthread.o # C Compiler Flags @@ -109,6 +112,21 @@ ${OBJECTDIR}/tasks.o: tasks.cpp ${RM} "$@.d" $(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp +${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp + ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a + ${RM} "$@.d" + $(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp + +${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp + ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a + ${RM} "$@.d" + $(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp + +${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp + ${MKDIR} -p ${OBJECTDIR} + ${RM} "$@.d" + $(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks_pthread.o tasks_pthread.cpp + # Subprojects .build-subprojects: diff --git a/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk b/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk index 24d259d..13ab52c 100644 --- a/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk +++ b/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk @@ -31,7 +31,7 @@ DEFAULTCONF=Debug CONF=${DEFAULTCONF} # All Configurations -ALLCONFS=Debug Release Debug__RPI_ +ALLCONFS=Debug Release Debug__RPI_ Debug__Pthread_ # build diff --git a/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk b/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk index 9dbbe3a..e97628a 100644 --- a/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk +++ b/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk @@ -30,6 +30,14 @@ CND_ARTIFACT_PATH_Debug__RPI_=dist/Debug__RPI_/GNU-Linux/superviseur-robot CND_PACKAGE_DIR_Debug__RPI_=dist/Debug__RPI_/GNU-Linux/package CND_PACKAGE_NAME_Debug__RPI_=superviseur-robot.tar CND_PACKAGE_PATH_Debug__RPI_=dist/Debug__RPI_/GNU-Linux/package/superviseur-robot.tar +# Debug__Pthread_ configuration +CND_PLATFORM_Debug__Pthread_=GNU-Linux +CND_ARTIFACT_DIR_Debug__Pthread_=dist/Debug__Pthread_/GNU-Linux +CND_ARTIFACT_NAME_Debug__Pthread_=superviseur-robot +CND_ARTIFACT_PATH_Debug__Pthread_=dist/Debug__Pthread_/GNU-Linux/superviseur-robot +CND_PACKAGE_DIR_Debug__Pthread_=dist/Debug__Pthread_/GNU-Linux/package +CND_PACKAGE_NAME_Debug__Pthread_=superviseur-robot.tar +CND_PACKAGE_PATH_Debug__Pthread_=dist/Debug__Pthread_/GNU-Linux/package/superviseur-robot.tar # # include compiler specific variables # diff --git a/software/raspberry/superviseur-robot/nbproject/Package-Debug__Pthread_.bash b/software/raspberry/superviseur-robot/nbproject/Package-Debug__Pthread_.bash new file mode 100644 index 0000000..bbc2489 --- /dev/null +++ b/software/raspberry/superviseur-robot/nbproject/Package-Debug__Pthread_.bash @@ -0,0 +1,76 @@ +#!/bin/bash -x + +# +# Generated - do not edit! +# + +# Macros +TOP=`pwd` +CND_PLATFORM=GNU-Linux +CND_CONF=Debug__Pthread_ +CND_DISTDIR=dist +CND_BUILDDIR=build +CND_DLIB_EXT=so +NBTMPDIR=${CND_BUILDDIR}/${CND_CONF}/${CND_PLATFORM}/tmp-packaging +TMPDIRNAME=tmp-packaging +OUTPUT_PATH=${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot +OUTPUT_BASENAME=superviseur-robot +PACKAGE_TOP_DIR=superviseur-robot/ + +# Functions +function checkReturnCode +{ + rc=$? + if [ $rc != 0 ] + then + exit $rc + fi +} +function makeDirectory +# $1 directory path +# $2 permission (optional) +{ + mkdir -p "$1" + checkReturnCode + if [ "$2" != "" ] + then + chmod $2 "$1" + checkReturnCode + fi +} +function copyFileToTmpDir +# $1 from-file path +# $2 to-file path +# $3 permission +{ + cp "$1" "$2" + checkReturnCode + if [ "$3" != "" ] + then + chmod $3 "$2" + checkReturnCode + fi +} + +# Setup +cd "${TOP}" +mkdir -p ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/package +rm -rf ${NBTMPDIR} +mkdir -p ${NBTMPDIR} + +# Copy files and create directories and links +cd "${TOP}" +makeDirectory "${NBTMPDIR}/superviseur-robot/bin" +copyFileToTmpDir "${OUTPUT_PATH}" "${NBTMPDIR}/${PACKAGE_TOP_DIR}bin/${OUTPUT_BASENAME}" 0755 + + +# Generate tar file +cd "${TOP}" +rm -f ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/package/superviseur-robot.tar +cd ${NBTMPDIR} +tar -vcf ../../../../${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/package/superviseur-robot.tar * +checkReturnCode + +# Cleanup +cd "${TOP}" +rm -rf ${NBTMPDIR} diff --git a/software/raspberry/superviseur-robot/nbproject/configurations.xml b/software/raspberry/superviseur-robot/nbproject/configurations.xml index a8a96fb..5c2bc32 100644 --- a/software/raspberry/superviseur-robot/nbproject/configurations.xml +++ b/software/raspberry/superviseur-robot/nbproject/configurations.xml @@ -5,6 +5,8 @@ displayName="Header Files" projectFiles="true"> ./lib/camera.h + /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h + /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h ./lib/definitions.h ./lib/image.h ./lib/img.h @@ -14,6 +16,7 @@ ./lib/robot.h ./lib/server.h ./tasks.h + tasks_pthread.h ./lib/camera.cpp + /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp + /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp ./lib/image.cpp ./lib/img.cpp ./main.cpp @@ -33,6 +38,7 @@ ./lib/robot.cpp ./lib/server.cpp ./tasks.cpp + tasks_pthread.cpp ./Makefile + + /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib + ./Makefile @@ -122,6 +131,30 @@ + + + + + + + + + + + + @@ -185,6 +218,30 @@ + + + + + + + + + + + + @@ -265,6 +322,128 @@ + + + + + + + + + + + + + + + + default + true + false + + + + + ./ + ./lib + + -I/usr/xenomai/include/mercury -I/usr/xenomai/include -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -D__MERCURY__ -I/usr/xenomai/include/alchemy + + + + ./ + ./lib + + -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables + + _WITH_TRACE_ + __FOR_PC__ + __WITH_PTHREAD__ + + + + + `pkg-config --libs opencv` + + -lpthread -lrt + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk b/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk index 949e112..bde17d1 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk +++ b/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk @@ -6,3 +6,4 @@ # Debug configuration # Release configuration # Debug__RPI_ configuration +# Debug__Pthread_ configuration diff --git a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml index 393661c..b55dcb1 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml @@ -105,5 +105,42 @@ + + + localhost + 2 + + + + + + + + + + + + + + + gdb + + + + sudo "${OUTPUT_PATH}" + sudo -E "${OUTPUT_PATH}" + pkexec "${OUTPUT_PATH}" + "${OUTPUT_PATH}" + + "${OUTPUT_PATH}" + + true + 1 + 0 + 0 + + + + diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 7a2adea..e64b33b 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -2,21 +2,27 @@ 1 - 0 + 3 file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/monitor.h + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/robot.h + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/image.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/server.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks.cpp + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/message.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/main.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.cpp diff --git a/software/raspberry/superviseur-robot/nbproject/project.xml b/software/raspberry/superviseur-robot/nbproject/project.xml index 3d6436b..5a95dd1 100644 --- a/software/raspberry/superviseur-robot/nbproject/project.xml +++ b/software/raspberry/superviseur-robot/nbproject/project.xml @@ -9,7 +9,9 @@ h UTF-8 - + + /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib + Debug @@ -23,6 +25,10 @@ Debug__RPI_ 1 + + Debug__Pthread_ + 1 + false diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 489a0e6..b4127b8 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -23,8 +23,8 @@ * \brief Miscellaneous functions used for destijl project. */ -#ifndef FUNCTIONS_H -#define FUNCTIONS_H +#ifndef __TASKS_H__ +#define __TASKS_H__ #include #include diff --git a/software/raspberry/superviseur-robot/tasks_pthread.cpp b/software/raspberry/superviseur-robot/tasks_pthread.cpp new file mode 100644 index 0000000..6e00725 --- /dev/null +++ b/software/raspberry/superviseur-robot/tasks_pthread.cpp @@ -0,0 +1,263 @@ +/* + * 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 . + */ + +/** + * \file functions.h + * \author PE.Hladik + * \version 1.0 + * \date 06/06/2017 + * \brief Miscellaneous functions used for destijl project. + */ + +#include "tasks_pthread.h" + +#ifdef __WITH_PTHREAD__ +char mode_start; + +void write_in_queue(RT_QUEUE *, MessageToMon); + +void f_server(void *arg) { + int err; + /* INIT */ + RT_TASK_INFO info; + rt_task_inquire(NULL, &info); + printf("Init %s\n", info.name); + rt_sem_p(&sem_barrier, TM_INFINITE); + + err=openServer(DEFAULT_SERVER_PORT); + + if (err < 0) { + printf("Failed to start server: %s\n", strerror(-err)); + exit(EXIT_FAILURE); + } else { +#ifdef _WITH_TRACE_ + printf("%s: server started\n", info.name); +#endif + //Waiting for a client to connect + err=acceptClient(); + + if (err<0) { + printf("Client accept failed: %s\n", strerror(-err)); + exit(EXIT_FAILURE); + } + +#ifdef _WITH_TRACE_ + printf ("client connected: %d\n", err); + printf ("Rock'n'roll baby !\n"); +#endif + rt_sem_broadcast(&sem_serverOk); + } +} + +void f_sendToMon(void * arg) { + int err; + MessageToMon msg; + + /* INIT */ + RT_TASK_INFO info; + rt_task_inquire(NULL, &info); + printf("Init %s\n", info.name); + rt_sem_p(&sem_barrier, TM_INFINITE); + +#ifdef _WITH_TRACE_ + printf("%s : waiting for sem_serverOk\n", info.name); +#endif + rt_sem_p(&sem_serverOk, TM_INFINITE); + while (1) { + +#ifdef _WITH_TRACE_ + printf("%s : waiting for a message in queue\n", info.name); +#endif + if (rt_queue_read(&q_messageToMon, &msg, sizeof (MessageToRobot), TM_INFINITE) >= 0) { +#ifdef _WITH_TRACE_ + printf("%s : message {%s,%s} in queue\n", info.name, msg.header, (char*)msg.data); +#endif + + send_message_to_monitor(msg.header, msg.data); + free_msgToMon_data(&msg); + rt_queue_free(&q_messageToMon, &msg); + } else { + printf("Error msg queue write: %s\n", strerror(-err)); + } + } +} + +void f_receiveFromMon(void *arg) { + MessageFromMon msg; + int err; + + /* INIT */ + RT_TASK_INFO info; + rt_task_inquire(NULL, &info); + printf("Init %s\n", info.name); + rt_sem_p(&sem_barrier, TM_INFINITE); + +#ifdef _WITH_TRACE_ + printf("%s : waiting for sem_serverOk\n", info.name); +#endif + rt_sem_p(&sem_serverOk, TM_INFINITE); + do { +#ifdef _WITH_TRACE_ + printf("%s : waiting for a message from monitor\n", info.name); +#endif + err = receive_message_from_monitor(msg.header, msg.data); +#ifdef _WITH_TRACE_ + printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data); +#endif + if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) { + if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot +#ifdef _WITH_TRACE_ + printf("%s: message open Xbee communication\n", info.name); +#endif + rt_sem_v(&sem_openComRobot); + } + } else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) { + if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot +#ifdef _WITH_TRACE_ + printf("%s: message start robot\n", info.name); +#endif + rt_sem_v(&sem_startRobot); + + } else if ((msg.data[0] == DMB_GO_BACK) + || (msg.data[0] == DMB_GO_FORWARD) + || (msg.data[0] == DMB_GO_LEFT) + || (msg.data[0] == DMB_GO_RIGHT) + || (msg.data[0] == DMB_STOP_MOVE)) { + + rt_mutex_acquire(&mutex_move, TM_INFINITE); + robotMove = msg.data[0]; + rt_mutex_release(&mutex_move); +#ifdef _WITH_TRACE_ + printf("%s: message update movement with %c\n", info.name, robotMove); +#endif + + } + } + } while (err > 0); + +} + +void f_openComRobot(void * arg) { + int err; + + /* INIT */ + RT_TASK_INFO info; + rt_task_inquire(NULL, &info); + printf("Init %s\n", info.name); + rt_sem_p(&sem_barrier, TM_INFINITE); + + while (1) { +#ifdef _WITH_TRACE_ + printf("%s : Wait sem_openComRobot\n", info.name); +#endif + rt_sem_p(&sem_openComRobot, TM_INFINITE); +#ifdef _WITH_TRACE_ + printf("%s : sem_openComRobot arrived => open communication robot\n", info.name); +#endif + err = open_communication_robot(); + if (err == 0) { +#ifdef _WITH_TRACE_ + printf("%s : the communication is opened\n", info.name); +#endif + MessageToMon msg; + set_msgToMon_header(&msg, (char*)HEADER_STM_ACK); + write_in_queue(&q_messageToMon, msg); + } else { + MessageToMon msg; + set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK); + write_in_queue(&q_messageToMon, msg); + } + } +} + +void f_startRobot(void * arg) { + int err; + + /* INIT */ + RT_TASK_INFO info; + rt_task_inquire(NULL, &info); + printf("Init %s\n", info.name); + rt_sem_p(&sem_barrier, TM_INFINITE); + + while (1) { +#ifdef _WITH_TRACE_ + printf("%s : Wait sem_startRobot\n", info.name); +#endif + rt_sem_p(&sem_startRobot, TM_INFINITE); +#ifdef _WITH_TRACE_ + printf("%s : sem_startRobot arrived => Start robot\n", info.name); +#endif + err = send_command_to_robot(DMB_START_WITHOUT_WD); + if (err == 0) { +#ifdef _WITH_TRACE_ + printf("%s : the robot is started\n", info.name); +#endif + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + robotStarted = 1; + rt_mutex_release(&mutex_robotStarted); + MessageToMon msg; + set_msgToMon_header(&msg, (char*)HEADER_STM_ACK); + write_in_queue(&q_messageToMon, msg); + } else { + MessageToMon msg; + set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK); + write_in_queue(&q_messageToMon, msg); + } + } +} + +void f_move(void *arg) { + /* INIT */ + RT_TASK_INFO info; + rt_task_inquire(NULL, &info); + printf("Init %s\n", info.name); + rt_sem_p(&sem_barrier, TM_INFINITE); + + /* PERIODIC START */ +#ifdef _WITH_PERIODIC_TRACE_ + printf("%s: start period\n", info.name); +#endif + rt_task_set_periodic(NULL, TM_NOW, 100000000); + while (1) { +#ifdef _WITH_PERIODIC_TRACE_ + printf("%s: Wait period \n", info.name); +#endif + rt_task_wait_period(NULL); +#ifdef _WITH_PERIODIC_TRACE_ + printf("%s: Periodic activation\n", info.name); + printf("%s: move equals %c\n", info.name, robotMove); +#endif + rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); + if (robotStarted) { + rt_mutex_acquire(&mutex_move, TM_INFINITE); + send_command_to_robot(robotMove); + rt_mutex_release(&mutex_move); +#ifdef _WITH_TRACE_ + printf("%s: the movement %c was sent\n", info.name, robotMove); +#endif + } + rt_mutex_release(&mutex_robotStarted); + } +} + +void write_in_queue(RT_QUEUE *queue, MessageToMon msg) { + void *buff; + buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon)); + memcpy(buff, &msg, sizeof (MessageToMon)); + rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL); +} +#endif //__WITH_PTHREAD__ \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/tasks_pthread.h b/software/raspberry/superviseur-robot/tasks_pthread.h new file mode 100644 index 0000000..71dd7b4 --- /dev/null +++ b/software/raspberry/superviseur-robot/tasks_pthread.h @@ -0,0 +1,104 @@ +/* + * 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 . + */ + +/** + * \file functions.h + * \author PE.Hladik + * \version 1.0 + * \date 06/06/2017 + * \brief Miscellaneous functions used for destijl project. + */ + +#ifndef __TASKS_H__ +#define __TASKS_H__ + +#ifdef __WITH_PTHREAD__ +#include +#include +#include + +#include + +#include "monitor.h" +#include "robot.h" +#include "image.h" +#include "message.h" +#include "server.h" + +extern RT_TASK th_server; +extern RT_TASK th_sendToMon; +extern RT_TASK th_receiveFromMon; +extern RT_TASK th_openComRobot; +extern RT_TASK th_startRobot; +extern RT_TASK th_move; + +extern RT_MUTEX mutex_robotStarted; +extern RT_MUTEX mutex_move; + +extern RT_SEM sem_barrier; +extern RT_SEM sem_openComRobot; +extern RT_SEM sem_serverOk; +extern RT_SEM sem_startRobot; + +extern RT_QUEUE q_messageToMon; + +extern int etatCommMoniteur; +extern int robotStarted; +extern char robotMove; + +extern int MSG_QUEUE_SIZE; + +extern int PRIORITY_TSERVER; +extern int PRIORITY_TOPENCOMROBOT; +extern int PRIORITY_TMOVE; +extern int PRIORITY_TSENDTOMON; +extern int PRIORITY_TRECEIVEFROMMON; +extern int PRIORITY_TSTARTROBOT; + +/** + * \brief Thread handling server communication. + */ +void f_server(void *arg); + +/** + * \brief Thread handling communication to monitor. + */ +void f_sendToMon(void *arg); + +/** + * \brief Thread handling communication from monitor. + */ +void f_receiveFromMon(void *arg); + +/** + * \brief Thread handling opening of robot communication. + */ +void f_openComRobot(void * arg); + +/** + * \brief Thread handling robot mouvements. + */ +void f_move(void *arg); + +/** + * \brief Thread handling robot activation. + */ +void f_startRobot(void *arg); + +#endif // __WITH_PTHREAD__ +#endif /* __TASKS_H__ */ +