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

Nouvelle branche pour les journées portes ouvertes 2019

Browse source
This commit is contained in:
Sébastien DI MERCURIO 2019-01-23 09:29:23 +01:00
parent a97e849155
commit 37b9477567
15 changed files with 2 additions and 1221 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
software/raspberry/superviseur-robot/dist/Debug/GNU-Linux/superviseur vendored
View file

Binary file not shown.

BIN
software/raspberry/superviseur-robot/dist/Debug/GNU-Linux/superviseur-robot vendored
View file

Binary file not shown.

BIN
software/raspberry/superviseur-robot/dist/Debug__Pthread_/GNU-Linux/superviseur-robot vendored
View file

Binary file not shown.

BIN
software/raspberry/superviseur-robot/dist/Debug__Pthread__RPI/GNU-Linux/superviseur-robot vendored
View file

Binary file not shown.

1
software/raspberry/superviseur-robot/not_for_students/examples/.gitignore vendored
View file

@ -1 +0,0 @@
bin/

11
software/raspberry/superviseur-robot/not_for_students/examples/CMakeLists.txt
View file

@ -1,11 +0,0 @@
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../examples/bin)
set(serialTest_FILES ./src/serialTest.cpp)
set(serverTest_FILES ./src/serverTest.cpp)
include_directories(./src ../lib)
add_executable(serialtest ${serialTest_FILES})
target_link_libraries(serialtest destijl)
add_executable(servertest ${serverTest_FILES})
target_link_libraries(servertest destijl)

89
software/raspberry/superviseur-robot/not_for_students/examples/src/rtvideoExample.cpp
View file

@ -1,89 +0,0 @@
#include <task.h>
#include "../src/imagerie.h"
#include "../src/serial.h"
#include "../src/tcpServer.h" // include himself imagerie.h
using namespace std;
using namespace cv;
using namespace raspicam;
RT_TASK video;
void threadVideo(void *arg)
{
printf("Thread lancé ... \n");
Camera rpiCam;
Image imgVideo;
Arene monArene;
position positionRobots[20];
Jpg compress;
openCamera(&rpiCam);
do
{
getImg(&rpiCam, &imgVideo);
if(detectArena(&imgVideo, &monArene)==0)
{
detectPosition(&imgVideo,positionRobots,&monArene);
drawArena(&imgVideo,&imgVideo,&monArene);
}
else
detectPosition(&imgVideo,positionRobots);
drawPosition(&imgVideo,&imgVideo,&positionRobots[0]);
imgCompress(&imgVideo,&compress);
sendToUI("IMG",&compress);
sendToUI("POS",&positionRobots[0]);
}while(waitKey(30)!='q');
closeCam(&rpiCam);
}
int main() {
serverOpen();
robotOpenCom();
char header[4];
char data[20];
memset(data, '\0',20);
memset(header,'\0',4);
if(rt_task_spawn(&video,"envoieVideo",0,20,0, threadVideo, NULL) == -1)
perror("erreur lors de la creation du thread\n");
do
{ receptionFromUI(header,data);
if(strcmp(header, DMB) == 0)
{
printf("EVENEMENT DUMBER DETECTE AVEC LE MESSAGE :%s \n",data);
int a = robotCmd(data[0]);
printf("Resultat CMD : %d \n", a);
if(data[0] == 'u' && a == 0)
{
sendToUI(ACK);
}
if(data[0] == 'r' && a == 0)
{
sendToUI(ACK);
}
}
if(strcmp(header, MES) == 0)
{
printf("EVENEMENT MESSAGE DETECTE AVEC LE MESSAGE :%s \n",data);
}
if(strcmp(header,POS)==0)
{
printf("EVENEMENT POSITION DETECTE AVEC LE MESSAGE :%s \n",data);
}
}while((strcmp(header,MES)!=0) || (data[0] != 'C'));
robotCloseCom();
serverClose();
return 0;
}

18
software/raspberry/superviseur-robot/not_for_students/examples/src/serialExample.cpp
View file

