307 lines
8.1 KiB
C++
307 lines
8.1 KiB
C++
/*
|
|
* To change this license header, choose License Headers in Project Properties.
|
|
* To change this template file, choose Tools | Templates
|
|
* and open the template in the editor.
|
|
*/
|
|
|
|
/*
|
|
* File: main.cpp
|
|
* Author: dimercur
|
|
*
|
|
* Created on 6 novembre 2018, 10:54
|
|
*/
|
|
|
|
#include <cstdlib>
|
|
|
|
#include "image.h"
|
|
#include "server.h"
|
|
#include "robot.h"
|
|
#include "message.h"
|
|
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <time.h>
|
|
#include <thread>
|
|
|
|
#include "definitions.h"
|
|
|
|
#define HEADER_STM_IMAGE "IMG" // Envoi d'une image
|
|
#define HEADER_STM_BAT "BAT" // Envoi de l'état de la batterie
|
|
#define HEADER_STM_POS "POS" // Envoi de la position
|
|
#define HEADER_STM_NO_ACK "NAK" // Acquittement d'un échec
|
|
#define HEADER_STM_ACK "ACK" // Acquittement d'un succès
|
|
#define HEADER_STM_MES "MSG" // Message textuel
|
|
#define HEADER_STM_LOST_DMB "LCD" // Perte de la communication avec le robot
|
|
|
|
#define HEADER_MTS_MSG "MSG" // Message directe pour Console Dumber
|
|
#define HEADER_MTS_DMB_ORDER "DMB" // Message d'ordre pour le robot
|
|
#define HEADER_MTS_COM_DMB "COM" // Message de gestion de la communication avec le robot
|
|
#define HEADER_MTS_CAMERA "CAM" // Message de gestion de la camera
|
|
#define HEADER_MTS_STOP "STO" // Message d'arrêt du system
|
|
|
|
int socketID;
|
|
char data[1000];
|
|
int receivedLength;
|
|
bool disconnected = true;
|
|
bool dataReady = false;
|
|
bool sysTick = false;
|
|
bool sendImage = false;
|
|
bool sendPos = false;
|
|
|
|
Image monImage;
|
|
Jpg imageCompressed;
|
|
Camera cam;
|
|
|
|
typedef struct {
|
|
char header[4];
|
|
char data[500];
|
|
} MessageFromMon;
|
|
|
|
MessageFromMon *message;
|
|
MessageToMon messageAnswered;
|
|
|
|
std::thread *threadTimer;
|
|
std::thread *threadServer;
|
|
|
|
char* imgMsg = NULL;
|
|
|
|
using namespace std;
|
|
|
|
/*
|
|
*
|
|
*/
|
|
void ThreadServer(void) {
|
|
// Recuperation d'une evenutelle commande sur le serveur
|
|
while (1) {
|
|
receivedLength = receiveDataFromServer(data, 1000);
|
|
if (receivedLength > 0) dataReady = true;
|
|
}
|
|
}
|
|
|
|
void ThreadTimer(void) {
|
|
struct timespec tim, tim2;
|
|
tim.tv_sec = 0;
|
|
tim.tv_nsec = 100000000;
|
|
int cnt =0;
|
|
|
|
while (1) {
|
|
//std::this_thread::sleep_for(std::chrono::seconds )
|
|
//sleep(1);
|
|
if (nanosleep(&tim, &tim2) < 0) {
|
|
printf("Nano sleep system call failed \n");
|
|
return;
|
|
}
|
|
|
|
cnt++;
|
|
|
|
if (cnt>=1)
|
|
{
|
|
sysTick = true;
|
|
cnt=0;
|
|
}
|
|
}
|
|
}
|
|
|
|
void printReceivedMessage(MessageFromMon *mes) {
|
|
cout << "Received " + to_string(receivedLength) + " data";
|
|
cout << std::endl;
|
|
cout << "Header: ";
|
|
|
|
for (int i = 0; i < 4; i++) {
|
|
cout << mes->header[i];
|
|
}
|
|
|
|
cout << std::endl;
|
|
cout << "Data: ";
|
|
for (int i = 0; i < receivedLength - 4; i++) {
|
|
cout << mes->data[i];
|
|
}
|
|
|
|
cout << std::endl;
|
|
}
|
|
|
|
int sendAnswer(string cmd, string data) {
|
|
int status = 0;
|
|
string msg;
|
|
|
|
msg = cmd + ':' + data;
|
|
cout << "Answer: " + msg;
|
|
cout << std::endl;
|
|
sendDataToServer((char*) msg.c_str(), msg.length());
|
|
|
|
return status;
|
|
}
|
|
|
|
int sendBinaryData(string cmd, char* data, int length) {
|
|
int status = 0;
|
|
int lengthSend;
|
|
|
|
if (imgMsg != NULL) free((void*) imgMsg);
|
|
imgMsg = (char*) malloc(length + 4);
|
|
imgMsg[0] = cmd[0];
|
|
imgMsg[1] = cmd[1];
|
|
imgMsg[2] = cmd[2];
|
|
imgMsg[3] = ':';
|
|
|
|
memcpy((void*) &imgMsg[4], (const void *) data, length);
|
|
cout << "Answer: " + cmd;
|
|
cout << std::endl;
|
|
|
|
lengthSend=sendDataToServer(imgMsg, length + 4);
|
|
|
|
cout << "Requested Length: " + to_string(length) + " / Send Length: " + to_string(lengthSend);
|
|
cout << std::endl;
|
|
return status;
|
|
}
|
|
|
|
int decodeMessage(MessageFromMon *mes, int dataLength) {
|
|
int status = 0;
|
|
string header(mes->header, 4);
|
|
string data(mes->data, dataLength);
|
|
|
|
if (header.find(HEADER_MTS_COM_DMB) != std::string::npos) // Message pour la gestion du port de communication
|
|
{
|
|
if (data.find(OPEN_COM_DMB) != std::string::npos) sendAnswer(HEADER_STM_ACK, "");
|
|
else if (data.find(CLOSE_COM_DMB) != std::string::npos) sendAnswer(HEADER_STM_ACK, "");
|
|
} else if (header.find(HEADER_MTS_CAMERA) != std::string::npos) // Message pour la camera
|
|
{
|
|
if (data.find(CAM_OPEN) != std::string::npos) {
|
|
sendAnswer(HEADER_STM_ACK, "");
|
|
sendImage = true;
|
|
} else if (data.find(CAM_CLOSE) != std::string::npos) {
|
|
sendImage = false;
|
|
} else if (data.find(CAM_COMPUTE_POSITION) != std::string::npos) {
|
|
sendPos = true;
|
|
} else if (data.find(CAM_STOP_COMPUTE_POSITION) != std::string::npos) {
|
|
sendPos = false;
|
|
} else {
|
|
|
|
}
|
|
} else if (header.find(HEADER_MTS_DMB_ORDER) != std::string::npos) // Message de com pour le robot
|
|
{
|
|
if (data.find(DMB_START_WITHOUT_WD) != std::string::npos) {
|
|
sendAnswer(HEADER_STM_ACK, "");
|
|
|
|
} else if (data.find(DMB_START_WITH_WD) != std::string::npos) {
|
|
sendAnswer(HEADER_STM_ACK, "");
|
|
|
|
} else if (data.find(DMB_GET_VBAT) != std::string::npos) {
|
|
sendAnswer(HEADER_STM_BAT, to_string(DMB_BAT_HIGH));
|
|
} else if (data.find(DMB_MOVE) != std::string::npos) {
|
|
|
|
} else if (data.find(DMB_TURN) != std::string::npos) {
|
|
|
|
} else {
|
|
|
|
}
|
|
} else if (header.find(HEADER_MTS_STOP) != std::string::npos) // Message d'arret
|
|
{
|
|
sendAnswer(HEADER_STM_ACK, "");
|
|
} else {
|
|
sendAnswer(HEADER_STM_NO_ACK, "");
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
int main(int argc, char** argv) {
|
|
|
|
// Ouverture de la com robot
|
|
#ifdef __FOR_PC__
|
|
if (open_communication_robot("/dev/ttyUSB0") != 0) {
|
|
#else
|
|
if (open_communication_robot("/dev/ttyS0") != 0) {
|
|
#endif /*__FOR_PC__ */
|
|
cerr << "Unable to open /dev/ttyUSB0: abort";
|
|
cout << std::endl;
|
|
return -1;
|
|
}
|
|
cout << "Com port opened";
|
|
cout << std::endl;
|
|
|
|
// Ouverture de la camera
|
|
if (open_camera(&cam) == -1) {
|
|
cerr << "Unable to open camera: abort";
|
|
cout << std::endl;
|
|
|
|
return -1;
|
|
}
|
|
cout << "Camera opened";
|
|
cout << std::endl;
|
|
|
|
// Ouverture du serveur
|
|
socketID = openServer(5544);
|
|
cout << "Server opened on port 5544";
|
|
cout << std::endl;
|
|
|
|
threadTimer = new std::thread(ThreadTimer);
|
|
|
|
for (;;) {
|
|
cout << "Waiting for client to connect ...";
|
|
cout << std::endl;
|
|
acceptClient();
|
|
disconnected = false;
|
|
dataReady = false;
|
|
cout << "Client connected";
|
|
cout << std::endl;
|
|
|
|
threadServer = new std::thread(ThreadServer);
|
|
|
|
while (disconnected == false) {
|
|
|
|
// Recuperation de l'image
|
|
get_image(&cam, &monImage, "");
|
|
|
|
if (dataReady == true) // des données ont été recu par le serveur
|
|
{
|
|
message = (MessageFromMon*) malloc(sizeof (MessageFromMon));
|
|
memcpy((void*) message, (const void*) data, sizeof (MessageFromMon));
|
|
dataReady = false;
|
|
|
|
//if (message->header[4] == ':') message->header[4];
|
|
printReceivedMessage(message);
|
|
decodeMessage(message, receivedLength - 4);
|
|
|
|
free(message);
|
|
}
|
|
|
|
if (sysTick) {
|
|
sysTick = false;
|
|
|
|
if (sendImage) {
|
|
compress_image(&monImage, &imageCompressed);
|
|
int length = imageCompressed.size();
|
|
sendBinaryData(HEADER_STM_IMAGE, reinterpret_cast<char*> (imageCompressed.data()), length);
|
|
//sendAnswer(HEADER_STM_IMAGE, reinterpret_cast<char*> (imageCompressed.data()));
|
|
}
|
|
|
|
if (sendPos) {
|
|
//sendAnswer(HEADER_STM_POS,)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
threadTimer->join();
|
|
threadServer->join();
|
|
|
|
// test de la camera
|
|
if (open_camera(0) != -1) {
|
|
for (;;) {
|
|
get_image(0, &monImage, "");
|
|
|
|
if (monImage.empty()) printf("image vide");
|
|
else {
|
|
imshow("Sortie Camera", monImage);
|
|
|
|
waitKey(10);
|
|
}
|
|
}
|
|
} else {
|
|
printf("Echec ouverture de camera");
|
|
return -1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|