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
}
/* 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;
}
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;
}
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;
}
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;
}
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;
}
}
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);
}
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;
}
// 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)
{
//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
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;
}
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;
}
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;
}
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;
}
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;
}
// 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);
}
}
// 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;
}
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;
}
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;
}
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;
}
/*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;
}
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;
}