@ -1,18 +0,0 @@
#include "../src/serial.h"
/*
* robotCmd return 0 if the cmd is received and understood.
* -1 for a bad argument; -2 for a bad command ; -3 for a timedOut; -4 for a checkSum error
*/
int main() {
robotOpenCom();
printf("Resultat commande : %d \n",sendCmdToRobot(WITHOUT_WD));
printf("Resultat commande : %d \n",sendCmdToRobot(SETMOVE,"+500"));
printf("Resultat commande : %d \n",sendCmdToRobot(SETTURN,"-180"));
robotCloseCom();
return 0;
}

186
software/raspberry/superviseur-robot/not_for_students/examples/src/serialTest.cpp
View file

@ -1,186 +0,0 @@
/*******************************************************************************
* Copyright (c) 2018 INSA - GEI, Toulouse, France.
* All rights reserved. This program and the accompanying materials
* are made available "AS IS", without any warranty of any kind.
*
* INSA assumes no responsibility for errors or omissions in the
* software or documentation available.
*
* Part of code Copyright ST Microelectronics.
*
* Contributors:
* Lucien Senaneuch - Initial API and implementation
* Sebastien DI MERCURIO - Maintainer since Octobre 2018
*******************************************************************************/
#include <iostream>
#include <ctime>
#include "Robot.h"
#include <unistd.h>
using namespace std;
#define NB_TEST 50
#define DELAY_BETWEEN_TEST 1000000 // 1 seconde
int nb_retries;
int nb_test;
int laststatus;
int nb_success;
int nb_timeout;
int nb_unknown_cmd;
int flipflop;
int Test(Robot rob) {
try {
if (flipflop == 0) {
rob.Move(100);
} else {
rob.Move(-100);
}
} catch (string e) {
//if (e.find("Timeout")==string.npos)
// status=ROBOT_TIMED_OUT;
}
return rob.GetLastCommandStatus();;
}
int main() {
Robot myRobot;
if (myRobot.Open("/dev/ttyUSB0") == SUCCESS) { // ouverture de la com avec le robot
std::cout << "Start robot: ";
try {
myRobot.StartWithoutWatchdog();
} catch ( string e ) {
std::cerr << std::endl << e << std::endl;
return 1;
}
if (myRobot.GetLastCommandStatus()==SUCCESS)
std::cout << "Ok" <<std::endl;
else {
std::cout << " Error: GetLastCommand returns " << myRobot.GetLastCommandStatus() <<std::endl;
return 2;
}
std::cout << "Start stress test" << std::endl;
time_t t_debut, t_fin;
struct tm *tm_debut, *tm_fin;
time(&t_debut);
tm_debut = localtime((const time_t*)&t_debut);
std::cout << "[start time]: " << asctime(tm_debut) << std::endl;
nb_success =0;
nb_timeout =0;
nb_unknown_cmd =0;
flipflop = 0;
for (nb_test=0; nb_test < NB_TEST; nb_test++) {
laststatus=Test(myRobot);
if (flipflop ==0) flipflop=1;
else flipflop =0;
if (laststatus == SUCCESS) nb_success++;
else if (laststatus == TIMEOUT_COMMAND) nb_timeout++;
else if ((laststatus == INVALID_ANSWER) || (laststatus == INVALID_COMMAND)) nb_unknown_cmd ++;
std::cout << "Test " << nb_test << " [s " << nb_success
<< ": t " << nb_timeout << ": u " << nb_unknown_cmd
<< "]"<< "\x1B" <<"[30D"<<std::flush;
usleep(DELAY_BETWEEN_TEST);
}
time(&t_fin);
tm_fin = localtime((const time_t*)&t_fin);
std::cout << std::endl << std::endl << "[end time]: " << asctime(tm_fin) << std::endl;
std::cout << "Reset robot" <<std::endl;
myRobot.Reset();
std::cout << "Close com" <<std::endl;
myRobot.Close();
} else {
std::cerr << "Unable to open \\dev\\ttyUSB0" << std::endl;
return 2;
}
// if (open_communication_robot("/dev/ttyUSB0") != 0) {
// std::cout << "Unable to open com port" << std::endl;
//
// return -1;
// }
//
// std::cout << "Debut du test de stress" << std::endl;
// time_t t_debut, t_fin;
// struct tm *tm_debut, *tm_fin;
//
// time(&t_debut);
// tm_debut = localtime((const time_t*)&t_debut);
//
// std::cout << "[heure debut]: " << asctime(tm_debut) << std::endl;
//
// nb_retries=-1;
// do {
// usleep(DELAY_BETWEEN_RETRIES);
// nb_retries++;
// laststatus = send_command_to_robot(DMB_START_WITHOUT_WD);
// } while ((nb_retries<NB_RETRIES_MAX) && (laststatus != 0));
//
// if(nb_retries == NB_RETRIES_MAX) {
// std::cout << "Unable to start robot" <<std::endl;
//
// return -1;
// }
//
// nb_success =0;
// nb_timeout =0;
// nb_unknown_cmd =0;
// flipflop = 0;
//
// for (nb_test=0; nb_test < NB_TEST; nb_test++) {
// nb_retries=-1;
//
// do {
// usleep(DELAY_BETWEEN_RETRIES);
// nb_retries++;
// if (flipflop == 0) {
// laststatus = send_command_to_robot(DMB_TURN, "100");
// } else {
// laststatus = send_command_to_robot(DMB_TURN, "-100");
// }
//
// } while ((nb_retries<NB_RETRIES_MAX) && (laststatus != 0));
//
// if (flipflop ==0) flipflop=1;
// else flipflop =0;
//
// if (laststatus == 0) nb_success++;
// else if (laststatus == ROBOT_TIMED_OUT) nb_timeout++;
// else if (laststatus == ROBOT_UKNOWN_CMD) nb_unknown_cmd ++;
//
// std::cout << "Test " << nb_test << " [s " << nb_success
// << ": t " << nb_timeout << ": u " << nb_unknown_cmd
// << "]"<< "\x1B" <<"[30D"<<std::flush;
//
// usleep(DELAY_BETWEEN_TEST);
// }
//
// time(&t_fin);
// tm_fin = localtime((const time_t*)&t_fin);
//
// std::cout << std::endl << std::endl << "[heure fin]: " << asctime(tm_fin) << std::endl;
return 0;
}

