void browse_cb(player_sd_t* sd, player_sd_dev_t* dev) { if(dev->interf == PLAYER_LASER_CODE) { clients[num_laserdevs] = playerc_client_create(mclient, dev->hostname, dev->robot); if(0 != playerc_client_connect(clients[num_laserdevs])) exit(-1); playerc_client_datamode(clients[num_laserdevs], PLAYERC_DATAMODE_PUSH); // Create and subscribe to a laser device. lasers[num_laserdevs] = playerc_laser_create(clients[num_laserdevs], dev->index); if(playerc_laser_subscribe(lasers[num_laserdevs], PLAYER_OPEN_MODE)) exit(-1); // Add a callback to be invoked whenever we receive new data from this // laser playerc_client_addcallback(clients[num_laserdevs], &(lasers[num_laserdevs]->info), device_cb, lasers[num_laserdevs]); num_laserdevs++; printf("subscribed to: %s:%u:%s:%u\n", dev->hostname, dev->robot, interf_to_str(dev->interf), dev->index); printf("Now receiving %d lasers\n", num_laserdevs); } }
int main(int argc, const char **argv) { //Variaveis int degrees,PosRelX,PosRelY; float radians,Dlaser,ODM_ang, ang; int width = 500, height = 500; //Coloque o tamanho do mapa aqui (em pixel) int centroX = (width / 2); int centroY = (height / 2); playerc_client_t *client; playerc_laser_t *laser; playerc_position2d_t *position2d; CvPoint pt,pt1,pt2; CvScalar cinzaE,preto,cinzaC; char window_name[] = "Mapa"; IplImage* image = cvCreateImage( cvSize(width,height), 8, 3 ); cvNamedWindow(window_name, 1 ); preto = CV_RGB(0, 0, 0); //Para indicar obstaculos cinzaE = CV_RGB(92, 92, 92); //Para indicar o desconhecido cinzaC = CV_RGB(150, 150, 150); //Para indicar espacos livres printf ("debug: 11 - INICIO\n"); client = playerc_client_create(NULL, "localhost", 6665); printf ("debug: 12\n"); if (playerc_client_connect(client) != 0) return -1; printf ("debug: 13\n"); laser = playerc_laser_create(client, 0); printf ("debug: 21\n"); if (playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE)) return -1; printf ("debug: 22\n"); position2d = playerc_position2d_create(client, 0); if (playerc_position2d_subscribe(position2d, PLAYERC_OPEN_MODE) != 0) { printf ("err1\n"); fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } printf ("debug: 23\n"); if (playerc_client_datamode (client, PLAYERC_DATAMODE_PULL) != 0) { printf ("err2\n"); fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } printf ("debug: 24\n"); if (playerc_client_set_replace_rule (client, -1, -1, PLAYER_MSGTYPE_DATA, -1, 1) != 0) { printf ("err3\n"); fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } playerc_position2d_enable(position2d, 1); // Liga os motores printf ("debug: 25\n"); playerc_position2d_set_odom(position2d, 0, 0, 0); // Zera o odômetro cvSet(image, cinzaE,0); //Preencha a imagem com fundo cinza escuro pt.x = centroX; // Zera a coordenada X pt.y = centroY; // Zera a coordenada Y /* if( 0 != playerc_position2d_set_cmd_vel(position2d, 0, 0, DTOR(40.0), 1)) return -1; */ while(1) { printf ("debug: 26\n"); playerc_client_read(client); printf ("debug: 27\n"); //cvSaveImage("mapa1.jpg",image,0); printf ("debug: 28\n"); //playerc_client_read(client); printf ("debug: 29\n"); for (degrees = 2; degrees <= 360; degrees+=2) { printf ("debug: 30\n"); Dlaser = laser->scan[degrees][0]; printf ("debug: 31\n"); if (Dlaser < 8) { radians = graus2rad (degrees/2); //Converte o angulo do laser em graus para radianos printf ("debug: 32\n"); ODM_ang = position2d->pa; //Obtem o angulo relativo do robo ang = ((1.5*PI)+radians+ODM_ang); //Converte o angulo relativo em global printf ("debug: 33\n"); PosRelX = arredonda(position2d->px); //Posicao X relativa do robo PosRelY = arredonda(position2d->py); //Posicao Y relativa do robo printf ("debug: 34\n"); pt1.y = (centroY-PosRelY); //Coordenada y global do robo pt1.x = (centroX+PosRelX); //Coordenada x global do robo //converte coordenadas polares para retangulares (global) printf ("debug: 35\n"); pt.y = (int)(pt1.y-(sin(ang)*Dlaser*10)); pt.x = (int)(pt1.x+(cos(ang)*Dlaser*10)); printf ("debug: 36\n"); //Desenha a area livre cvLine(image, pt1,pt,cinzaC, 1,4,0); printf ("debug: 37\n"); //Marca o objeto no mapa cvLine(image, pt,pt,preto, 1,4,0); printf ("debug: 38\n"); //Mostra o resultado do mapeamento na tela //cvShowImage(window_name, image ); printf ("debug: 39\n"); //cvWaitKey(10); printf ("debug: 40\n"); } } } //Desconecta o player printf ("debug: 41\n"); playerc_laser_unsubscribe(laser); printf ("debug: 42\n"); playerc_laser_destroy(laser); printf ("debug: 43\n"); playerc_client_disconnect(client); printf ("debug: 44\n"); playerc_client_destroy(client); printf ("debug: 45\n"); //Destroi a janela OpenCV cvReleaseImage(&image); printf ("debug: 46\n"); cvDestroyWindow(window_name); printf ("debug: 47\n"); return 0; }
int player_init(int argc, char *argv[]) { int csize, usize, i; if(get_options(argc, argv) < 0) { print_usage(); exit(-1); } // Create a g_client object and connect to the server; the server must // be running on "localhost" at port 6665 g_client = playerc_client_create(NULL, g_hostname, g_port); playerc_client_set_transport(g_client, g_transport); if (0 != playerc_client_connect(g_client)) { fprintf(stderr, "error: %s\n", playerc_error_str()); exit(-1); } /* if (0 != playerc_client_datafreq(g_client, 20)) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } */ // Create a camera proxy (device id "camera:index") and susbscribe g_camera = playerc_camera_create(g_client, g_camera_index); if (0 != playerc_camera_subscribe(g_camera, PLAYER_OPEN_MODE)) { fprintf(stderr, "camera error: %s\n", playerc_error_str()); fprintf(stderr, "playercam will attempt to continue without a camera\n"); playerc_camera_destroy(g_camera); g_camera = NULL; } // Create a blobfinder proxy (device id "blobfinder:index") and susbscribe g_blobfinder = playerc_blobfinder_create(g_client, g_blobfinder_index); if (0 != playerc_blobfinder_subscribe(g_blobfinder, PLAYER_OPEN_MODE)) { fprintf(stderr, "blobfinder error: %s\n", playerc_error_str()); fprintf(stderr, "playercam will attempt to continue without a blobfinder\n"); playerc_blobfinder_destroy(g_blobfinder); g_blobfinder = NULL; } if ((NULL == g_camera) && (NULL == g_blobfinder)) { fprintf(stderr, "we need either a camera or blobfinder! aborting\n"); exit(-1); } // Get up to 10 images until we have a valid frame (g_wdith > 0) for (i=0, g_width=0; i < 10 && g_width==0 && NULL != playerc_client_read(g_client); ++i) { if (NULL != g_camera) { // Decompress the image csize = g_camera->image_count; playerc_camera_decompress(g_camera); usize = g_camera->image_count; g_print("camera: [w %d h %d d %d] [%d/%d bytes]\n", g_camera->width, g_camera->height, g_camera->bpp, csize, usize); g_width = g_camera->width; g_height = g_camera->height; if (allocated_size != usize) { g_img = realloc(g_img, usize); allocated_size = usize; } } else // try the blobfinder { g_print("blobfinder: [w %d h %d]\n", g_blobfinder->width, g_blobfinder->height); g_width = g_blobfinder->width; g_height = g_blobfinder->height; usize = g_width * g_height * 3; if (allocated_size != usize) { g_img = realloc(g_img, usize); allocated_size = usize; } // set the image data to 0 since we don't have a camera memset(g_img, 128, usize); } } g_window_width = g_width; g_window_height = g_height; assert(g_width>0); assert(g_height>0); playerc_client_datamode(g_client,PLAYER_DATAMODE_PULL); playerc_client_set_replace_rule(g_client,-1,-1,PLAYER_MSGTYPE_DATA,-1,1); return 0; }
/* * Connect to player at each host:port pair, as specified by the global * vars 'hostnames' and 'ports'. Also subscribes to each device, and adds * each client into a new multiclient (which is returned) */ playerc_mclient_t* init_player(playerc_client_t** clients, playerc_map_t** maps, playerc_localize_t** localizes, playerc_planner_t** planners, int num_bots, char** hostnames, int* ports, int data_freq, int map_idx, int planner_idx) { int i; playerc_mclient_t* mclient; /* Connect to Player */ assert(mclient = playerc_mclient_create()); for(i=0; i<num_bots; i++) { assert(clients[i] = playerc_client_create(mclient, hostnames[i], ports[i])); if(playerc_client_connect(clients[i]) < 0) { fprintf(stderr, "Failed to connect to %s:%d\n", hostnames[i], ports[i]); return(NULL); } #if 0 if(playerc_client_datafreq(clients[i],data_freq) < 0) { fprintf(stderr, "Failed to set data frequency\n"); return(NULL); } // request ALL data, rather than just NEW data, because the localizer // may only send out updates occasionally. if(playerc_client_datamode(clients[i],PLAYER_DATAMODE_PUSH_ALL) < 0) { fprintf(stderr, "Failed to set data mode\n"); return(NULL); } #endif // only subscribe to the first robot's map if(i==0) { assert(maps[i] = playerc_map_create(clients[i], map_idx)); if(playerc_map_subscribe(maps[i],PLAYER_OPEN_MODE) < 0) { fprintf(stderr, "Failed to subscribe to map\n"); return(NULL); } } else maps[i] = NULL; assert(localizes[i] = playerc_localize_create(clients[i], 0)); if(playerc_localize_subscribe(localizes[i],PLAYER_OPEN_MODE) < 0) { fprintf(stderr, "Warning: Failed to subscribe to localize on robot %d; you won't be able to set its pose.\n",i); playerc_localize_destroy(localizes[i]); localizes[i] = NULL; } assert(planners[i] = playerc_planner_create(clients[i], planner_idx)); if(playerc_planner_subscribe(planners[i],PLAYER_OPEN_MODE) < 0) { fprintf(stderr, "Warning: Failed to subscribe to planner on robot %d; you won't be able to give it goals.\n",i); playerc_planner_destroy(planners[i]); planners[i] = NULL; } } //playerc_map_unsubscribe(maps[0]); #if 0 /* Get at least one round of data from each robot */ for(;;) { if(playerc_mclient_read(mclient,-1) < 0) { fprintf(stderr, "Error on read\n"); return(NULL); } for(i=0; i<num_bots; i++) { if(!truths[i]->info.fresh || !lasers[i]->info.fresh || !planners[i]->info.fresh) break; } if(i==num_bots) break; } #endif return(mclient); }
static gint playerout_open(AFormat fmt, gint rate, gint nch) { gint pos; written = 0; afmt = fmt; sampleRate = rate; numChannels = nch; startTime = 0.0f; pausedTime = 0.0f; if (xmms_check_realtime_priority()) { xmms_show_message("Error", "You cannot use the Player Output plugin\n" "when you're running in realtime mode.", "Ok", FALSE, NULL, NULL); return 0; } /* do player server connection here */ // Create a client object and connect to the server; the server must // be running on "localhost" at port 6665 client = playerc_client_create(NULL, server_address, server_port); if (playerc_client_connect(client) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); client = NULL; return 0; } // Create a audio proxy susbscribe audio_proxy = playerc_audio_create(client, server_index); if (playerc_audio_subscribe(audio_proxy, PLAYERC_OPEN_MODE) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return 0; } // Set to PULL data mode if (playerc_client_datamode (client, PLAYERC_DATAMODE_PULL) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return 0; } if (playerc_client_set_replace_rule (client, -1, -1, PLAYER_MSGTYPE_DATA, -1, 1) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return 0; } bytesPerSecond = rate * nch; if (fmt == FMT_S16_LE || fmt == FMT_S16_BE || fmt == FMT_S16_NE || fmt == FMT_U16_LE || fmt == FMT_U16_BE || fmt == FMT_U16_NE) bytesPerSecond *= 2; // Allocate a buffer bufferLength = (int) (((float) bufferTime / 1000.0f) * bytesPerSecond); if (buffer != NULL) free (buffer); buffer = malloc (bufferLength); bufferPos = 0; return 1; }
int main(int argc, const char **argv) { playerc_client_t *client; playerc_opaque_t *ringLEDs; playerc_opaque_t *frontLED; playerc_opaque_t *bodyLED; client = playerc_client_create(NULL, "localhost", 6665); if(playerc_client_connect(client) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } ringLEDs = playerc_opaque_create(client, 0); if(playerc_opaque_subscribe(ringLEDs, PLAYERC_OPEN_MODE) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } frontLED = playerc_opaque_create(client, 1); if(playerc_opaque_subscribe(frontLED, PLAYERC_OPEN_MODE) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } bodyLED = playerc_opaque_create(client, 2); if(playerc_opaque_subscribe(bodyLED, PLAYERC_OPEN_MODE) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } if(playerc_client_datamode(client, PLAYERC_DATAMODE_PULL) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } if(playerc_client_set_replace_rule(client, -1, -1, PLAYER_MSGTYPE_DATA, -1, 1) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } /* Turn on the ring LEDs 2 and 6, and the front and body LEDs. */ player_opaque_data_t ringData, frontData, bodyData; ringData.data_count = 8; int led; for(led = 0; led < 8; led++) { ringData.data[led] = 0; } ringData.data[2] = 1; ringData.data[6] = 1; frontData.data_count = 1; frontData.data[0] = 1; bodyData.data_count = 1; bodyData.data[0] = 1; playerc_opaque_cmd(ringLEDs, &ringData); playerc_opaque_cmd(frontLED, &frontData); playerc_opaque_cmd(bodyLED, &bodyData); /* Without this sleep there will not be enough time for process all above * * messages. If the camera interface is not in provides section of Player * * configuration file, the time for e-puck initialization will be smaller, * * and consequently this sleep time will can be smaller. */ usleep(3e6); /* Shutdown and tidy up */ playerc_opaque_unsubscribe(ringLEDs); playerc_opaque_destroy(ringLEDs); playerc_opaque_unsubscribe(frontLED); playerc_opaque_destroy(frontLED); playerc_opaque_unsubscribe(bodyLED); playerc_opaque_destroy(bodyLED); playerc_client_disconnect(client); playerc_client_destroy(client); return 0; }
// Connect to the server int playerc_client_connect(playerc_client_t *client) { #if defined(HAVE_GETADDRINFO) struct addrinfo* addr_ptr = NULL; #else struct hostent* entp = NULL; #endif char banner[PLAYER_IDENT_STRLEN]; int ret; //double t; /* struct timeval last; struct timeval curr; */ #if defined (WIN32) unsigned long setting = 0; #else int old_flags; struct itimerval timer; struct sigaction sigact; #endif struct sockaddr_in clientaddr; // Construct socket if(client->transport == PLAYERC_TRANSPORT_UDP) { #if defined (WIN32) if((client->sock = socket(PF_INET, SOCK_DGRAM, 0)) == INVALID_SOCKET) #else if((client->sock = socket(PF_INET, SOCK_DGRAM, 0)) < 0) #endif { STRERROR(PLAYERC_ERR2, "socket() failed with error [%d: %s]"); return -1; } /* * INADDR_ANY indicates that any network interface (IP address) * for the local host may be used (presumably the OS will choose the * right one). * * Specifying sin_port = 0 allows the system to choose the port. */ clientaddr.sin_family = AF_INET; clientaddr.sin_addr.s_addr = INADDR_ANY; clientaddr.sin_port = 0; if(bind(client->sock, (struct sockaddr*)&clientaddr, sizeof(clientaddr)) < -1) { STRERROR(PLAYERC_ERR2, "bind() failed with error [%d: %s]"); return -1; } } else { #if defined (WIN32) if((client->sock = socket(PF_INET, SOCK_STREAM, 0)) == INVALID_SOCKET) #else if((client->sock = socket(PF_INET, SOCK_STREAM, 0)) < 0) #endif { STRERROR(PLAYERC_ERR2, "socket() failed with error [%d: %s]"); return -1; } } #if ENABLE_TCP_NODELAY // Disable Nagel's algorithm for lower latency { int yes = 1; if(setsockopt(client->sock, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(int)) == -1) { PLAYERC_ERR("failed to enable TCP_NODELAY - setsockopt failed"); return -1; } } #endif // Construct server address memset(&client->server, 0, sizeof(client->server)); client->server.sin_family = AF_INET; client->server.sin_port = htons(client->port); #if defined(HAVE_GETADDRINFO) if (getaddrinfo(client->host, NULL, NULL, &addr_ptr) != 0) { playerc_client_disconnect(client); PLAYERC_ERR("getaddrinfo() failed with error"); return -1; } assert(addr_ptr); assert(addr_ptr->ai_addr); #ifdef AF_INET6 if (((addr_ptr->ai_addr->sa_family) != AF_INET) && ((addr_ptr->ai_addr->sa_family) != AF_INET6)) #else if ((addr_ptr->ai_addr->sa_family) != AF_INET) #endif { playerc_client_disconnect(client); PLAYERC_ERR("unsupported internet address family"); return -1; } client->server.sin_addr.s_addr = ((struct sockaddr_in *)(addr_ptr->ai_addr))->sin_addr.s_addr; freeaddrinfo(addr_ptr); addr_ptr = NULL; #else entp = gethostbyname(client->host); if (entp == NULL) { playerc_client_disconnect(client); STRERROR(PLAYERC_ERR2, "gethostbyname() failed with error [%d: %s]"); return -1; } assert(entp->h_length <= sizeof (client->server.sin_addr)); memcpy(&(client->server.sin_addr), entp->h_addr_list[0], entp->h_length); #endif // Connect the socket /* t = client->request_timeout; do { if (t <= 0) { PLAYERC_ERR2("connect call on [%s:%d] timed out", client->host, client->port); return -1; } gettimeofday(&last,NULL); puts("calling connect"); ret = connect(client->sock, (struct sockaddr*)&client->server, sizeof(client->server)); gettimeofday(&curr,NULL); t -= ((curr.tv_sec + curr.tv_usec/1e6) - (last.tv_sec + last.tv_usec/1e6)); } while (ret == -1 && (errno == EALREADY || errno == EAGAIN || errno == EINPROGRESS)); */ // In Windows, the connect timeout is (apparently) a registry setting. #if !defined (WIN32) /* Set up a timer to interrupt the connection process */ timer.it_interval.tv_sec = 0; timer.it_interval.tv_usec = 0; timer.it_value.tv_sec = (int)floor(client->request_timeout); timer.it_value.tv_usec = (int)rint(fmod(client->request_timeout,timer.it_value.tv_sec)*1e6); if(setitimer(ITIMER_REAL, &timer, NULL) != 0) PLAYERC_WARN("failed to set up connection timer; " "indefinite hang may result"); /* Turn off system call restart so that connect() will terminate when the * alarm goes off */ if(sigaction(SIGALRM, NULL, &sigact) != 0) PLAYERC_WARN("failed to get SIGALRM action data; " "unexpected exit may result"); else { #ifdef SA_RESTART sigact.sa_handler = dummy; sigact.sa_flags &= ~SA_RESTART; if(sigaction(SIGALRM, &sigact, NULL) != 0) #endif PLAYERC_WARN("failed to set SIGALRM action data; " "unexpected exit may result"); } #endif ret = connect(client->sock, (struct sockaddr*)&client->server, sizeof(client->server)); #if !defined (WIN32) /* Turn off timer */ timer.it_value.tv_sec = 0; timer.it_value.tv_usec = 0; if(setitimer(ITIMER_REAL, &timer, NULL) != 0) PLAYERC_WARN("failed to turn off connection timer; " "unexpected exit may result"); /* Restore normal SIGALRM behavior */ #ifdef SA_RESTART sigact.sa_handler = SIG_DFL; sigact.sa_flags |= SA_RESTART; if(sigaction(SIGALRM, &sigact, NULL) != 0) #endif PLAYERC_WARN("failed to reset SIGALRM action data; " "unexpected behavior may result"); #endif if (ret < 0) { playerc_client_disconnect(client); #if defined (WIN32) FormatMessage (FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL, ErrNo, 0, (LPTSTR) &errBuffer, 0, NULL); PLAYERC_ERR4 ("connect call on [%s:%d] failed with error [%d:%s]", client->host, client->port, ErrNo, (LPTSTR) errBuffer); LocalFree (errBuffer); #else PLAYERC_ERR4("connect call on [%s:%d] failed with error [%d:%s]", client->host, client->port, errno, strerror(ErrNo)); #endif return -1; } // For UDP, send an empty msg to get things going if(client->transport == PLAYERC_TRANSPORT_UDP) { if(send(client->sock, NULL, 0, 0) < 0) { STRERROR(PLAYERC_ERR2, "send() failed with error [%d: %s]"); return -1; } } // set socket to be blocking #if defined (WIN32) if (ioctlsocket (client->sock, FIONBIO, &setting) == SOCKET_ERROR) { FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL, ErrNo, 0, (LPTSTR) &errBuffer, 0, NULL); PLAYERC_ERR1("error getting socket flags [%s]", (LPTSTR) errBuffer); LocalFree(errBuffer); } #else if ((old_flags = fcntl(client->sock, F_GETFL)) < 0) { PLAYERC_ERR1("error getting socket flags [%s]", strerror(errno)); return -1; } if (fcntl(client->sock, F_SETFL, old_flags & ~O_NONBLOCK) < 0) { PLAYERC_ERR1("error setting socket non-blocking [%s]", strerror(errno)); return -1; } #endif // Get the banner if (timed_recv(client->sock, banner, sizeof(banner), 0, 2000) < sizeof(banner)) { playerc_client_disconnect(client); PLAYERC_ERR("incomplete initialization string"); return -1; } //set the datamode to pull playerc_client_datamode(client, PLAYER_DATAMODE_PULL); PLAYERC_WARN4("[%s] connected on [%s:%d] with sock %d\n", banner, client->host, client->port, client->sock); client->connected = 1; return 0; }
// Main int main(int argc, char **argv) { playerc_client_t *client; rtk_app_t *app; mainwnd_t *mainwnd; opt_t *opt; const char *host; int port; int i; int count; double rate; char section[256]; int device_count; device_t devices[PLAYER_MAX_DEVICES]; device_t *device; struct timeval tv, tc = {0, 0}; struct timespec st = {0, (1.0/GUI_UPDATE_RATE) * 1e9}; printf("PlayerViewer %s\n", PLAYER_VERSION); // Initialise rtk lib (after we have read the program options we // want). rtk_init(&argc, &argv); // Register signal handlers signal(SIGINT, sig_quit); signal(SIGQUIT, sig_quit); // Load program options opt = opt_init(argc, argv, NULL); if (!opt) { print_usage(); return -1; } // Pick out some important program options host = opt_get_string(opt, "", "host", NULL); if (!host) host = opt_get_string(opt, "", "h", "localhost"); port = opt_get_int(opt, "", "port", -1); if (port < 0) port = opt_get_int(opt, "", "p", 6665); rate = opt_get_double(opt, "", "rate", 5.0); if(rate < 0.0) rate = 0.0; // Connect to the server printf("Connecting to [%s:%d]\n", host, port); client = playerc_client_create(NULL, host, port); if (playerc_client_connect(client) != 0) { PRINT_ERR1("%s", playerc_error_str()); print_usage(); return -1; } if(rate == 0.0) { printf("Setting delivery mode to PLAYER_DATAMODE_PUSH\n"); // Change the server's data delivery mode. if (playerc_client_set_replace_rule(client, -1, -1, -1, -1, 0) != 0) { PRINT_ERR1("%s", playerc_error_str()); return -1; } // Change the server's data delivery mode. // PLAYERC_DATAMODE_PUSH, PLAYERC_DATAMODE_PULL if (playerc_client_datamode(client, PLAYERC_DATAMODE_PUSH) != 0) { PRINT_ERR1("%s", playerc_error_str()); return -1; } } // Get the available devices. if (playerc_client_get_devlist(client) != 0) { PRINT_ERR1("%s", playerc_error_str()); return -1; } // Create gui app = rtk_app_create(); // Create a window for most of the sensor data mainwnd = mainwnd_create(app, host, port); if (!mainwnd) return -1; // Create a list of available devices, with their gui proxies. device_count = 0; for (i = 0; i < client->devinfo_count; i++) { device = devices + device_count; device->addr = client->devinfos[i].addr; device->drivername = strdup(client->devinfos[i].drivername); // See if the device should be subscribed immediately. snprintf(section, sizeof(section), "%s:%d", interf_to_str(device->addr.interf), device->addr.index); device->subscribe = opt_get_int(opt, section, "", 0); device->subscribe = opt_get_int(opt, section, "subscribe", device->subscribe); if (device->addr.index == 0) { snprintf(section, sizeof(section), "%s", interf_to_str(device->addr.interf)); device->subscribe = opt_get_int(opt, section, "", device->subscribe); device->subscribe = opt_get_int(opt, section, "subscribe", device->subscribe); } // Allow for --position instead of --position2d if(device->addr.interf == PLAYER_POSITION2D_CODE) { snprintf(section, sizeof(section), "%s:%d", PLAYER_POSITION2D_STRING, device->addr.index); device->subscribe = opt_get_int(opt, section, "", device->subscribe); device->subscribe = opt_get_int(opt, section, "subscribe", device->subscribe); if (device->addr.index == 0) { snprintf(section, sizeof(section), "%s", PLAYER_POSITION2D_STRING); device->subscribe = opt_get_int(opt, section, "", device->subscribe); device->subscribe = opt_get_int(opt, section, "subscribe", device->subscribe); } } // Create the GUI proxy for this device. create_proxy(device, opt, mainwnd, client); device_count++; } // Print the list of available devices. printf("Available devices: %s:%d\n", host, port); for (i = 0; i < device_count; i++) { device = devices + i; snprintf(section, sizeof(section), "%s:%d", interf_to_str(device->addr.interf), device->addr.index); printf("%-16s %-40s", section, device->drivername); if (device->proxy) { if (device->subscribe) printf("subscribed"); else printf("ready"); } else printf("unsupported"); printf("\n"); } // Print out a list of unused options. opt_warn_unused(opt); // Start the gui; dont run in a separate thread and dont let it do // its own updates. rtk_app_main_init(app); // start out timer if in pull mode if(rate > 0.0) gettimeofday(&tv, NULL); while (!quit) { // Let gui process messages rtk_app_main_loop(app); if(rate == 0.0) // if we're in push mode { // see if there's data count = playerc_client_peek(client, 50); if (count < 0) { PRINT_ERR1("%s", playerc_error_str()); break; } if (count > 0) { /*proxy = */playerc_client_read_nonblock(client); } } else // we're in pull mode { // we only want to request new data at the target rate gettimeofday(&tc, NULL); if(((tc.tv_sec - tv.tv_sec) + (tc.tv_usec - tv.tv_usec)/1e6) > 1.0/rate) { tv = tc; // this requests a round of data from the server to be read playerc_client_requestdata(client); playerc_client_read_nonblock(client); } else { // sleep for the minimum time we can, so we don't use up too much // processor nanosleep(&st, NULL); } } // Update the devices for (i = 0; i < device_count; i++) { device = devices + i; if(device->proxy) (*(device->fnupdate)) (device->proxy); } // Update the main window if (mainwnd_update(mainwnd) != 0) break; } // Stop the gui rtk_app_main_term(app); // Destroy devices for (i = 0; i < device_count; i++) { device = devices + i; if (device->proxy) (*(device->fndestroy)) (device->proxy); free(device->drivername); } // Disconnect from server if (playerc_client_disconnect(client) != 0) { PRINT_ERR1("%s", playerc_error_str()); return -1; } playerc_client_destroy(client); // For some reason, either of the following calls makes the program // segfault on exit. I haven't figured out why, so I'm commenting them out. - BPG // Destroy the windows //mainwnd_destroy(mainwnd); // Destroy the gui //rtk_app_destroy(app); opt_term(opt); return 0; }
int main(int argc, const char **argv) { //Variables int degrees,PosRelX,PosRelY; float radians,Dlaser,ODM_ang, ang; int width = 500, height = 500; //Create the size of the map here (in pixel) int centroX = (width / 2); int centroY = (height / 2); playerc_client_t *client; playerc_laser_t *laser; playerc_position2d_t *position2d; CvPoint pt,pt1,pt2; CvScalar cinzaE,preto,cinzaC; char window_name[] = "Map"; IplImage* image = cvCreateImage( cvSize(width,height), 8, 3 ); cvNamedWindow(window_name, 1 ); preto = CV_RGB(0, 0, 0); //for indicating obstacles cinzaE = CV_RGB(92, 92, 92); //To indicate the stranger cinzaC = CV_RGB(150, 150, 150); //To indicate free spaces client = playerc_client_create(NULL, "localhost", 6665); if (playerc_client_connect(client) != 0) return -1; laser = playerc_laser_create(client, 0); if (playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE)) return -1; position2d = playerc_position2d_create(client, 0); if (playerc_position2d_subscribe(position2d, PLAYERC_OPEN_MODE) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } if (playerc_client_datamode (client, PLAYERC_DATAMODE_PULL) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } if (playerc_client_set_replace_rule (client, -1, -1, PLAYER_MSGTYPE_DATA, -1, 1) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } playerc_position2d_enable(position2d, 1); // initialise motors playerc_position2d_set_odom(position2d, 0, 0, 0); // Set odometer to zero cvSet(image, cinzaE,0); //set the image colour to dark pt.x = centroX; // Zero coordinate for x pt.y = centroY; // Zero coordinate for y while(1) { playerc_client_read(client); cvSaveImage("mapa.jpg",image); playerc_client_read(client); for (degrees = 2; degrees <= 360; degrees+=2) { Dlaser = laser->scan[degrees][0]; if (Dlaser < 8) { radians = graus2rad (degrees/2); //Convert the angle of the laser to radians ODM_ang = position2d->pa; //Obtain the angle relative to the robot ang = ((1.5*PI)+radians+ODM_ang); //Converte the angle relative to the world PosRelX = arredonda(position2d->px); //Position x relative to robot PosRelY = arredonda(position2d->py); //Position y relative to robot pt1.y = (centroY-PosRelY); //Co-ordinated global y of the robot pt1.x = (centroX+PosRelX); //Co-ordinated global x of the robot //t converts polar coordinates for rectangular (global) pt.y = (int)(pt1.y-(sin(ang)*Dlaser*10)); pt.x = (int)(pt1.x+(cos(ang)*Dlaser*10)); //The free area draws cvline cvLine(image, pt1,pt,cinzaC, 1,4,0); //marks the object in the map cvLine(image, pt,pt,preto, 1,4,0); //Shows the result of the map to the screen cvShowImage(window_name, image ); cvWaitKey(10); } } } //Disconnect player playerc_laser_unsubscribe(laser); playerc_laser_destroy(laser); playerc_client_disconnect(client); playerc_client_destroy(client); //Destroy the OpenCV window cvReleaseImage cvReleaseImage(&image); cvDestroyWindow(window_name); return 0; }