void Nav_MotionEstimator::Nav_join()
{
	m_Thread.join();

	cout
	<< "<TRACE><LOG><Nav_MotionEstimator><Nav_join> Destroy Laser Object "
	<< endl;
	// Disable and delete elements the motor
	delete ScanObj;
	sleep (1);
	cout
	<< "<TRACE><LOG><Nav_MotionEstimator><Nav_join> Disable Motors and close connection to Server "
	<< endl;

#ifdef USEGAZEBO

#elif defined PLAYERPLUGGING
#else
	playerc_position2d_enable (position2d, false);
	playerc_position2d_unsubscribe ( position2d );
	playerc_position2d_destroy ( position2d );
	playerc_client_disconnect(client);
	playerc_client_destroy(client);
#endif

}
示例#2
0
/* Main()*/
int main(int argc, const char **argv)
{
  // Create a client object and connect to the server; the server must
  // be running on "localhost" at port 6665
  client = playerc_client_create(NULL, "gort", 9876);
  if (playerc_client_connect(client) != 0)
  {
    fprintf(stderr, "error: %s\n", playerc_error_str());
    return -1;
  }

  // Create a bumper proxy (device id "bumper:0" and subscribe
   // in read mode
  bumper = playerc_bumper_create(client, 0);
  if(playerc_bumper_subscribe(bumper,PLAYERC_OPEN_MODE)!= 0)
  {
    fprintf(stderr, "error: %s\n", playerc_error_str());
    return -1;
  }

  // Create a position2d proxy (device id "position2d:0") and susbscribe
  // in read/write mode
  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 the robots motors
  playerc_position2d_enable(position2d, 1);
  playerc_client_read(client);
  
  // Point 1 to Point 2
  Move(client, MOVE1, ANGLE1);
  Turn(client, TURN1);

  // Point 2 to Point 3
  Move(client, MOVE2, ANGLE2);
  Turn(client, TURN2);

  // Point 3 to Point 4
  Move(client, MOVE3, ANGLE3);
  Turn(client, TURN3);

  // Point 4 to Point 5
  Move(client, MOVE4, ANGLE4);

  // Shutdown and tidy up
  playerc_position2d_unsubscribe(position2d);
  playerc_position2d_destroy(position2d);
  playerc_client_disconnect(client);
  playerc_client_destroy(client);

  return 0;
}
示例#3
0
int main(int argc, char *argv[]) {
  playerc_client_t *robot;
  playerc_position2d_t *pp;
  playerc_simulation_t *sp;

  /* Create a client and connect it to the server. */
  robot = playerc_client_create(NULL, "localhost", 6665);
  if (0 != playerc_client_connect(robot)) return -1;

  /* Create and subscribe to a simulation proxy. */
  sp = playerc_simulation_create(robot, 0);
  if (playerc_simulation_subscribe(sp, PLAYER_OPEN_MODE)) return -1;

  pp = playerc_position2d_create(robot, 0);
  if (playerc_position2d_subscribe(pp, PLAYER_OPEN_MODE)) return -1;

  /* read from the proxies */
  playerc_client_read(robot);

    // get and re-set the color
    float green[]= {0.67, 0.88, 0.43, 1};
    float puckcolor[4];
  	playerc_client_read(robot);

	playerc_simulation_get_property(sp,(char *)"puck1",(char*)"color",puckcolor,4*sizeof(float));
	printf("Puck1 is color = (%.2f,%.2f,%.2f,%.2f)\n",
			puckcolor[0], puckcolor[1], puckcolor[2], puckcolor[3]);

	playerc_simulation_get_property(sp,(char *)"puck2",(char*)"color",puckcolor,4*sizeof(float));
	printf("Puck2 is color = (%.2f,%.2f,%.2f,%.2f)\n",
			puckcolor[0], puckcolor[1], puckcolor[2], puckcolor[3]);

	playerc_simulation_get_property(sp,(char *)"puck3",(char*)"color",puckcolor,4*sizeof(float));
	printf("Puck3 is color = (%.2f,%.2f,%.2f,%.2f)\n",
			puckcolor[0], puckcolor[1], puckcolor[2], puckcolor[3]);

    printf("setting puck1 to green\n");

	playerc_simulation_set_property(sp,(char *)"puck1",(char*)"color",green,4*sizeof(float));

	playerc_simulation_get_property(sp,(char *)"puck1",(char*)"color",puckcolor,4*sizeof(float));
	printf("Puck3 is color = (%.2f,%.2f,%.2f,%.2f)\n",
			puckcolor[0], puckcolor[1], puckcolor[2], puckcolor[3]);

  /* Shutdown */
  playerc_simulation_unsubscribe(sp);
  playerc_position2d_unsubscribe(pp);
  playerc_simulation_destroy(sp);
  playerc_position2d_destroy(pp);
  playerc_client_disconnect(robot);
  playerc_client_destroy(robot);

  return 0;
}
示例#4
0
文件: tenft.c 项目: murrayju/cs1567-1
int main(int argc, const char **argv)
{
    double dist, speed;
    playerc_client_t *client;
    playerc_position2d_t *position2d;

    // Create a client object and connect to the server; the server must
    // be running on "localhost" at port 6665
    client = playerc_client_create(NULL, "gort", 9876);
    if (playerc_client_connect(client) != 0)
    {
        fprintf(stderr, "error: %s\n", playerc_error_str());
        return -1;
    }

    // Create a position2d proxy (device id "position2d:0") and susbscribe
    // in read/write mode
    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 the robots motors
    playerc_position2d_enable(position2d, 1);

    // Start the robot moving
    dist = 0;
    speed = 1;
    playerc_position2d_set_cmd_vel(position2d, speed, 0, 0, 1);
    while( dist <= 3.048 ) {
        // Read data from the server and display current robot position
        playerc_client_read(client);
        printf("position : %f %f %f\n", position2d->px, position2d->py, position2d->pa);
        dist = position2d->px;
        if(dist > 2.6 && speed > .01) {
            playerc_position2d_set_cmd_vel(position2d, (speed /= 2), 0, 0, 1);
        }
    }

    //stop the robot
    playerc_position2d_set_cmd_vel(position2d, 0, 0, 0, 1);

    // Shutdown and tidy up
    playerc_position2d_unsubscribe(position2d);
    playerc_position2d_destroy(position2d);
    playerc_client_disconnect(client);
    playerc_client_destroy(client);

    return 0;
}
示例#5
0
int main(int argc, const char **argv)
{
  int i;
  playerc_client_t *client;
  playerc_position2d_t *position2d;

  // Create a client object and connect to the server; the server must
  // be running on "localhost" at port 6665
  client = playerc_client_create(NULL, "localhost", 6665);
  if (playerc_client_connect(client) != 0)
  {
    fprintf(stderr, "error: %s\n", playerc_error_str());
    return -1;
  }

  // Create a position2d proxy (device id "position2d:0") and susbscribe
  // in read/write mode
  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 the robots motors
  playerc_position2d_enable(position2d, 1);

  // Start the robot turning slowing
  playerc_position2d_set_cmd_vel(position2d, 0, 0, 0.1, 1);

  for (i = 0; i < 200; i++)
  {
    // Read data from the server and display current robot position
    playerc_client_read(client);
    printf("position : %f %f %f\n",
           position2d->px, position2d->py, position2d->pa);
  } 

  // Shutdown and tidy up
  playerc_position2d_unsubscribe(position2d);
  playerc_position2d_destroy(position2d);
  playerc_client_disconnect(client);
  playerc_client_destroy(client);

  return 0;
}
示例#6
0
static void playerout_close(void)
{
	if (client)
	{
		// Shutdown and tidy up
		playerc_audio_unsubscribe(audio_proxy);
		playerc_audio_destroy(audio_proxy);
		playerc_client_disconnect(client);
		playerc_client_destroy(client);

		audio_proxy = NULL;
		client = NULL;

		free (buffer);
		buffer = NULL;
	}
}
示例#7
0
int
player_quit()
{
  if (NULL != g_camera)
  {
    playerc_camera_unsubscribe(g_camera);
    playerc_camera_destroy(g_camera);
  }
  if (NULL != g_blobfinder)
  {
    playerc_blobfinder_unsubscribe(g_blobfinder);
    playerc_blobfinder_destroy(g_blobfinder);
  }
  playerc_client_disconnect(g_client);
  playerc_client_destroy(g_client);
  return 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);
}
示例#9
0
int
main(int argc, const char **argv)
{
  int i;
  playerc_client_t *client;
  playerc_position2d_t *position2d;

  // Create a client and connect it to the server.
  client = playerc_client_create(NULL, "localhost", 6665);
  if (0 != playerc_client_connect(client))
    return -1;

  // Create and subscribe to a position2d device.
  position2d = playerc_position2d_create(client, 0);
  if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
    return -1;

  // Make the robot spin!
  if (0 != playerc_position2d_set_cmd_vel(position2d, 0.25, 0, DTOR(40.0), 1))
    return -1;

  for (i = 0; i < 200; i++)
  {
    // Wait for new data from server
    playerc_client_read(client);

    // Print current robot pose
    printf("position2d : %f %f %f\n",
           position2d->px, position2d->py, position2d->pa);
  }

  // Shutdown
  playerc_position2d_unsubscribe(position2d);
  playerc_position2d_destroy(position2d);
  playerc_client_disconnect(client);
  playerc_client_destroy(client);

  return 0;
}
示例#10
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;
}
示例#11
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;
}
示例#12
0
int
main(int argc, char **argv)
{
  // Initialize movement parameters  (default to a square)
  int n = 4;
  int l = 65;
  int v = 50;
  int a = 90;

  // Parse command-line arguements
  int option_index = 0;
  static struct option long_options[] = {
    {"line",     0, 0, 0 },
    {"triangle", 0, 0, 1 },
    {"square",   0, 0, 2 },
    {"pentagon", 0, 0, 3 },
    {"hexagon",  0, 0, 4 },
    {0,          0, 0, 0 }
  };

  int shape_set = false;
  int angle_set = false;
  int num_set = false;
  int reverse_direction = false;
  int smooth_acceleration = false;
  int c;
  while ((c = getopt_long(argc, argv, "l:v:n:a:rs",
              long_options, &option_index)) != -1)
    switch (c) {
      case 0: // line
        n = 2; a = 180; shape_set = true; break;
      case 1: // triangle
        n = 3; a = 120; shape_set = true; break;
      case 2: // square
        n = 4; a = 90; shape_set = true; break;
      case 3: // pentagon
        n = 5; a = 72; shape_set = true; break;
      case 4: // hexagon
        n = 6; a = 60; shape_set = true; break;
      case 'l': // 0 to 200 cm (default 65)
        l = atoi(optarg); break;
      case 'v': // 0 to 50 cm/sec (default 50)
        v = atoi(optarg); break;
      case 'n': // 0 to 10 (default 4)
        n = atoi(optarg); num_set = true; break;
      case 'a': // 0 to 180 degrees (default 90)
        a = atoi(optarg); angle_set = true; break;
      case 'r':
        reverse_direction = true; break;
      case 's':
        smooth_acceleration = true; break;
      case '?':
        if (optopt == 'v' || optopt == 'a' || optopt == 'l' || optopt == 'n')
          fprintf (stderr, "Option -%c requires an argument.\n", optopt);
        else if (isprint (optopt))
          fprintf (stderr, "Unknown option `-%c'.\n", optopt);
        else
          fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
        return 1;
      default:
        printf("Bad arguments.\n\nUsage: dead_reckoning -n <number of sides> "
               "-l <length of each side> -v <velocity (cm/sec)> "
               "-a <turning angle (degrees)>");
        exit(1);
    }
  int i = 0;
  for (i = optind; i < argc; i++)
    printf ("Non-option argument %s\n", argv[i]);

  if(!shape_set && (!num_set || !angle_set)) {
    printf("\nYou need to specify a shape (by name, or with the arguments "
           "'n' and 'a').\n\nUsage: dead_reckoning --<shape> "
           "[-l <side length>] [-v <velocity>] [-r] [-s]\n\n"
           "Alternative usage: dead_reckoning -n <number of sides> "
           "-a <angle of each corner in degrees> "
           "[-l <length of each side>] [-v <velocity>]\n\n"
           "Valid shapes are: line, triangle, square, pentagon, hexagon\n"
           "Side length is in cm. Valid range is [10,200]. Default value is 65.\n"
           "Velocity is in cm/sec. Valid range is [10,50]. Default value is 50.\n"
           "The -r option reverses the turn direction (clockwise by default)\n"
           "The -s option enables smooth acceleration and deceleration\n\n"
           "e.g. dead_reckoning --hexagon\n"
           "e.g. dead_reckoning --hexagon -l 100\n"
           "e.g. dead_reckoning --hexagon -l 100 -v 40\n"
           "e.g. dead_reckoning -n 6 -a 60 -l 100 -v 40\n\n");
    exit(1);
  }

  // Initialize player
  i = 0;
  playerc_client_t *client;
  playerc_position2d_t *position2d;

  // Create a client and connect it to the server.
  client = playerc_client_create(NULL, "localhost", 6665);
  if (0 != playerc_client_connect(client))
    return -1;

  // Create and subscribe to a position2d device.
  position2d = playerc_position2d_create(client, 0);
  if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
    return -1;

  // Read initial position
  playerc_client_read(client);
  double start_x = position2d->px;
  double start_y = position2d->py;
  double start_a = position2d->pa;

  // Convert parameters to motor control arguments
  double max_vel = 0.01 * v;
  int move_time = l * 28000;
  move_time *= 50.0 / (double)v;
  double max_turn_speed = 1.57;     // 90 degrees per second
  int turn_time = (int)(((double)a/90.0) * 560000);
  int d = -1;
  if (reverse_direction) d = 1;

  // Move the robot (e.g. square)
  double accel_rate = 0.01;         // 2 meters per sec^2
  double turn_accel_rate = 0.0314;  // 360 degrees per sec^2
  int cycle_time = 5000;            // 5 miliseconds
  for (i = 0; i < n; ++i) {
    // FORWARD
    int acceleration_time = 0;
    double vel = 0;
    int t = 0;
    if (smooth_acceleration) {
      // accelerate (up to max speed in 0.25 seconds)
      acceleration_time = (max_vel / accel_rate) * cycle_time;
      for (; vel < max_vel && t < move_time / 2;
          t += cycle_time, vel += accel_rate, usleep(cycle_time))
        if (playerc_position2d_set_cmd_vel(position2d, vel, 0, 0, 1)) return -1;
    }
    //max forward speed
    if (playerc_position2d_set_cmd_vel(position2d, max_vel, 0, 0, 1)) return -1;
    usleep(move_time - acceleration_time);  // not 2x acceleration time in order to make
                                            // up for ground not covered durring acceleration
                                            // (at an average of half of full speed)
    if (smooth_acceleration) {
      // decelerate
      for (; vel >= 0; vel -= accel_rate, usleep(cycle_time))
        if (playerc_position2d_set_cmd_vel(position2d, vel, 0, 0, 1)) return -1;
    }    
    
    // TURN
    double turn_speed = 0;
    if (smooth_acceleration) {
      // accelerate (up to max speed in 0.25 seconds)
      acceleration_time = (max_turn_speed / turn_accel_rate) * cycle_time;
      for (t = 0; turn_speed < max_turn_speed && t < move_time / 2;
          t += cycle_time, turn_speed += turn_accel_rate, usleep(cycle_time))
        if (playerc_position2d_set_cmd_vel(position2d, 0, 0, d * turn_speed, 1)) return -1;
    }
    // max turn speed
    if (playerc_position2d_set_cmd_vel(position2d, 0, 0, d * max_turn_speed, 1)) return -1;
    usleep(turn_time - acceleration_time);
    if (smooth_acceleration) {
      // decelerate
      for (; turn_speed >= 0; turn_speed -= turn_accel_rate, usleep(cycle_time))
        if (playerc_position2d_set_cmd_vel(position2d, 0, 0, d * turn_speed, 1)) return -1;
    }
  }
  // Stop
  if (playerc_position2d_set_cmd_vel(position2d, 0, 0, 0, 1)) return -1;
  
  // Read final position, get error
  playerc_client_read(client);
  double error_x = position2d->px - start_x;
  double error_y = position2d->py - start_y;
  double error_a = position2d->pa - start_a;

  // Report odometry error
  printf("\n\nMovement error according to onboard odometry:\n"
         "\tX pos: %f cm\n\tY pos: %f cm\n\tAngle: %f degrees\n",
         error_x * 100, error_y * 100, error_a * -57.325);

  // Shutdown
  playerc_position2d_unsubscribe(position2d);
  playerc_position2d_destroy(position2d);
  playerc_client_disconnect(client);
  playerc_client_destroy(client);

  return 0;
}
示例#13
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;
}
示例#14
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;
}
示例#15
0
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;
}
示例#16
0
int main(int argc, const char **argv)
{
	double  actual_result_trans;
	double  actual_result_angle;
	int finished;
	playerc_client_t *client;
	playerc_position2d_t *position2d;
	playerc_bumper_t * bumper;
	
	// Create a client object and connect to the server
	client = playerc_client_create(NULL, SERVER, PORT);
	if (playerc_client_connect(client) != 0)
	{
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}
	printf("Connected...");
	
	// Create a position2d proxy (device id "position2d:0") and susbscribe
	// in read/write mode
	position2d = playerc_position2d_create(client, 0);
	if (playerc_position2d_subscribe(position2d, PLAYERC_OPEN_MODE))
	{
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}
	printf("Position2D Subscribed...");
	
	//Creates a Bumper Device Proxy
	bumper = playerc_bumper_create(client, 0);
	if(playerc_bumper_subscribe(bumper, PLAYERC_OPEN_MODE)) {
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}
	printf("Bumper Subscribed...");
	
	// Enable the robots motors
	playerc_position2d_enable(position2d, 1);
	printf("Motor Enabled\n");
	

	
#ifdef ABSOLUTE_COORD
	//calls our move function to move to second point
	actual_result_trans = Move(client,position2d,bumper,3.2,0.0);
	printf("Results Returned from Move: %f\n",actual_result_trans);
	printPos(client,position2d,bumper);
	
	//rotates robot into position for third point
	actual_result_angle = Turn(client,position2d,bumper,(PI/2.0));
	printf("Results Returned from TurnL %f\n",actual_result_angle);
	printPos(client,position2d,bumper);
	
	//moves to third point from second point
	actual_result_trans = Move(client,position2d,bumper,3.2,3.04);
	printf("Results Returned from Move: %f\n",actual_result_trans);
	printPos(client,position2d,bumper);
	
	//rotates robot into position for the fourth point
	actual_result_angle = Turn(client,position2d,bumper,2.75741633);
	printf("Results Returned from TurnL %f\n",actual_result_angle);
	printPos(client,position2d,bumper);
	
	//moves to fouth point from third point
	actual_result_trans = Move(client,position2d,bumper,-0.5,4.7);
	printf("Results Returned from Move: %f\n",actual_result_trans);
	printPos(client,position2d,bumper);
	
	//rotates robot into position for point five
	actual_result_angle = Turn(client,position2d,bumper,(PI/2.0));
	printf("Results Returned from TurnL %f\n",actual_result_angle);
	printPos(client,position2d,bumper);
	
	//moves robot from position four to position five
	actual_result_trans = Move(client,position2d,bumper,-0.55,11.6);
	printf("Results Returned from Move: %f\n",actual_result_trans);
	printPos(client,position2d,bumper);
#else
	//calls our move function to move to second point
	actual_result_trans = Move(client,position2d,bumper,3.2,0.0);
	printf("Results Returned from Move: %f\n",actual_result_trans);
	printPos(client,position2d,bumper);
	
	//rotates robot into position for third point
	actual_result_angle = Turn(client,position2d,bumper,(PI/2.0));
	printf("Results Returned from TurnL %f\n",actual_result_angle);
	printPos(client,position2d,bumper);
	
	//moves to third point from second point
	actual_result_trans = Move(client,position2d,bumper,3.04,0.0);
	printf("Results Returned from Move: %f\n",actual_result_trans);
	printPos(client,position2d,bumper);
	
	//rotates robot into position for the fourth point
	actual_result_angle = Turn(client,position2d,bumper,1.18662);
	printf("Results Returned from TurnL %f\n",actual_result_angle);
	printPos(client,position2d,bumper);
	
	//moves to fouth point from third point
	actual_result_trans = Move(client,position2d,bumper,4.02,0.0);
	printf("Results Returned from Move: %f\n",actual_result_trans);
	printPos(client,position2d,bumper);
	
	//rotates robot into position for point five
	actual_result_angle = Turn(client,position2d,bumper,-1.18662);
	printf("Results Returned from TurnL %f\n",actual_result_angle);
	printPos(client,position2d,bumper);
	
	//moves robot from position four to position five
	actual_result_trans = Move(client,position2d,bumper,6.83,0.0);
	printf("Results Returned from Move: %f\n",actual_result_trans);
	printPos(client,position2d,bumper);
#endif
	
	// Shutdown and Unsubscribe Devices
	playerc_position2d_unsubscribe(position2d);
	playerc_position2d_destroy(position2d);
	playerc_bumper_unsubscribe(bumper);
	playerc_bumper_destroy(bumper);
	playerc_client_disconnect(client);
	playerc_client_destroy(client);
	
	return 0;
}
示例#17
0
// Disconnect from the server, with potential retry
int playerc_client_disconnect_retry(playerc_client_t *client)
{
  int retval;
  int i;
  int j;
  struct timespec sleeptime;

  sleeptime.tv_sec = (long) client->retry_time;
  sleeptime.tv_nsec = 0;

  /* Disconnect */
  if((retval = playerc_client_disconnect(client)) != 0)
    PLAYERC_WARN("playerc_client_disconnect() failed");

  for(j=0; (client->retry_limit < 0) || (j<client->retry_limit); j++)
  {
    PLAYERC_WARN1("Reconnecting, attempt %d", j);
    /* Reconnect */
    if((retval = playerc_client_connect(client)) != 0)
      PLAYERC_WARN("playerc_client_connect() failed");
    else
    {
      /* Clean out buffers */
      client->read_xdrdata_len = 0;

      /* TODO: re-establish replacement rules, delivery modes, etc. */

      /* Re-subscribe to devices */
      for(i=0;i<client->device_count;i++)
      {
        if(client->device[i]->subscribed)
        {
          // TODO: what should access be here?
          if((retval = playerc_device_subscribe(client->device[i],
                                                PLAYERC_OPEN_MODE)) != 0)
          {
            PLAYERC_WARN2("playerc_device_subscribe() failed for %d:%d",
                         client->device[i]->addr.interf,
                         client->device[i]->addr.index);

            // TODO: Subscription failed for one device; should we give up?
            if(playerc_client_disconnect(client) != 0)
              PLAYERC_WARN("playerc_client_disconnect() failed");
            break;
          }
        }
      }
      // Did we get all of them?
      if(i == client->device_count)
        break;
    }

    nanosleep(&sleeptime,NULL);
  }

  if((client->retry_limit < 0) || (j < client->retry_limit))
  {
    PLAYERC_WARN("successfully reconnected");
    return(0);
  }
  else
  {
    PLAYERC_WARN("failed to reconnect");
    return(-1);
  }
}
示例#18
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;
}
示例#19
0
int main(int argc, const char **argv)
{
	int r, i, j;
	playerc_client_t *client;
	playerc_position2d_t *position2d;


	//sonar
	playerc_sonar_t *sonar;

	celda celdas[25];
	int actual=0;
	int forward=1;
	int no_solucion=0;
	int flag_celda_final=0;

	// Create a client and connect it to the server.
	client = playerc_client_create(NULL, "localhost", 6665);
	if (playerc_client_connect(client) != 0) {
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}

	// Create and subscribe to a position2d device.
	position2d = playerc_position2d_create(client, 0);
	if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE) != 0) {
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}
	// Fixing initial position
	playerc_position2d_set_odom(position2d, 0.0, 0.0, 0.0);

	// Create and subscribe to a sonar device
	sonar = playerc_sonar_create(client, 0);
	if (playerc_sonar_subscribe(sonar, PLAYER_OPEN_MODE) != 0) {
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}
/* Obtener la geometría de los sensores de ultrasonidos sobre el pioneer 2 */
	if (playerc_sonar_get_geom(sonar) != 0) {
		fprintf(stderr, "error: %s\n", playerc_error_str());
		return -1;
	}

	// Enable motors 
	playerc_position2d_enable(position2d, 1);

	playerc_client_read(client);

	inspeccionar_celda(sonar,&celdas[actual]);

	//Rutina para asegurarse de que ni nos dejamos un camino atrás,
	//ni estamos ya en la salida:"Ponemos el culo contra la pared"
	
	if(celda_final(&celdas[actual])) {//Si no veo ninguna pared
		girar_dch(client,position2d); // giro para comprobar la espalda
		inspeccionar_celda(sonar,&celdas[actual]);
		if(celdas[actual].pared[D_DCH]) { // había pared detrás
	    		girar_izq(client,position2d); 
			//establecer orientación de referencia aquí
			theta = D_ARRIBA;
			inspeccionar_celda(sonar,&celdas[actual]);
		} else {
			printf("Ya estamos en la salida\n");
			flag_celda_final=1;
		}
	} else {
		// me pongo de espaldas a la pared
		if(celdas[actual].pared[D_DCH]) { //si tengo pared a la dcha
			girar_izq(client,position2d); 
		} else if(celdas[actual].pared[D_IZQ]) { //si tengo pared a la izda
			girar_dch(client,position2d);
		} else if(celdas[actual].pared[D_ARRIBA]) { //si la tengo en fente
			girar_180(client,position2d);
		}
		//establecer orientación de referencia aquí
		theta = D_ARRIBA;
		inspeccionar_celda(sonar,&celdas[actual]);
	}


	while(!(flag_celda_final || no_solucion)) {
		printf("celda: %d [%d,%d]\n",actual, x, y);
		r = mejor_ruta(celdas, actual, &forward);
		if (forward) {
			ir_direccion(client, position2d, r);
			actual++;
			inspeccionar_celda(sonar,&celdas[actual]);
		} else if (actual > 0) {
			ir_direccion(client, position2d, r);
			actual--;
		} else {
			no_solucion=1;
			printf("no hay solucion\n");
		}
		
		if ((flag_celda_final = celda_final(&celdas[actual])) != 0) {
			printf("Fuera del laberinto!!\n\n");
			printf("Camino:\n");
			printf("-------\n");
			for (i=0; i <= actual; i++) {
				for (j = 0; (j < 3) && (celdas[i].pared[j] != RUTA); j++) ;
				printf("(%d,%d) - %s\n", celdas[i].pos[0], celdas[i].pos[1], dirs[j]);
			}
		}
	}


	// Unsuscribe and Destroy
	// position2d
	playerc_position2d_unsubscribe(position2d);
	playerc_position2d_destroy(position2d);
	// sonar
	playerc_sonar_unsubscribe(sonar);
	playerc_sonar_destroy(sonar);



	// client
	playerc_client_disconnect(client);
	playerc_client_destroy(client);

	// End
	return 0;
}
示例#20
0
int main(int argc, char **argv)
{
  int i;
  playerc_client_t *client;
  playerc_map_t *map;
  FILE* fp;

  host = "localhost";
  port = 6665;
  omap = 1;
  idx = 0;

  if(parse_args(argc,argv) < 0)
  {
    puts(USAGE);
    exit(-1);
  }

  // Create a client and connect it to the server.
  client = playerc_client_create(NULL, host, port);
  if (playerc_client_connect(client) != 0)
    return -1;

  // Create and subscribe to a map device.
  map = playerc_map_create(client, idx);
  if (playerc_map_subscribe(map, PLAYER_OPEN_MODE))
    return -1;

  if(omap)
  {
    // Get the occ grid map
    if (playerc_map_get_map(map) != 0)
      return -1;

    printf("map: %d X %d @ %.3f origin: %f %f \n", 
           map->width, map->height, 
           map->resolution,
           map->origin[0], map->origin[1]);

    create_map_image(map, fname, "png");
  }
  else
  {
    // Get the vector map
    if (playerc_map_get_vector(map) != 0)
      return -1;

    if(!(fp = fopen(fname,"w+")))
    {
      perror("fopen() failed");
      exit(-1);
    }

    fprintf(fp,"# Created by benson's SfLineScan.\n");
    fprintf(fp,"origin %.0f %.0f\n", map->vminx * 1e3, map->vminy * 1e3);
    fprintf(fp,"width %.0f\n", (map->vmaxx - map->vminx) * 1e3);
    fprintf(fp,"height %.0f\n", (map->vmaxy - map->vminy) * 1e3);
    for (i = 0; i < map->num_segments; i++)
    {
      fprintf(fp,"%.0f %.0f %.0f %.0f\n",
             1e3*map->segments[i].x0,
             1e3*map->segments[i].y0,
             1e3*map->segments[i].x1,
             1e3*map->segments[i].y1);
    }

    fclose(fp);
  }

  // Shutdown
  playerc_map_unsubscribe(map);
  playerc_map_destroy(map);
  playerc_client_disconnect(client);
  playerc_client_destroy(client);

  return 0;
}
示例#21
0
int
main(int argc, const char **argv)
{
  int i;
  playerc_client_t *client;
  player_pose2d_t position2d_vel;
  player_pose2d_t position2d_target;
  playerc_position2d_t *position2d;

  // Create a client and connect it to the server.
  client = playerc_client_create(NULL, "localhost", 6665);
  if (0 != playerc_client_connect(client))
    return -1;

  // Create and subscribe to a position2d device.
  position2d = playerc_position2d_create(client, 0);
  if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
    return -1;

// Make the robot spin!
 if (0 != playerc_position2d_set_cmd_vel(position2d, 0.25, 0, DTOR(40.0), 1))
    return -1;

  for (i = 0; i < 50; i++)
  {
		playerc_client_read(client);

		// Print current robot pose
		printf("position2d : %f %f %f\n",
		position2d->px, position2d->py, position2d->pa);
  }
/* con el comando playerc_position2d_set_cmd_vel() simplemente se establece una
 * consigna de velocidad la cual es independiente de la posición en la que se
 * encuentra el robot */




      position2d_target.px = 2;
      position2d_target.py = -3;
      position2d_target.pa = 0;
      
      // Move to pose  
      playerc_position2d_set_cmd_pose(position2d, position2d_target.px , position2d_target.py, position2d_target.pa , 1);

	printf("position2d : %f %f %f\n",
	position2d->px, position2d->py, position2d->pa);
      // Stop when reach the target
      while  (sqrt(pow(position2d->px - position2d_target.px,2.0) + pow(position2d->py - position2d_target.py,2.0)) > 0.05 )
	{
		playerc_client_read(client);

		// Print current robot pose
		printf("position2d : %f %f %f\n",
		position2d->px, position2d->py, position2d->pa);
	}
/* Con el comando position2d_set_cmd_pose() se establece una pose de destino.
 * Cuando se utiliza este método la información obtenida de la odometría sí que
 * afecta a la trayectoria seguida y al punto final */


      position2d_target.px = 0;
      position2d_target.py = -3;
      position2d_target.pa = 0;
      position2d_vel.px = 0.6;
      position2d_vel.py = 0;
      position2d_vel.pa = 0;

      // Move to pose  
      playerc_position2d_set_cmd_pose_with_vel(position2d, position2d_target, position2d_vel, 1);

      // Stop when reach the target
      while  (sqrt(pow(position2d->px - position2d_target.px,2.0) + pow(position2d->py - position2d_target.py,2.0)) > 0.05 )
	{
		playerc_client_read(client);

		// Print current robot pose
		printf("position2d : %f %f %f\n",
		position2d->px, position2d->py, position2d->pa);
	}
/* Con el comando playerc_position2d_set_cmd_pose_with_vel se hace lo mismo que
 * con playerc_position2d_set_cmd_pose() pero además se puede indicar una
 * consigna de velocidad */

  // Shutdown
  playerc_position2d_unsubscribe(position2d);
  playerc_position2d_destroy(position2d);
  playerc_client_disconnect(client);
  playerc_client_destroy(client);

  return 0;
}
示例#22
0
/*double
randomInt(int low, int high)
{
return low + (high - low) * (rand()/(RAND_MAX * 1.0 ));
}
// Main function for the program*/
int
main(int argc, const char **argv)
{
playerc_client_t *client;
playerc_position2d_t *position2d;
int cycle, index=0;
double dist,angle,fidAngle = 0,lineAngle=0, fidDist=0, prevYaw=0,posAngle=0;
// Create a client and connect it to the server.
client = playerc_client_create(NULL, "localhost", 6665);
if (0 != playerc_client_connect(client))
return -1;
// Create and subscribe to a position2d device.
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
return -1;
// Initiating random walk movement
/*if (0 != playerc_position2d_set_cmd_vel(position2d, randomInt(0.1,1)
,randomInt(0.1,1),DTOR(randomInt(-20,20)) ,1))
return -1;
fprintf(stdout, "robot random positions \n");*/
//looping in random positions
int i;
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 3
,0 , 0,1))
return -1;
usleep(1000);
}
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 0
,0 , 1,1))
return -1;
usleep(1000);
}
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 3
,0 , 0,1))
return -1;
usleep(1000);
}
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 0
,0 , 1,1))
return -1;
usleep(1000);
}
for (i = 0; i<=10; i++)
{
// Wait for new data from server
playerc_client_read(client);
fprintf(stdout, "X: %3.2f, Y: %3.2f, Yaw: %3.2f \n",
position2d->px, position2d->py, position2d->pa);
// Random walk is continued till finding first marker
if (0 != playerc_position2d_set_cmd_vel(position2d, 3
,0 , 0,1))
return -1;
usleep(1000);
}