111
software/raspberry/superviseur-robot/not_for_students/examples/src/serverTest.cpp
View file

@ -1,111 +0,0 @@
/*******************************************************************************
* Copyright (c) 2018 INSA - GEI, Toulouse, France.
* All rights reserved. This program and the accompanying materials
* are made available "AS IS", without any warranty of any kind.
*
* INSA assumes no responsibility for errors or omissions in the
* software or documentation available.
*
* Part of code Copyright ST Microelectronics.
*
* Contributors:
* Lucien Senaneuch - Initial API and implementation
* Sebastien DI MERCURIO - Maintainer since Octobre 2018
*******************************************************************************/
#include <iostream>
#include "Robot.h"
#include "TcpServer.h"
int main (void)
{
TcpServer server;
int clientFD;
string msgIn, msgOut,tmp;
bool finish;
Robot robot;
cout << "TCP server example" << endl;
cout << "Bind and listen on port 1337: ";
try {
server.Listen(1337);
}
catch ( const invalid_argument &ia ) {
cerr << "Error binding server: " << ia.what() << endl;
return 1;
}
cout << "Ok" <<endl;
cout << "Open com with robot: ";
try {
robot.Open ("/dev/ttyUSB0");
} catch (string e)
{
cerr << e << endl;
return 3;
}
cout << "Ok" <<endl;
while (1)
{
cout << "Wait for client to connect: ";
try {
clientFD= server.AcceptClient();
}
catch ( const invalid_argument &ia ) {
cerr << "Error during client accept: " << ia.what() << endl;
return 2;
}
cout << to_string(clientFD) << endl;
msgIn.clear();
finish=false;
do {
msgIn = server.Receive(clientFD, 2); // cmd + \n
if (msgIn.empty()) finish=true;
else {
try {
switch (msgIn[0]) {
case 's':
// Start robot
robot.StartWithoutWatchdog();
break;
case 'f':
robot.Move(100);
break;
case 'b':
robot.Move(-100);
break;
case 'r':
robot.Turn(90);
break;
case 'l':
robot.Turn(-90);
break;
case 'Q':
robot.Reset();
break;
}
} catch (string e) {
cerr << e;
}
if (robot.GetLastCommandStatus() == SUCCESS)
server.Send(clientFD, "OK\n");
else server.Send(clientFD, "ERR\n");
}
} while (finish==false);
cout << "Client disconnected" <<endl;
clientFD=-1;
}
return 0;
}

