コード例 #1
0
int
main(int argc, const char **argv)
{
  int i;

  // A service discovery object
  player_sd_t* sd;

  // Initialize multiclient
  mclient = playerc_mclient_create();

  // Initialize service discovery
  sd = player_sd_init();

  // Look for Player devices
  if(player_sd_browse(sd, 0.0, 1, browse_cb) != 0)
  {
    puts("player_sd_browse error");
    exit(-1);
  }

  for(;;)
  {
    // Update name service
    player_sd_update(sd,0.0);

    // Wait for new data from server
    playerc_mclient_read(mclient,10);
  }

  // Shutdown
  for(i=0;i<num_laserdevs;i++)
  {
    playerc_laser_unsubscribe(lasers[i]);
    playerc_laser_destroy(lasers[i]);
    playerc_client_disconnect(clients[i]);
    playerc_client_destroy(clients[i]);
  }

  playerc_mclient_destroy(mclient);

  return(0);
}
コード例 #2
0
// Destroy a laser device
void laser_destroy(laser_t *laser)
{
  if (laser->proxy->info.subscribed)
    playerc_laser_unsubscribe(laser->proxy);
  playerc_laser_destroy(laser->proxy);

  rtk_fig_destroy(laser->scan_fig);

#if 0
  rtk_menuitem_destroy(laser->res025_item);
  rtk_menuitem_destroy(laser->res050_item);
  rtk_menuitem_destroy(laser->res100_item);
#endif
  rtk_menuitem_destroy(laser->range_mm_item);
  rtk_menuitem_destroy(laser->range_cm_item);
  rtk_menuitem_destroy(laser->range_dm_item);
  rtk_menuitem_destroy(laser->subscribe_item);
  rtk_menuitem_destroy(laser->style_item);
  rtk_menu_destroy(laser->menu);

  free(laser->drivername);
  
  free(laser);
}
コード例 #3
0
ファイル: 2Dmapping.c プロジェクト: alessandrocollioni/TCC
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;
}
コード例 #4
0
ファイル: sensor1.cpp プロジェクト: lucasaoki/prm-trab
int main(int argc, const char **argv) {
    int i;
    int porta = 6665;
    double x, y;
    char livre;
    char end_ip[20];

    // OpenCV Variables  
    char wndname[30] = "Drawing Demo";
    int line_type = CV_AA; // change it to 8 to see non-antialiased graphics
    CvPoint pt1, pt2;
    IplImage* image;
    int width = MAX_X, height = MAX_Y; // 200 x 100 pixels 

    // Player-Stage Variables
    playerc_client_t *client;
    playerc_position2d_t *position2d;
    playerc_laser_t *laser;

    // Create a window
    image = cvCreateImage(cvSize(width, height), 8, 3);
    cvNamedWindow(wndname, 1);
    cvZero(image);
    pt1.x = 100;
    pt1.y = MAX_Y;
    pt2.x = 100;
    pt2.y = MAX_Y - 80;
    cvLine(image, pt1, pt2, CV_RGB(255, 255, 255), 2, line_type, 0);
    pt2.x = 20;
    pt2.y = MAX_Y;
    cvLine(image, pt1, pt2, CV_RGB(255, 255, 255), 2, line_type, 0);
    pt2.x = 180;
    pt2.y = MAX_Y;
    cvLine(image, pt1, pt2, CV_RGB(255, 255, 255), 2, line_type, 0);
    cvShowImage(wndname, image);
    cvWaitKey(1000);

    cvZero(image);
    pt1.x = 20;
    pt1.y = MAX_Y;
    pt2.x = 160;
    pt2.y = MAX_Y - 80;
    cvRectangle(image, pt1, pt2, CV_RGB(255, 255, 255), 2, line_type, 0);
    cvShowImage(wndname, image);
    cvWaitKey(1000);

    strcpy(end_ip, "localhost");

    if (argc >= 2) /* Get Port */
        porta = atoi(argv[1]);
    if (argc >= 3) /* Get IP Address */
        strcpy(end_ip, argv[2]);

    printf("Porta: %d\n", porta);
    printf("IP: %s\n", end_ip);

    client = playerc_client_create(NULL, end_ip, porta);
    if (playerc_client_connect(client) != 0)
        return -1;

    // Connect to Position
    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;
    }

    // Enable motor control
    playerc_position2d_enable(position2d, 1);

    // Connect to Laser
    laser = playerc_laser_create(client, 0);
    if (playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE))
        return -1;

    // Read several times the robot data (delay)
    playerc_client_read(client);
    playerc_client_read(client);
    playerc_client_read(client);
    playerc_client_read(client);
    playerc_client_read(client);

    while (1) {
        playerc_client_read(client);

        // scan for free 100 cm in front of robot
        livre = 1;
        cvZero(image);
        for (i = 0; i < 360; i++) {
            if ((laser->scan[i][0]) < 0.5)
                livre = 0;

            //  Debug: if (laser->scan[i][0] <= 0) printf("#");

            if (laser->scan[i][0] < 7.8) {
                x = laser->scan[i][0] * cos(laser->scan[i][1] + 3.1415926 / 2.0);
                y = laser->scan[i][0] * sin(laser->scan[i][1] + 3.1415926 / 2.0);

                pt1.x = (int) (x * 10 + 100);
                pt1.y = (int) (MAX_Y - y * 10);
                cvCircle(image, pt1, 2, CV_RGB(255, 255, 255), 1, line_type, 0);
            }
        }
        cvShowImage(wndname, image);

        // if free moves, otherwise turns
        if (livre)
            playerc_position2d_set_cmd_vel(position2d, 0.2, 0, 0.0, 1);
        else
            playerc_position2d_set_cmd_vel(position2d, 0.0, 0, 0.4, 1);

        cvWaitKey(10);
    }

    playerc_laser_unsubscribe(laser);
    playerc_laser_destroy(laser);
    playerc_client_disconnect(client);
    playerc_client_destroy(client);

    return 0;
}
コード例 #5
0
ファイル: 2Dmapping.c プロジェクト: swapi122/group12
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;
}