void
browse_cb(player_sd_t* sd, player_sd_dev_t* dev)
{
if(dev->interf == PLAYER_LASER_CODE)
{
clients[num_laserdevs] = playerc_client_create(mclient,
dev->hostname,
dev->robot);
if(0 != playerc_client_connect(clients[num_laserdevs]))
exit(-1);
playerc_client_datamode(clients[num_laserdevs], PLAYERC_DATAMODE_PUSH);
// Create and subscribe to a laser device.
lasers[num_laserdevs] = playerc_laser_create(clients[num_laserdevs],
dev->index);
if(playerc_laser_subscribe(lasers[num_laserdevs], PLAYER_OPEN_MODE))
exit(-1);
// Add a callback to be invoked whenever we receive new data from this
// laser
playerc_client_addcallback(clients[num_laserdevs],
&(lasers[num_laserdevs]->info),
device_cb, lasers[num_laserdevs]);
num_laserdevs++;
printf("subscribed to: %s:%u:%s:%u\n",
dev->hostname,
dev->robot,
interf_to_str(dev->interf),
dev->index);
printf("Now receiving %d lasers\n", num_laserdevs);
}
}
// Create a laser device
laser_t *laser_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
int index, const char *drivername, int subscribe)
{
char label[64];
char section[64];
laser_t *laser;
laser = malloc(sizeof(laser_t));
laser->proxy = playerc_laser_create(client, index);
laser->drivername = strdup(drivername);
laser->datatime = 0;
snprintf(section, sizeof(section), "laser:%d", index);
// Construct the menu
snprintf(label, sizeof(label), "laser:%d (%s)", index, laser->drivername);
laser->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
laser->subscribe_item = rtk_menuitem_create(laser->menu, "Subscribe", 1);
laser->style_item = rtk_menuitem_create(laser->menu, "Filled", 1);
#if 0
laser->res025_item = rtk_menuitem_create(laser->menu, "0.25 deg resolution", 1);
laser->res050_item = rtk_menuitem_create(laser->menu, "0.50 deg resolution", 1);
laser->res100_item = rtk_menuitem_create(laser->menu, "1.00 deg resolution", 1);
#endif
laser->range_mm_item = rtk_menuitem_create(laser->menu, "mm Range Resolution",1);
laser->range_cm_item = rtk_menuitem_create(laser->menu, "cm Range Resolution",1);
laser->range_dm_item = rtk_menuitem_create(laser->menu, "dm Range Resolution",1);
// Set the initial menu state
rtk_menuitem_check(laser->subscribe_item, subscribe);
rtk_menuitem_check(laser->style_item, 1);
// Construct figures
laser->scan_fig = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 0);
return laser;
}
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, 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)
{
//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;
}