PhasespaceCore::PhasespaceCore(std::string mode) {
// handles server private parameters (private names are protected from accidental name collisions)
private_node_handle_ = new ros::NodeHandle("~");
// publisher/subscriber info (from ROS params if specified)
private_node_handle_->param("topic_name", topic_name_, std::string(DEFAULT_TOPIC_NAME));
private_node_handle_->param("topic_queue_length", topic_queue_length_, DEFAULT_TOPIC_QUEUE_LENGTH);
private_node_handle_->param("verbose_mode", verbose_mode_, false);
if (mode == "talker") {
// PhaseSpace configuration dependent ROS params
private_node_handle_->param("server_ip", server_ip_, std::string(DEFAULT_SERVER_IP));
private_node_handle_->param("init_marker_count", init_marker_count_, DEFAULT_MARKER_COUNT);
private_node_handle_->param("init_flags", init_flags_, DEFAULT_INIT_FLAGS);
// initializes the communication with PhaseSpace server
tracker_ = 0;
markers_ = new OWLMarker[init_marker_count_];
initializeCommunication();
publisher_ = node_handle_.advertise<phasespace_acquisition::PhaseSpaceMarkerArray>(topic_name_, topic_queue_length_);
ROS_INFO_STREAM("[PhaseSpace] Node is publishing messages... (<ctrl+c> to terminate)");
}
else if (mode == "listener") {
private_node_handle_->param("log_file_name", log_file_name_, std::string(DEFAULT_LOG_FILE_TERMINAL_NAME));
private_node_handle_->param("log_file_name_old", log_file_name_old_, std::string(DEFAULT_LOG_FILE_TERMINAL_NAME_OLD));
// if 'log_multi_files_' is enabled, the user has to handle the logging process inside the main code using methods
// provided by this class and the 'Subject()' one. Otherwise any linstener node will build just two log files for
// the entire acquisition (same data but with a different way of formatting it)
private_node_handle_->param("log_multi_files", log_multi_files_, DEFAULT_LOG_MULTI_FILES);
private_node_handle_->param("log_files_base_path", log_file_base_path_, std::string(DEFAULT_LOG_FILE_BASE_PATH));
if (!log_multi_files_) {
// stores in 'date_time_' the current time converted into a handful form (date/time format YYYYMMDD_HHMMSS)
std::time_t raw_time;
char buffer[16];
std::time(&raw_time);
std::strftime(buffer, 16, "%G%m%d_%H%M%S", std::localtime(&raw_time));
date_time_ = buffer;
// creates folder if it doesn't exist
std::string command = "mkdir -p " + log_file_base_path_;
system(command.c_str());
// opens the log files
log_file_.open(log_file_base_path_ + date_time_ + log_file_name_ + ".dat");
log_file_old_.open(log_file_base_path_ + date_time_ + log_file_name_old_ + ".dat");
}
// the 'log_files_map_' is initialized in both cases
log_files_map_.insert(std::make_pair(log_file_name_, &log_file_));
log_files_map_.insert(std::make_pair(log_file_name_old_, &log_file_old_));
subscriber_ = node_handle_.subscribe(topic_name_, topic_queue_length_, &PhasespaceCore::messageCallback, this);
ROS_INFO_STREAM("[PhaseSpace] Node is retrieving messages... (<ctrl+c> to terminate)");
}
else {
// string 'mode' is not either 'talker' or 'listener' (unexpectedly)
ROS_FATAL_STREAM("[PhaseSpace] Undefined string passed to the constructor");
throw excp_;
}
// statistics variables initialization
markers_count_ = new std::vector<int> (init_marker_count_, 0);
partial_markers_count_ = new std::vector<int> (init_marker_count_, 0);
num_data_retrieved_ = 0;
partial_num_data_retrieved_ = 0;
num_log_files_ = 0;
start_time_ = ros::Time::now();
}
int main(int argc, char const *argv[]) {
//inicializacao
if(initializeCommunication(argc, argv) != 0) exit(-1); //inicailizacao falhou
putdebug("inicialização completa\n");
//inicializar conjunto de ligacoes
initializeConnectionSet();
int maxFd = curNode.fd; //descritor com valor mais elevado
char buffer[BUFSIZE]; //buffer utilizado para fazer a leitura dos descritores
int inputReady = 0; //indica se existe input disponivel para ler
int quit = FALSE;
while(!quit) {
//reinicializar conjunto de fds de leitura
FD_ZERO(&readFds);
//adicionar ligacoes no conjunto de fds
copySet(&readFds);
//adicionar listen fd ao conjunto de fds
FD_SET(curNode.fd, &readFds);
//adicionar stdin ao conjunto de fds
FD_SET(STDIN_FILENO, &readFds);
putdebug("CurNode - ring: %d id: %d ip: %s port: %s fd: %d",
curRing, curNode.id, curNode.ip, curNode.port, curNode.fd);
putdebug("SucciNode - id: %d ip: %s port: %s fd: %d",
succiNode.id, succiNode.ip, succiNode.port, succiNode.fd);
putdebug("PrediNode - id: %d ip: %s port: %s fd: %d",
prediNode.id, prediNode.ip, prediNode.port, prediNode.fd);
if(maxFd < getMaxConnection()) {
maxFd = getMaxConnection();
}
//esperar por descritor pronto para ler
putmessage("\r> ");
inputReady = select(maxFd + 1, &readFds, NULL, NULL, NULL);
if(inputReady <= 0) {
putdebugError("main", "select falhou");
continue;
}
if(FD_ISSET(curNode.fd, &readFds)) { //testar se o listen fd esta pronto para leitura
struct sockaddr_in addr;
socklen_t addrlen = sizeof(addr);
bzero(&addr, addrlen);
//aceitar ligacao
int connectionFd;
if( (connectionFd = accept(curNode.fd, (struct sockaddr*)&addr, &addrlen)) == -1) {
putdebugError("main", "ligação não foi aceite");
} else {
putdebug("nova ligação %d com endereço: %s", connectionFd, inet_ntoa(addr.sin_addr));
//adicionar descritor ao conjunto de descritores de ligacao
addConnection(connectionFd);
//definir um timeout para as comunicações
struct timeval tv;
tv.tv_sec = 5;
setsockopt(connectionFd, SOL_SOCKET, SO_SNDTIMEO,(struct timeval *)&tv,sizeof(struct timeval));
setsockopt(connectionFd, SOL_SOCKET, SO_RCVTIMEO,(struct timeval *)&tv,sizeof(struct timeval));
}
}
if(FD_ISSET(STDIN_FILENO, &readFds)) { //testar se o utilizador executou um comando
//ler comando do utilizador
bzero(buffer, sizeof(buffer));
if(fgets(buffer, BUFSIZE, stdin) != NULL) {
//executar comando do utilizador
putdebug("processar comando do utilizador");
int errorCode = executeUserCommand(buffer);
switch(errorCode) {
case 0: putdebug("comando de utilizador processado com sucesso"); break;
case -1: putdebugError("main", "falha no processamento do comando de utilizador"); break;
case 1:
{
putmessage("programa vai sair\n");
//fechar todos os sockets
int fd = getFirstConnection();
while(fd >= 0) {
close(fd);
rmConnection(fd);
//proxima ligacao
fd = getNextConnection(fd);
}
//fechar socket de escuta e socket do servidor de arranque
closeSockets();
quit = TRUE;
continue;
}
}
}
}
//ler fds de ligacoes actuais com o nó
int connectionFd = getFirstConnection();
while(connectionFd >= 0) {
if(FD_ISSET(connectionFd, &readFds)) {
//limpar buffer de rececao
bzero(buffer, sizeof(buffer));
//ler mensagem
if(readMessage(connectionFd, buffer, sizeof(buffer)) <= 0) {
close(connectionFd); //fechar ligacao
rmConnection(connectionFd); //remover no do conjunto de ligacoes
putdebug("ligacao %d terminada", connectionFd);
if(connectionFd == prediNode.fd) {
prediNode.fd = -1;
prediNode.id = -1;
putdebug("ligação terminada com predi");
if(succiNode.fd == -1 && !iAmStartNode) {
//estou sozinha mas não sou o nó de arranque
//isto significa que houve uma saida abrupta
//registar num novo anel
if(registerNewRing() == -1) {
putdebugError("handleEND", "não foi possível registar novo anel");
return -1;
}
}
}
if(connectionFd == succiNode.fd) {
succiNode.fd = -1;
succiNode.id = -1;
putdebug("ligação terminada com succi");
//colocar um alarme de 2 segundos para garnatir que todas as ligações
//que são par ser terminadas têm efeito no estado do nó
signal(SIGALRM, rebuildSignal);
alarm(REBUILD_INTERVAL);
}
} else {
//tratar mensagem recebida
putdebug("mensagem recebida pela ligacao %d: %s", connectionFd, buffer);
//remover \n
int length = strlen(buffer);
if(buffer[length - 1] == '\n')
buffer[length - 1] = '\0';
if(strcmp(buffer, "ERROR") == 0) {
//houve um erro na comunicação TCP
puterror("ocorreu um erro na comunicação entre nós\n");
closeConnection(&connectionFd);
continue;
}
if(handleMessage(buffer, connectionFd) == -1) {
//notificar quem enviou o pedido que ocorreu um erro
char answer[] = "ERROR\n";
sendMessage(connectionFd, answer);
if(connectionFd == prediNode.fd) {
prediNode.fd = -1;
prediNode.id = -1;
putdebug("ligação terminada com predi por causa de erro");
}
if(connectionFd == succiNode.fd) {
succiNode.fd = -1;
succiNode.id = -1;
putdebug("ligação terminada com succi por causa de erro");
}
//terminar ligacao
closeConnection(&connectionFd);
}
}
}
//proxima ligacao
connectionFd = getNextConnection(connectionFd);
}
}
return 0;
}
