touchais/Projet_RT
1
0
Fork 0
Code Issues Pull requests Releases Activity

passage au tout objet en cours

Browse source
This commit is contained in:
Sébastien DI MERCURIO 2018-12-19 09:15:42 +01:00
parent a8b1fb8f44
commit 34d1cb6bd8
24 changed files with 2212 additions and 370 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

189
software/raspberry/superviseur-robot/lib/comgui.cpp
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include "comgui.h"
#include <iostream>
#include <sys/socket.h>
#include <netdb.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <algorithm>
#include <stdexcept>
#include <string>
/*
* 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;
}

336
software/raspberry/superviseur-robot/lib/commonitor.cpp Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include "commonitor.h"
#include <iostream>
#include <sys/socket.h>
#include <netdb.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <algorithm>
#include <stdexcept>
#include <string>
/*
* @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;
}

52
software/raspberry/superviseur-robot/lib/comgui.h → software/raspberry/superviseur-robot/lib/commonitor.h
View file

@ -15,8 +15,8 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __COMGUI_H__
#define __COMGUI_H__
#ifndef __COMMONITOR_H__
#define __COMMONITOR_H__
#include "messages.h"
#include <string>
@ -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__ */

334
software/raspberry/superviseur-robot/lib/comrobot.cpp Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include "comrobot.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string>
#include <stdexcept>
#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; //<Set baud rate
options.c_iflag = IGNPAR; // ignores bytes with bad parity
options.c_oflag = 0;
options.c_lflag = 0;
tcflush(fd, TCIFLUSH);
tcsetattr(fd, TCSANOW, &options);
return fd;
}
/**
* Close serial link
* @return Success if above 0, failure if below 0
*/
int ComRobot::Close() {
return close(fd);
}
/**
* 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* ComRobot::Read() {
int rxLength;
unsigned char rxBuffer[6];
unsigned char receivedChar;
bool messageComplete = false;
Message *msg;
unsigned int i;
/* Call pre method for read */
Read_Pre();
/* a message is composed of 7 bytes.
the byte 0 should always be '<'
the byte 1 should be an ascii char that is the label. It define what the data represent
the bytes 2 to 5 are the float value
the byte 6 should always be a '\n'
*/
while (messageComplete == false) {
rxLength = read(this->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];
}

143
software/raspberry/superviseur-robot/lib/comrobot.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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__ */

17
software/raspberry/superviseur-robot/lib/img.cpp
View file

