Exemplo n.º 1
0
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);
  }
}
Exemplo n.º 2
0
// Create a laser device
laser_t *laser_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
                      int index, const char *drivername, int subscribe)
{
  char label[64];
  char section[64];
  laser_t *laser;
  
  laser = malloc(sizeof(laser_t));

  laser->proxy = playerc_laser_create(client, index);
  laser->drivername = strdup(drivername);
  laser->datatime = 0;

  snprintf(section, sizeof(section), "laser:%d", index);

  // Construct the menu
  snprintf(label, sizeof(label), "laser:%d (%s)", index, laser->drivername);
  laser->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
  laser->subscribe_item = rtk_menuitem_create(laser->menu, "Subscribe", 1);
  laser->style_item = rtk_menuitem_create(laser->menu, "Filled", 1);

#if 0
  laser->res025_item = rtk_menuitem_create(laser->menu, "0.25 deg resolution", 1);
  laser->res050_item = rtk_menuitem_create(laser->menu, "0.50 deg resolution", 1);
  laser->res100_item = rtk_menuitem_create(laser->menu, "1.00 deg resolution", 1);
#endif
  laser->range_mm_item = rtk_menuitem_create(laser->menu, "mm Range Resolution",1);
  laser->range_cm_item = rtk_menuitem_create(laser->menu, "cm Range Resolution",1);
  laser->range_dm_item = rtk_menuitem_create(laser->menu, "dm Range Resolution",1);

  // Set the initial menu state
  rtk_menuitem_check(laser->subscribe_item, subscribe);
  rtk_menuitem_check(laser->style_item, 1);

  // Construct figures
  laser->scan_fig = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 0);
  
  return laser;
}
Exemplo n.º 3
0
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;
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
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;
}