while(1)
{if (0 != playerc_position2d_set_cmd_vel(position2d, 0,0 , 0,1))
return -1;
}

playerc_position2d_unsubscribe(position2d);
playerc_position2d_destroy(position2d);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
示例#23
0
int main(int argc, const char **argv)
{
	int i;
	playerc_client_t *client;
	nsdnet_t *device;

	// Takes a parameter for the index.
	int index = 0;
	if (argc < 2)
	{
		printf("Needs one parameter.\n");
		return 0;
	}
	else
	{
		index = atoi(argv[1]);
		printf("Using index %d\n", index);
	}

	// Create a client and connect it to the server.
	client = playerc_client_create(NULL, "localhost", 6665);
	if (0 != playerc_client_connect(client))
	{
		printf("Could not connect\n");
		return -1;
	}

	// Load the plugin interface
	if (playerc_add_xdr_ftable (player_plugininterf_gettable (), 0) < 0)
		printf("Could not add xdr functions\n");

	// Create and subscribe to a device using the interface.
	device = nsdnet_create(client, index);
	if (nsdnet_subscribe(device, PLAYER_OPEN_MODE) != 0)
	{
		printf("Could not subscribe\n");
		return -1;
	}

	// Get our clientid
	if (nsdnet_property_get(device, "self.id"))
	{
		printf("Failed to get registered device id...\n");
		return -1;
	}

	// Print out what we get...
	printf("Client id: %s\n", device->propval);

	// Get the list of clients
	if (nsdnet_get_listclients(device))
	{
		printf("Failed to get list of clients...\n");
		return -1;
	}

	// Print out what we get...
	printf("Clients: ");
	for (i = 0; i < device->listclients_count; i++)
	{
		printf("%s ", device->listclients[i]);
	}
	printf("\n");

	// Endless loop...
	for (i = 0; i < 1000; i++)
	{
		if (playerc_client_peek(client, 0) == 1)
		{
			playerc_client_read(client);
			while (device->queue_head != device->queue_tail)
			{
				nsdmsg_t *msg; int err;
				if ((err = nsdnet_receive_message(device, &msg)))
				{
					if (err < 0)
						printf("ERROR\n");
					else
						printf("%s: %s [%d]\n", msg->clientid, msg->msg, i);
				}
			}
			usleep(0);
		}
		else
		{
			printf("Sending Hello World\n");
			if (nsdnet_send_message(device, NULL, 12, "Hello World"))
			{
				printf("Could not broadcast 'Hello World'\n");
				return -1;
			}
			usleep(100000);
		}
	}

	// Shutdown
	nsdnet_unsubscribe(device);
	nsdnet_destroy(device);
	playerc_client_disconnect(client);
	playerc_client_destroy(client);

	return 0;
}