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