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