// Update a laser device
void laser_update(laser_t *laser)
{
// Update the device subscription
if (rtk_menuitem_ischecked(laser->subscribe_item))
{
if (!laser->proxy->info.subscribed)
{
if (playerc_laser_subscribe(laser->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
// Get the laser geometry
if (playerc_laser_get_geom(laser->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
rtk_fig_origin(laser->scan_fig,
laser->proxy->pose[0],
laser->proxy->pose[1],
laser->proxy->pose[2]);
}
}
else
{
if (laser->proxy->info.subscribed)
if (playerc_laser_unsubscribe(laser->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
rtk_menuitem_check(laser->subscribe_item, laser->proxy->info.subscribed);
// Making config changes here causes X to go nuts. Don't know why - BPG
// Update the configuration stuff
if (laser->proxy->info.subscribed)
laser_update_config(laser);
if (laser->proxy->info.subscribed)
{
// Draw in the laser scan if it has been changed.
if (laser->proxy->info.datatime != laser->datatime)
{
laser_draw(laser);
laser->datatime = laser->proxy->info.datatime;
}
}
else
{
// Dont draw the laser.
rtk_fig_show(laser->scan_fig, 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);
}
// Destroy a laser device
void laser_destroy(laser_t *laser)
{
if (laser->proxy->info.subscribed)
playerc_laser_unsubscribe(laser->proxy);
playerc_laser_destroy(laser->proxy);
rtk_fig_destroy(laser->scan_fig);
#if 0
rtk_menuitem_destroy(laser->res025_item);
rtk_menuitem_destroy(laser->res050_item);
rtk_menuitem_destroy(laser->res100_item);
#endif
rtk_menuitem_destroy(laser->range_mm_item);
rtk_menuitem_destroy(laser->range_cm_item);
rtk_menuitem_destroy(laser->range_dm_item);
rtk_menuitem_destroy(laser->subscribe_item);
rtk_menuitem_destroy(laser->style_item);
rtk_menu_destroy(laser->menu);
free(laser->drivername);
free(laser);
}
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;
}
