237 lines
6.4 KiB
C++
237 lines
6.4 KiB
C++
/*
|
|
* biosemi_talker.cpp
|
|
*
|
|
*
|
|
* Programme permettant de diffuser sous ROS les échantillons en provenance
|
|
* du système d'acquisition biosemi.
|
|
* Note : un message est diffusé par échantillon, un échantillon peut
|
|
* contenir plusieurs canaux.
|
|
*
|
|
*
|
|
*
|
|
* Ce programme transmet des messages qui contiennt :
|
|
* - le numéro d'ordre de l'échantillon
|
|
* - la fréquence d'échantillonnage
|
|
* - un tableau de valeurs réelles (double) dont la taille dépend
|
|
* des paramètres de lancement du serveur d'acquistion (nombre de canaux)
|
|
* - une chaîne de caractères extra (pour les codes d'erreurs).
|
|
*
|
|
*/
|
|
|
|
#include "ros/ros.h"
|
|
#include "acquisition_biosemi/biosemi_result.h"
|
|
#include "acquisition_biosemi/biosemi_echantillon.h"
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
|
|
|
|
|
|
|
|
#define TAILLE_BUFFER 20000
|
|
#define FREQUENCE 256
|
|
|
|
using namespace std;
|
|
|
|
|
|
static int frequence = FREQUENCE;
|
|
static int fin_de_donnees = 0;
|
|
|
|
|
|
/*************************************************************************************************************/
|
|
|
|
void lecture_fichier(int fd, char * buffer, int * entree) {
|
|
ssize_t taille;
|
|
|
|
if (fin_de_donnees) return;
|
|
taille = 2 * TAILLE_BUFFER - (*entree);
|
|
if (taille > SSIZE_MAX) taille = SSIZE_MAX;
|
|
taille = read(fd, buffer + (*entree), taille);
|
|
if (taille <= 0) {
|
|
fin_de_donnees = 1;
|
|
} else {
|
|
*entree += taille;
|
|
buffer[*entree] = 0;
|
|
}
|
|
}
|
|
|
|
/*************************************************************************************************************/
|
|
|
|
int recupere_ligne(acquisition_biosemi::biosemi_echantillon & echantillon, const char * buffer, int & lecture, int & entree) {
|
|
int local_lect = 0;
|
|
int local_fin;
|
|
char * fin;
|
|
double valeur;
|
|
int ok;
|
|
char local_buffer[TAILLE_BUFFER+1];
|
|
int i;
|
|
|
|
static int compteur = 0;
|
|
|
|
|
|
// Récupération d'une ligne au maximum dans un buffer local.
|
|
for(i = 0; (i+lecture < entree) && (buffer[i+lecture] != '\n') && (buffer[i+lecture] != 0); i++) {
|
|
local_buffer[i] = buffer[i+lecture];
|
|
local_buffer[i+1] = '\0';
|
|
}
|
|
if (buffer[i+lecture] != '\n') return 0; // Fin de la ligne non trouvée.
|
|
|
|
// On efface l'ancien contenu :
|
|
echantillon.valeurs.clear();
|
|
echantillon.frequence = frequence;
|
|
echantillon.extra = "";
|
|
|
|
// Lecture du numéro :
|
|
echantillon.numero = strtoul(local_buffer + local_lect, &fin, 10);
|
|
local_fin = fin - local_buffer;
|
|
// Le caractère suivant est forcément un ':', sinon le numéro n'est pas présent.
|
|
if (local_buffer[local_fin] != ':') {
|
|
compteur += 1;
|
|
echantillon.numero = compteur;
|
|
} else {
|
|
local_lect = local_fin + 1;
|
|
}
|
|
|
|
|
|
// On scanne des doubles, autant qu'on en trouve :
|
|
do {
|
|
ok = 0;
|
|
valeur = strtod(local_buffer + local_lect, &fin);
|
|
local_fin = fin - local_buffer;
|
|
if (valeur != 0.0) {
|
|
ok = 1; // Une valeur non nulle indique une conversion correcte.
|
|
} else {
|
|
// Une valeur nulle peut indiquer une conversion incorrecte, ou le fait que le nombre soit nul.
|
|
// Il faut vérifier cela. On doit trouver des chiffres dans l'espace qui a étré scanné.
|
|
ok = 0;
|
|
for(int i = local_lect; i < local_fin; i++) {
|
|
if ((local_buffer[i] >= '0') && (local_buffer[i] <= '9')) ok = 1; // Il y a des chiffres.
|
|
}
|
|
}
|
|
if (ok) {
|
|
local_lect = local_fin;
|
|
echantillon.valeurs.push_back(valeur);
|
|
}
|
|
} while (ok);
|
|
|
|
|
|
// On recherche s'il y a encore des caractères sur la ligne :
|
|
while(local_buffer[local_lect] == ' ') local_lect ++;
|
|
echantillon.extra = local_buffer + local_lect;
|
|
|
|
lecture += strlen(local_buffer) + 1;
|
|
return 1; // Ici la ligne a été complètement décodée.
|
|
}
|
|
|
|
|
|
/*************************************************************************************************************/
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
int fd;
|
|
char buffer[2 * TAILLE_BUFFER + 1];
|
|
int entree, lecture;
|
|
acquisition_biosemi::biosemi_result msg;
|
|
|
|
|
|
// Vérification des paramètres de la ligne de commande :
|
|
|
|
if (argc < 2) {
|
|
cerr << "Appel par : biosemi_simul <nom du fichier> [<fréquence>]." << std::endl;
|
|
cerr << " -- le fichier doit contienir l'enregistrement BCI." << std::endl;
|
|
cerr << " -- la fréquence par défaut, si non spécifié, est 256 ech/sec." << std::endl;
|
|
exit(1);
|
|
}
|
|
|
|
|
|
// Connexion au fichier :
|
|
|
|
entree = 0;
|
|
lecture = 0;
|
|
fd = open(argv[1], O_RDONLY);
|
|
if (fd == -1) {
|
|
cout << "Erreur : fichier introuvable : " << argv[1] << "." << endl;
|
|
exit(1);
|
|
}
|
|
if (argc > 2) frequence = atoi(argv[2]);
|
|
|
|
|
|
// Initialisation de ROS :
|
|
|
|
ros::init(argc, argv, "biosemi_simul");
|
|
ros::NodeHandle n;
|
|
ros::Publisher biosemi_pub = n.advertise<acquisition_biosemi::biosemi_result>("biosemi_result", 3);
|
|
ros::Rate loop_rate(10);
|
|
|
|
|
|
msg.numero = 0;
|
|
msg.nombre = 0;
|
|
msg.frequence = 0;
|
|
msg.valeurs.clear();
|
|
msg.extra = "";
|
|
while (ros::ok())
|
|
{
|
|
acquisition_biosemi::biosemi_echantillon ech;
|
|
|
|
lecture_fichier(fd, buffer, &entree);
|
|
|
|
while(recupere_ligne(ech, buffer, lecture, entree)) {
|
|
if (lecture >= TAILLE_BUFFER) {
|
|
lecture -= TAILLE_BUFFER;
|
|
entree -= TAILLE_BUFFER;
|
|
memcpy(buffer, buffer+TAILLE_BUFFER, TAILLE_BUFFER + 1);
|
|
}
|
|
|
|
|
|
|
|
|
|
if (msg.numero == 0) msg.numero = ech.numero;
|
|
msg.nombre += 1;
|
|
msg.frequence = ech.frequence;
|
|
if (ech.extra != "") msg.extra = ech.extra;
|
|
for(std::vector<double>::const_iterator i = ech.valeurs.begin(); i != ech.valeurs.end(); ++i)
|
|
msg.valeurs.push_back(*i);
|
|
|
|
|
|
std::cout.setf(std::ios::fixed, std::ios::floatfield);
|
|
std::cout.precision(12);
|
|
std::cout.width(12); std::cout.fill('0');
|
|
std::cout << ech.numero;
|
|
std::cout.width(0); std::cout.fill(' ');
|
|
std::cout << "[" << ech.frequence << "]: ";
|
|
for(std::vector<double>::const_iterator i = ech.valeurs.begin(); i != ech.valeurs.end(); ++i)
|
|
std::cout << " " << *i;
|
|
std::cout << " " << ech.extra << " (" << msg.nombre << ")" << std::endl;
|
|
|
|
|
|
|
|
|
|
if (msg.nombre == frequence/10) {
|
|
biosemi_pub.publish(msg);
|
|
ros::spinOnce();
|
|
msg.numero = 0;
|
|
msg.nombre = 0;
|
|
msg.frequence = 0;
|
|
msg.valeurs.clear();
|
|
msg.extra = "";
|
|
loop_rate.sleep();
|
|
}
|
|
}
|
|
if (fin_de_donnees) {
|
|
biosemi_pub.publish(msg);
|
|
ros::spinOnce();
|
|
msg.numero = 0;
|
|
msg.nombre = 0;
|
|
msg.frequence = 0;
|
|
msg.valeurs.clear();
|
|
msg.extra = "";
|
|
std::cerr << "FIN DE DONNEES" << std::endl;
|
|
usleep(300000);
|
|
ros::shutdown();
|
|
}
|
|
}
|
|
|
|
close(fd);
|
|
return 0;
|
|
}
|