@ -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<Position> Img::search_robot(Arene monArene) {
std::list<Position> robotsFind;

19
software/raspberry/superviseur-robot/lib/img.h
View file

@ -20,6 +20,7 @@
#include <iostream>
#include <list>
#include <string>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
@ -33,16 +34,18 @@
#define ARENA_NOT_DETECTED -1
using namespace std;
typedef cv::Mat ImageMat;
typedef std::vector<unsigned char> Jpg;
typedef vector<unsigned char> 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<Position> search_robot(Arene monArene);
#ifdef __WITH_ARUCO__
std::list<Position> search_aruco(Arene monArene = NULL);
list<Position> search_aruco(Arene monArene = NULL);
#endif // __WITH_ARUCO__
private:
cv::Mat img;
ImageMat img;
#ifdef __WITH_ARUCO__
Ptr<std::Dictionary> dictionary;

469
software/raspberry/superviseur-robot/lib/messages.cpp
View file

@ -21,13 +21,14 @@
#include <string>
/*
* 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;
}

142
software/raspberry/superviseur-robot/lib/messages.h
View file

@ -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;

20
software/raspberry/superviseur-robot/nbproject/Makefile-Debug.mk
View file

@ -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:

131
software/raspberry/superviseur-robot/nbproject/Makefile-Debug__Pthread_.mk Normal file
View file

@ -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

20
software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk
View file

@ -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:

20
software/raspberry/superviseur-robot/nbproject/Makefile-Release.mk
View file

@ -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:

2
software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk
View file

@ -31,7 +31,7 @@ DEFAULTCONF=Debug
CONF=${DEFAULTCONF}
# All Configurations
ALLCONFS=Debug Release Debug__RPI_
ALLCONFS=Debug Release Debug__RPI_ Debug__Pthread_
# build

8
software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk
View file

@ -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
#

76
software/raspberry/superviseur-robot/nbproject/Package-Debug__Pthread_.bash Normal file
View file

@ -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}

179
software/raspberry/superviseur-robot/nbproject/configurations.xml
View file

@ -5,6 +5,8 @@
displayName="Header Files"
projectFiles="true">
<itemPath>./lib/camera.h</itemPath>
<itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h</itemPath>
<itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h</itemPath>
<itemPath>./lib/definitions.h</itemPath>
<itemPath>./lib/image.h</itemPath>
<itemPath>./lib/img.h</itemPath>
@ -14,6 +16,7 @@
<itemPath>./lib/robot.h</itemPath>
<itemPath>./lib/server.h</itemPath>
<itemPath>./tasks.h</itemPath>
<itemPath>tasks_pthread.h</itemPath>
</logicalFolder>
<logicalFolder name="ResourceFiles"
displayName="Resource Files"
@ -24,6 +27,8 @@
displayName="Source Files"
projectFiles="true">
<itemPath>./lib/camera.cpp</itemPath>
<itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</itemPath>
<itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</itemPath>
<itemPath>./lib/image.cpp</itemPath>
<itemPath>./lib/img.cpp</itemPath>
<itemPath>./main.cpp</itemPath>
@ -33,6 +38,7 @@
<itemPath>./lib/robot.cpp</itemPath>
<itemPath>./lib/server.cpp</itemPath>
<itemPath>./tasks.cpp</itemPath>
<itemPath>tasks_pthread.cpp</itemPath>
</logicalFolder>
<logicalFolder name="TestFiles"
displayName="Test Files"
@ -46,6 +52,9 @@
<itemPath>./Makefile</itemPath>
</logicalFolder>
</logicalFolder>
<sourceRootList>
<Elem>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib</Elem>
</sourceRootList>
<projectmakefile>./Makefile</projectmakefile>
<confs>
<conf name="Debug" type="1">
@ -122,6 +131,30 @@
</item>
<item path="./tasks.h" ex="false" tool="3" flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h"
ex="false"
tool="3"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h"
ex="false"
tool="3"
flavor2="0">
</item>
<item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0">
</item>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
</item>
</conf>
<conf name="Release" type="1">
<toolsSet>
@ -185,6 +218,30 @@
</item>
<item path="./tasks.h" ex="false" tool="3" flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h"
ex="false"
tool="3"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h"
ex="false"
tool="3"
flavor2="0">
</item>
<item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0">
</item>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
</item>
</conf>
<conf name="Debug__RPI_" type="1">
<toolsSet>
@ -265,6 +322,128 @@
</item>
<item path="./tasks.h" ex="false" tool="3" flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h"
ex="false"
tool="3"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h"
ex="false"
tool="3"
flavor2="0">
</item>
<item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0">
</item>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
</item>
</conf>
<conf name="Debug__Pthread_" type="1">
<toolsSet>
<compilerSet>default</compilerSet>
<dependencyChecking>true</dependencyChecking>
<rebuildPropChanged>false</rebuildPropChanged>
</toolsSet>
<compileType>
<cTool>
<incDir>
<pElem>./</pElem>
<pElem>./lib</pElem>
</incDir>
<commandLine>-I/usr/xenomai/include/mercury -I/usr/xenomai/include -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -D__MERCURY__ -I/usr/xenomai/include/alchemy</commandLine>
</cTool>
<ccTool>
<incDir>
<pElem>./</pElem>
<pElem>./lib</pElem>
</incDir>
<commandLine>-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables</commandLine>
<preprocessorList>
<Elem>_WITH_TRACE_</Elem>
<Elem>__FOR_PC__</Elem>
<Elem>__WITH_PTHREAD__</Elem>
</preprocessorList>
</ccTool>
<linkerTool>
<linkerLibItems>
<linkerOptionItem>`pkg-config --libs opencv`</linkerOptionItem>
</linkerLibItems>
<commandLine>-lpthread -lrt</commandLine>
</linkerTool>
</compileType>
<item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/camera.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/camera.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/image.cpp" ex="true" tool="1" flavor2="9">
</item>
<item path="./lib/image.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/img.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/img.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/message.cpp" ex="false" tool="1" flavor2="9">
</item>
<item path="./lib/message.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/messages.cpp" ex="false" tool="1" flavor2="9">
</item>
<item path="./lib/messages.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/monitor.cpp" ex="false" tool="1" flavor2="9">
</item>
<item path="./lib/monitor.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/robot.cpp" ex="false" tool="1" flavor2="9">
</item>
<item path="./lib/robot.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/server.cpp" ex="false" tool="1" flavor2="9">
</item>
<item path="./lib/server.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./main.cpp" ex="false" tool="1" flavor2="9">
</item>
<item path="./tasks.cpp" ex="true" tool="1" flavor2="9">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h"
ex="false"
tool="3"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h"
ex="false"
tool="3"
flavor2="0">
</item>
<item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="9">
</item>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
</item>
</conf>
</confs>
</configurationDescriptor>

1
software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk
View file

@ -6,3 +6,4 @@
# Debug configuration
# Release configuration
# Debug__RPI_ configuration
# Debug__Pthread_ configuration

37
software/raspberry/superviseur-robot/nbproject/private/configurations.xml
View file

@ -105,5 +105,42 @@
</environment>
</runprofile>
</conf>
<conf name="Debug__Pthread_" type="1">
<toolsSet>
<developmentServer>localhost</developmentServer>
<platform>2</platform>
</toolsSet>
<dbx_gdbdebugger version="1">
<gdb_pathmaps>
</gdb_pathmaps>
<gdb_interceptlist>
<gdbinterceptoptions gdb_all="false" gdb_unhandled="true" gdb_unexpected="true"/>
</gdb_interceptlist>
<gdb_options>
<DebugOptions>
</DebugOptions>
</gdb_options>
<gdb_buildfirst gdb_buildfirst_overriden="false" gdb_buildfirst_old="false"/>
</dbx_gdbdebugger>
<nativedebugger version="1">
<engine>gdb</engine>
</nativedebugger>
<runprofile version="9">
<runcommandpicklist>
<runcommandpicklistitem>sudo "${OUTPUT_PATH}"</runcommandpicklistitem>
<runcommandpicklistitem>sudo -E "${OUTPUT_PATH}"</runcommandpicklistitem>
<runcommandpicklistitem>pkexec "${OUTPUT_PATH}"</runcommandpicklistitem>
<runcommandpicklistitem>"${OUTPUT_PATH}"</runcommandpicklistitem>
</runcommandpicklist>
<runcommand>"${OUTPUT_PATH}"</runcommand>
<rundir></rundir>
<buildfirst>true</buildfirst>
<console-type>1</console-type>
<terminal-type>0</terminal-type>
<remove-instrumentation>0</remove-instrumentation>
<environment>
</environment>
</runprofile>
</conf>
</confs>
</configurationDescriptor>

8
software/raspberry/superviseur-robot/nbproject/private/private.xml
View file

@ -2,21 +2,27 @@
<project-private xmlns="http://www.netbeans.org/ns/project-private/1">
<data xmlns="http://www.netbeans.org/ns/make-project-private/1">
<activeConfTypeElem>1</activeConfTypeElem>
<activeConfIndexElem>0</activeConfIndexElem>
<activeConfIndexElem>3</activeConfIndexElem>
</data>
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
<group>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/monitor.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/robot.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/image.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/server.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/message.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/main.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>

8
software/raspberry/superviseur-robot/nbproject/project.xml
View file

@ -9,7 +9,9 @@
<header-extensions>h</header-extensions>
<sourceEncoding>UTF-8</sourceEncoding>
<make-dep-projects/>
<sourceRootList/>
<sourceRootList>
<sourceRootElem>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib</sourceRootElem>
</sourceRootList>
<confList>
<confElem>
<name>Debug</name>
@ -23,6 +25,10 @@
<name>Debug__RPI_</name>
<type>1</type>
</confElem>
<confElem>
<name>Debug__Pthread_</name>
<type>1</type>
</confElem>
</confList>
<formatting>
<project-formatting-style>false</project-formatting-style>

4
software/raspberry/superviseur-robot/tasks.h
View file

@ -23,8 +23,8 @@
* \brief Miscellaneous functions used for destijl project.
*/
#ifndef FUNCTIONS_H
#define FUNCTIONS_H
#ifndef __TASKS_H__
#define __TASKS_H__
#include <stdio.h>
#include <stdlib.h>

263
software/raspberry/superviseur-robot/tasks_pthread.cpp Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
/**
* \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__

104
software/raspberry/superviseur-robot/tasks_pthread.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
/**
* \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 <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/mman.h>
#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__ */