58
software/raspberry/superviseur-robot/not_for_students/examples/src/uiExample.cpp
View file

@ -1,58 +0,0 @@
#include "../src/serial.h"
#include "../src/tcpServer.h"
#include "../src/imagerie.h"
#include <unistd.h>
#include <pthread.h>
int main() {
//startTrace();
runNodejs("/usr/bin/nodejs", "/home/pehladik/Interface-TP-RT/interface.js");
printf("Lancement serveur ... \n");
serverOpen();
//stopTrace();
printf("Serveur lancé ... \n");
robotOpenCom();
char header[4];
char data[20];
memset(data, '\0',20);
memset(header,'\0',4);
int res;
do
{
res = receptionFromUI(header,data);
printf ("res : %d\n", res);
if(strcmp(header, DMB) == 0)
{
printf("EVENEMENT DUMBER DETECTE AVEC LE MESSAGE :%s \n",data);
int a = sendCmdToRobot(data[0]);
printf("Resultat CMD : %d \n", a);
if(data[0] == 'u' && a == 0)
{
sendToUI(ACK);
}
if(data[0] == 'r' && a == 0)
{
sendToUI(ACK);
}
}
if(strcmp(header, MES) == 0)
{
printf("EVENEMENT MESSAGE DETECTE AVEC LE MESSAGE :%s \n",data);
}
if(strcmp(header,POS)==0)
{
printf("EVENEMENT POSITION DETECTE AVEC LE MESSAGE :%s \n",data);
}
}while((strcmp(header,MES)!=0) || (data[0] != 'C'));
killNodejs();
robotCloseCom();
serverClose();
pause();
return 0;
}

108
software/raspberry/superviseur-robot/not_for_students/examples/src/videoExample.cpp
View file

@ -1,108 +0,0 @@
#include "../src/imagerie.h"
#include "../src/serial.h"
#include "../src/tcpServer.h" // include himself imagerie.h
#include <pthread.h>
#include <unistd.h>
using namespace std;
using namespace cv;
#ifndef __STUB__
using namespace raspicam;
#endif
void * threadVideo(void *arg)
{
printf("Thread lancé ... \n");
Camera rpiCam;
Image imgVideo;
Arene monArene;
Position positionRobots[20];
Jpg compress;
openCamera(&rpiCam);
do
{
#ifndef __STUB__
getImg(&rpiCam, &imgVideo);
#else
getImg(&rpiCam, &imgVideo, "/home/pehladik/C-TP-RT/src/mondrian22.jpeg");
#endif
/*if(detectArena(&imgVideo, &monArene)==0)
{
detectPosition(&imgVideo,positionRobots,&monArene);
drawArena(&imgVideo,&imgVideo,&monArene);
}
else
detectPosition(&imgVideo,positionRobots);
drawPosition(&imgVideo,&imgVideo,&positionRobots[0]);*/
imgCompress(&imgVideo,&compress);
sendToUI("IMG",&compress);
sendToUI("POS",&positionRobots[0]);
}while(waitKey(30)!='q');
closeCam(&rpiCam);
pthread_exit(NULL);
}
void * threadClient(void *arg){
char * args [] = {"nodejs", "/home/pehladik/Interface-TP-RT/interface.js", NULL};
if (execv("/usr/bin/nodejs", args)== -1){
perror("execv");
return EXIT_FAILURE;
}
}
int main() {
pthread_t threadClient;
if(pthread_create(&threadClient,NULL, threadClient, NULL) == -1)
perror("erreur lors de la creation du thread\n");
serverOpen();
robotOpenCom();
char header[4];
char data[20];
memset(data, '\0',20);
memset(header,'\0',4);
pthread_t thread1;
if(pthread_create(&thread1,NULL, threadVideo, NULL) == -1)
perror("erreur lors de la creation du thread\n");
do
{ receptionFromUI(header,data);
printf("Msg reçu : %s %s", header, data);
if(strcmp(header, DMB) == 0)
{
printf("EVENEMENT DUMBER DETECTE AVEC LE MESSAGE :%s \n",data);
int a = sendCmdToRobot(data[0]);
printf("Resultat CMD : %d \n", a);
if(data[0] == WITHOUT_WD && a == 0)
{
sendToUI(ACK);
}
else if(data[0] == BACKIDLE && a == 0)
{
sendToUI(ACK);
}
}
if(strcmp(header, MES) == 0)
{
printf("EVENEMENT MESSAGE DETECTE AVEC LE MESSAGE :%s \n",data);
}
if(strcmp(header,POS)==0)
{
printf("EVENEMENT POSITION DETECTE AVEC LE MESSAGE :%s \n",data);
}
}while((strcmp(header,MES)!=0) || (data[0] != 'C'));
robotCloseCom();
serverClose();
return 0;
}

494
software/raspberry/superviseur-robot/not_for_students/pthread_version/tasks_pthread.cpp
View file

@ -1,494 +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 "tasks_pthread.h"
#include <time.h>
#ifdef __WITH_PTHREAD__
// Déclaration des priorités des taches
#define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20
#define PRIORITY_TMOVE 10
#define PRIORITY_TSENDTOMON 25
#define PRIORITY_TRECEIVEFROMMON 22
#define PRIORITY_TSTARTROBOT 20
/*
* Some remarks:
* 1- This program is mostly a template. It shows you how to create tasks, semaphore
* message queues, mutex ... and how to use them
*
* 2- semDumber is, as name say, useless. Its goal is only to show you how to use semaphore
*
* 3- Data flow is probably not optimal
*
* 4- Take into account that ComRobot::Write will block your task when serial buffer is full,
* time for internal buffer to flush
*
* 5- Same behavior existe for ComMonitor::Write !
*
* 6- When you want to write something in terminal, use cout and terminate with endl and flush
*
* 7- Good luck !
*/
void Tasks::Init() {
int status;
/* Open com port with STM32 */
cout << "Open serial com (";
status = robot.Open();
cout << status;
cout << ")" << endl;
if (status >= 0) {
// Open server
status = monitor.Open(SERVER_PORT);
cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
if (status < 0) throw std::runtime_error {
"Unable to start server on port " + std::to_string(SERVER_PORT)
};
} else
throw std::runtime_error {
"Unable to open serial port /dev/ttyS0 "
};
}
void Tasks::Run() {
threadTimer = new thread((void (*)(void*)) & Tasks::TimerTask, this);
threadServer = new thread((void (*)(void*)) & Tasks::ServerTask, this);
// threadSendToMon=new thread((void (*)(void*)) &Tasks::SendToMonTask,this);
//
// Camera camera=Camera(sm);
// cout << "Try opening camera"<<endl<<flush;
// if (camera.Open()) cout<<"Camera opened successfully"<<endl<<flush;
// else cout<<"Failed to open camera"<<endl<<flush;
//
// counter = 0;
// while (1) {
// Img image=camera.Grab();
//
// counter++;
//
// if (flag == true) {
// cout<< "Image info: "<<image.ToString()<<endl<<flush;
// cout << "FPS = "<<to_string(counter)<<endl<<flush;
// flag=false;
// counter=0;
// }
// }
// msgSend = ComRobot::Ping();
// cout << "Send => " << msgSend->ToString() << endl << flush;
// msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3);
// cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
//
// delete(msgRcv);
//
// msgSend = ComRobot::StartWithoutWD();
// cout << "Send => " << msgSend->ToString() << endl << flush;
// msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3);
// cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
//
// delete(msgRcv);
//
// msgSend = ComRobot::Move(1000);
// cout << "Send => " << msgSend->ToString() << endl << flush;
// msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3);
// cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
//
// delete(msgRcv);
//
// msgSend = ComRobot::GetBattery();
// cout << "Send => " << msgSend->ToString() << endl << flush;
// msgRcv = robot.SendCommand(msgSend, MESSAGE_ROBOT_BATTERY_LEVEL, 3);
// cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
//
// delete(msgRcv);
cout << "Tasks launched" << endl << flush;
}
void Tasks::Stop() {
monitor.Close();
robot.Close();
}
void Tasks::ServerTask(void *arg) {
Message *msgRcv;
Message *msgSend;
bool isActive = true;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
while (isActive) {
msgRcv = NULL;
msgSend = NULL;
msgRcv = monitor.Read();
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) msgSend = new Message(MESSAGE_ANSWER_ACK);
if (msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) msgSend = new Message(MESSAGE_ANSWER_ACK);
if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) msgSend = new Message(MESSAGE_ANSWER_ACK);
if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) msgSend = new Message(MESSAGE_ANSWER_ACK);
if (msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) isActive = false;
if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
sendImage = true;
msgSend = new Message(MESSAGE_ANSWER_ACK);
}
if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
sendImage = false;
msgSend = new Message(MESSAGE_ANSWER_ACK);
}
if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
sendPosition = true;
msgSend = new Message(MESSAGE_ANSWER_ACK);
}
if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
sendPosition = false;
msgSend = new Message(MESSAGE_ANSWER_ACK);
}
if (msgRcv->CompareID(MESSAGE_ROBOT_BATTERY_GET)) msgSend = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL);
if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) showArena = true;
if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) showArena = false;
if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) showArena = false;
if (msgSend != NULL) monitor.Write(msgSend);
delete(msgRcv);
}
}
void Tasks::TimerTask(void* arg) {
struct timespec tim, tim2;
Message *msgSend;
int counter;
int cntFrame = 0;
Position pos;
Arena arena;
tim.tv_sec = 0;
tim.tv_nsec = 50000000; // 50ms (20fps)
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
Camera camera = Camera(sm, 20);
cout << "Try opening camera" << endl << flush;
if (camera.Open()) cout << "Camera opened successfully" << endl << flush;
else {
cout << "Failed to open camera" << endl << flush;
exit(0);
}
pos.angle = 0.0;
pos.robotId = -1;
pos.center = cv::Point2f(0, 0);
pos.direction = cv::Point2f(0, 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;
// }
// counter++;
// if (counter>=10) {
// flag=true;
// counter=0;
// }
//mutexTimer.unlock();
Img image = camera.Grab(); // 15fps
if (sendPosition == true) {
counter++;
if (counter >= 1) { // div =15
counter = 0;
//if (!arena.IsEmpty()) {
image.dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(3));
std::list<Position> poses = image.SearchRobot(arena);
cout << "Nbr of pos detected: " << to_string(poses.size()) << endl << flush;
if (poses.size() > 0) {
Position firstPos = poses.front();
pos.angle = firstPos.angle;
pos.robotId = firstPos.robotId;
pos.center = firstPos.center;
pos.direction = firstPos.direction;
} else {
// Nothing found
pos.angle = 0.0;
pos.robotId = -1;
pos.center = cv::Point2f(0,0);
pos.direction = cv::Point2f(0,0);
}
MessagePosition *msgp = new MessagePosition(MESSAGE_CAM_POSITION, pos);
monitor.Write(msgp);
cout << "Position sent" << endl << flush;
}
}
if (sendImage == true) {
if (showArena) {
arena = image.SearchArena();
if (!arena.IsEmpty()) image.DrawArena(arena);
else cout << "Arena not found" << endl << flush;
}
if (sendPosition == true) {
image.DrawRobot(pos);
}
if (!arena.IsEmpty()) image.DrawArena(arena);
MessageImg *msg = new MessageImg(MESSAGE_CAM_IMAGE, &image);
monitor.Write(msg);
cntFrame++;
cout << "cnt: " << to_string(cntFrame) << endl << flush;
}
}
}
void Tasks::SendToMonTask(void* arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
while (1) {
}
}
//void Tasks::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 Tasks::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 Tasks::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 Tasks::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 Tasks::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__

143
software/raspberry/superviseur-robot/not_for_students/pthread_version/tasks_pthread.h
View file

@ -1,143 +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/>.
*/
#ifndef __TASKS_H__
#define __TASKS_H__
#ifdef __WITH_PTHREAD__
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
//#include "monitor.h"
//#include "robot.h"
//#include "image.h"
//#include "message.h"
//#include "server.h"
#include "camera.h"
#include "img.h"
#include "messages.h"
#include "commonitor.h"
#include "comrobot.h"
#include <thread>
#include <mutex>
#include <condition_variable>
class Tasks {
public:
/**
* @brief Initialisation des structures de l'application (tâches, mutex,
* semaphore, etc.)
*/
void Init();
/**
* @brief Démarrage des tâches
*/
void Run();
/**
* @brief Arrêt des tâches
*/
void Stop();
/**
*/
void Join() {
threadServer->join();
threadTimer->join();
threadSendToMon->join();
}
/**
*/
bool AcceptClient() {
return monitor.AcceptClient();
}
/**
* @brief Thread handling server communication.
*/
void ServerTask(void *arg);
/**
* @brief Thread handling server communication.
*/
void TimerTask(void *arg);
/**
* @brief Thread handling communication to monitor.
*/
void SendToMonTask(void *arg);
private:
ComMonitor monitor;
ComRobot robot;
bool sendImage=false;
bool sendPosition=false;
int counter;
bool flag;
bool showArena=false;
thread *threadServer;
thread *threadSendToMon;
thread *threadTimer;
// thread *threadReceiveFromMon;
// thread *threadOpenComRobot;
// thread *threadStartRobot;
// thread *threadMove;
// thread *threadTimer;
mutex mutexTimer;
// mutex mutexRobotStarted;
// mutex mutexMove;
// mutex semBarrier;
// mutex semOpenComRobot;
// mutex semServerOk;
// mutex semStartRobot;
//
// /**
// * @brief Thread handling communication from monitor.
// */
// void ReceiveFromMonTask(void *arg);
//
// /**
// * @brief Thread handling opening of robot communication.
// */
// void OpenComRobotTask(void * arg);
//
// /**
// * @brief Thread handling robot mouvements.
// */
// void MoveTask(void *arg);
//
// /**
// * @brief Thread handling robot activation.
// */
// void StartRobotTask(void *arg);
};
#endif // __WITH_PTHREAD__
#endif /* __TASKS_H__ */

4
software/robot/.settings/language.settings.xml
View file

@ -4,7 +4,7 @@
<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.atollic.truestudio.mbs.GCCSpecsDetectorAtollicArm" console="false" env-hash="-1714585567149667287" id="com.atollic.truestudio.mbs.provider" keep-relative-paths="false" name="Atollic ARM Tools Language Settings" parameter="${COMMAND} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.atollic.truestudio.mbs.GCCSpecsDetectorAtollicArm" console="false" env-hash="-862055963807747175" id="com.atollic.truestudio.mbs.provider" keep-relative-paths="false" name="Atollic ARM Tools Language Settings" parameter="${COMMAND} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
@ -14,7 +14,7 @@
<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.atollic.truestudio.mbs.GCCSpecsDetectorAtollicArm" console="false" env-hash="-1714585567149667287" id="com.atollic.truestudio.mbs.provider" keep-relative-paths="false" name="Atollic ARM Tools Language Settings" parameter="${COMMAND} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.atollic.truestudio.mbs.GCCSpecsDetectorAtollicArm" console="false" env-hash="-862055963807747175" id="com.atollic.truestudio.mbs.provider" keep-relative-paths="false" name="Atollic ARM Tools Language Settings" parameter="${COMMAND} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>