// Update a dio device
void dio_update(dio_t *dio)
{
// Update the device subscription
if (rtk_menuitem_ischecked(dio->subscribe_item))
{
if (!dio->proxy->info.subscribed)
if (playerc_dio_subscribe(dio->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
else
{
if (dio->proxy->info.subscribed)
if (playerc_dio_unsubscribe(dio->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
rtk_menuitem_check(dio->subscribe_item, dio->proxy->info.subscribed);
if (dio->proxy->info.subscribed)
{
// Draw in the dio scan if it has been changed.
if (dio->proxy->info.datatime != dio->datatime)
{
dio_draw(dio);
dio->datatime = dio->proxy->info.datatime;
}
}
else
{
// Dont draw the dio.
rtk_fig_show(dio->fig, 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;
}
// Update a actarray device
void actarray_update(actarray_t *actarray)
{
int ii;
// Update the device subscription
if (rtk_menuitem_ischecked(actarray->subscribe_item))
{
if (!actarray->proxy->info.subscribed)
{
if (playerc_actarray_subscribe(actarray->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
playerc_actarray_get_geom(actarray->proxy);
}
}
else
{
if (actarray->proxy->info.subscribed)
if (playerc_actarray_unsubscribe(actarray->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
rtk_menuitem_check(actarray->subscribe_item, actarray->proxy->info.subscribed);
// Draw in the actarray scan if it has been changed.
if (actarray->proxy->info.subscribed)
{
if (actarray->proxy->info.datatime != actarray->datatime)
actarray_draw(actarray);
actarray->datatime = actarray->proxy->info.datatime;
}
else
{
for (ii = 0; ii < actarray->fig_count; ++ii)
{
rtk_fig_show(actarray->actuator_fig[ii], 0);
}
}
// Move the actarray
if (actarray->proxy->info.subscribed && rtk_menuitem_ischecked(actarray->command_item))
{
for (ii = 0; ii < actarray->fig_count; ++ii)
{
rtk_fig_show(actarray->actuator_fig_cmd[ii], 1);
}
actarray_move(actarray);
}
else
{
for (ii = 0; ii < actarray->fig_count; ++ii)
{
rtk_fig_show(actarray->actuator_fig_cmd[ii], 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;
}
// Update a map device
void map_update(map_t *map)
{
// Update the device subscription
if (rtk_menuitem_ischecked(map->subscribe_item))
{
if (!map->proxy->info.subscribed)
{
if (playerc_map_subscribe(map->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
// download a map
if (playerc_map_get_map( map->proxy ) >= 0)
map_draw( map );
}
}
else
{
if (map->proxy->info.subscribed)
if (playerc_map_unsubscribe(map->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
rtk_menuitem_check(map->subscribe_item, map->proxy->info.subscribed);
if (map->proxy->info.subscribed)
{
if(rtk_menuitem_ischecked(map->continuous_item))
{
static struct timeval old_time = {0,0};
struct timeval time;
double time_diff = 0.0;
gettimeofday(&time, NULL);
// i don't use (map->proxy->info.datatime != map->datatime))
// because some drivers return strange datatimes
// and the map may change to often for the current map format
// in playerv.h you can adjust MAP_UPDATE_TIME (default 1 sec)
time_diff = (double)(time.tv_sec - old_time.tv_sec) +
(double)(time.tv_usec - old_time.tv_usec)/1000000;
if (time_diff > MAP_UPDATE_TIME)
{
playerc_map_get_map(map->proxy);
map_draw(map);
old_time = time;
}
}
}
else
{
// Dont draw the map.
rtk_fig_show(map->fig, 0);
}
}
// 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);
}
}
// Set the robot pose (mean and covariance)
int playerc_localize_set_pose(playerc_localize_t *device, double pose[3], double cov[6])
{
player_localize_set_pose_t req;
req.mean.px = pose[0];
req.mean.py = pose[1];
req.mean.pa = pose[2];
req.cov[0] = cov[0];
req.cov[1] = cov[1];
req.cov[2] = cov[2];
req.cov[3] = cov[3];
req.cov[4] = cov[4];
req.cov[5] = cov[5];
if(playerc_client_request(device->info.client,
&device->info,
PLAYER_LOCALIZE_REQ_SET_POSE,
&req, NULL) < 0)
{
PLAYERC_WARN1("%s\n", playerc_error_str());
return -1;
}
return 0;
}
// Move the actarray
void actarray_move(actarray_t *actarray)
{
double ox, oy, oa, min, max;
double value;
int ii;
for(ii = 0; ii < actarray->fig_count; ++ii)
{
rtk_fig_get_origin(actarray->actuator_fig_cmd[ii], &ox, &oy, &oa);
value = oy;
min = -1;
max = 1;
if (actarray->proxy->actuators_geom && actarray->proxy->actuators_geom_count == actarray->proxy->actuators_count)
{
min = actarray->proxy->actuators_geom[ii].min;
max = actarray->proxy->actuators_geom[ii].max;
}
// now limit and scale the value to the actuator bar
value = ((oy+1)/2)*(max-min)+min;
if (value > max) value = max;
if (value < min) value = min;
if (actarray->lastvalue[ii] != value)
{
if (playerc_actarray_position_cmd(actarray->proxy, ii, value) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
actarray->lastvalue[ii] = value;
}
}
}
// Update an ir device
void ir_update(ir_t *ir)
{
int i;
// Update the device subscription
if (rtk_menuitem_ischecked(ir->subscribe_item))
{
if (!ir->proxy->info.subscribed)
{
if (playerc_ir_subscribe(ir->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("subscribe failed : %s", playerc_error_str());
// Get the ir geometry
if (playerc_ir_get_geom(ir->proxy) != 0)
PRINT_ERR1("get_geom failed : %s", playerc_error_str());
ir_allocate_figures(ir, ir->proxy->poses.poses_count);
for (i = 0; i < ir->proxy->poses.poses_count; i++)
{
rtk_fig_origin(ir->scan_fig[i],
ir->proxy->poses.poses[i].px,
ir->proxy->poses.poses[i].py,
ir->proxy->poses.poses[i].pyaw);
}
}
}
else
{
if (ir->proxy->info.subscribed)
if (playerc_ir_unsubscribe(ir->proxy) != 0)
PRINT_ERR1("unsubscribe failed : %s", playerc_error_str());
}
rtk_menuitem_check(ir->subscribe_item, ir->proxy->info.subscribed);
if (ir->proxy->info.subscribed)
{
// Draw in the ir scan if it has been changed.
if (ir->proxy->info.datatime != ir->datatime)
ir_draw(ir);
ir->datatime = ir->proxy->info.datatime;
}
else
{
// Dont draw the ir.
ir_nodraw(ir);
}
}
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;
}
// Update a ptz device
void ptz_update(ptz_t *ptz)
{
// Update the device subscription
if (rtk_menuitem_ischecked(ptz->subscribe_item))
{
if (!ptz->proxy->info.subscribed)
if (playerc_ptz_subscribe(ptz->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
else
{
if (ptz->proxy->info.subscribed)
if (playerc_ptz_unsubscribe(ptz->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
rtk_menuitem_check(ptz->subscribe_item, ptz->proxy->info.subscribed);
// Draw in the ptz scan if it has been changed.
if (ptz->proxy->info.subscribed)
{
if (ptz->proxy->info.datatime != ptz->datatime)
ptz_draw(ptz);
ptz->datatime = ptz->proxy->info.datatime;
}
else
{
rtk_fig_show(ptz->data_fig, 0);
rtk_fig_show(ptz->data_fig_tilt, 0);
}
// Move the ptz
if (ptz->proxy->info.subscribed && rtk_menuitem_ischecked(ptz->command_item))
{
rtk_fig_show(ptz->cmd_fig, 1);
rtk_fig_show(ptz->cmd_fig_tilt, 1);
ptz_move(ptz);
}
else
{
rtk_fig_show(ptz->cmd_fig, 0);
rtk_fig_show(ptz->cmd_fig_tilt, 0);
}
}
// Move the ptz
void ptz_move(ptz_t *ptz)
{
double ox, oy, oa, oxt, oyt, oat;
double pan, tilt, zoom, speed;
rtk_fig_get_origin(ptz->cmd_fig, &ox, &oy, &oa);
rtk_fig_get_origin(ptz->cmd_fig_tilt, &oxt, &oyt, &oat);
pan = atan2(oy, ox);
tilt = atan2(oyt,oxt);
zoom = 2 * atan2(0.5, sqrt(ox * ox + oy * oy));
speed = sqrt(oy*oy + ox*ox);
if (playerc_ptz_set_ws(ptz->proxy, pan, tilt, zoom,speed,0) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
// Create a ptz device
ptz_t *ptz_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
int index, const char *drivername, int subscribe)
{
char section[64];
char label[64];
ptz_t *ptz;
ptz = malloc(sizeof(ptz_t));
ptz->datatime = 0;
ptz->drivername = strdup(drivername);
ptz->proxy = playerc_ptz_create(client, index);
// Set initial device state
snprintf(section, sizeof(section), "ptz:%d", index);
if (subscribe)
{
if (playerc_ptz_subscribe(ptz->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
// Construct the menu
snprintf(label, sizeof(label), "ptz:%d (%s)", index, ptz->drivername);
ptz->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
ptz->subscribe_item = rtk_menuitem_create(ptz->menu, "Subscribe", 1);
ptz->command_item = rtk_menuitem_create(ptz->menu, "Command", 1);
// Set the initial menu state
rtk_menuitem_check(ptz->subscribe_item, ptz->proxy->info.subscribed);
// Construct figures
ptz->data_fig = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 10);
ptz->cmd_fig = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 11);
ptz->data_fig_tilt = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 12);
ptz->cmd_fig_tilt = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 13);
rtk_fig_movemask(ptz->cmd_fig, RTK_MOVE_TRANS);
rtk_fig_origin(ptz->cmd_fig, 1, 0, 0);
rtk_fig_color_rgb32(ptz->cmd_fig, COLOR_PTZ_CMD);
rtk_fig_ellipse(ptz->cmd_fig, 0, 0, 0, 0.2, 0.2, 0);
rtk_fig_movemask(ptz->cmd_fig_tilt, RTK_MOVE_TRANS);
rtk_fig_origin(ptz->cmd_fig_tilt, 0.8, 0, 0);
rtk_fig_color_rgb32(ptz->cmd_fig_tilt, COLOR_PTZ_CMD_TILT);
rtk_fig_ellipse(ptz->cmd_fig_tilt, 0, 0, 0, 0.2, 0.2, 0);
return ptz;
}
// Create a actarray device
actarray_t *actarray_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
int index, const char *drivername, int subscribe)
{
char section[64];
char label[64];
actarray_t *actarray;
actarray = malloc(sizeof(actarray_t));
actarray->datatime = 0;
actarray->drivername = strdup(drivername);
actarray->proxy = playerc_actarray_create(client, index);
// Set initial device state
snprintf(section, sizeof(section), "actarray:%d", index);
if (subscribe)
{
if (playerc_actarray_subscribe(actarray->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
playerc_actarray_get_geom(actarray->proxy);
}
// Construct the menu
snprintf(label, sizeof(label), "actarray:%d (%s)", index, actarray->drivername);
actarray->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
actarray->subscribe_item = rtk_menuitem_create(actarray->menu, "Subscribe", 1);
actarray->command_item = rtk_menuitem_create(actarray->menu, "Command", 1);
// Set the initial menu state
rtk_menuitem_check(actarray->subscribe_item, actarray->proxy->info.subscribed);
// Construct figures
actarray->actuator_fig = NULL;
actarray->actuator_fig_cmd = NULL;
actarray->lastvalue = NULL;
actarray->mainwnd = mainwnd;
return actarray;
}
// 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, 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;
}
// Update the laser configuration
void laser_update_config(laser_t *laser)
{
int update;
double min, max, range_res, res, scanning_frequency;
unsigned char intensity;
min = laser->proxy->scan_start;
max = laser->proxy->scan_start + laser->proxy->scan_count*laser->proxy->scan_res;
range_res = laser->proxy->range_res;
res = laser->proxy->scan_res;
scanning_frequency = laser->proxy->scanning_frequency;
intensity = laser->proxy->intensity_on;
update = 0;
#if 0
if (rtk_menuitem_isactivated(laser->res025_item))
{
res = 25; min = -50*M_PI/180; max = +50*M_PI/180; update = 1;
}
if (rtk_menuitem_isactivated(laser->res050_item))
{
res = 50; min = -M_PI/2; max = +M_PI/2; update = 1;
}
if (rtk_menuitem_isactivated(laser->res100_item))
{
res = 100; min = -M_PI/2; max = +M_PI/2; update = 1;
}
#endif
if (rtk_menuitem_isactivated(laser->range_mm_item)) {
range_res = .001;
update = 1;
}
if (rtk_menuitem_isactivated(laser->range_cm_item)) {
range_res = .010;
update = 1;
}
if (rtk_menuitem_isactivated(laser->range_dm_item)) {
range_res = .100;
update = 1;
}
// Set the laser configuration.
if (update)
{
if (playerc_laser_set_config(laser->proxy, min, max, res, range_res,
intensity, scanning_frequency) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
#if 0
res = (int) (laser->proxy->scan_res * 180 / M_PI * 100);
rtk_menuitem_check(laser->res025_item, (res == 25));
rtk_menuitem_check(laser->res050_item, (res == 50));
rtk_menuitem_check(laser->res100_item, (res == 100));
#endif
range_res = laser->proxy->range_res;
rtk_menuitem_check(laser->range_mm_item, (range_res < .0011));
rtk_menuitem_check(laser->range_cm_item, (.009 < range_res && range_res < .011));
rtk_menuitem_check(laser->range_dm_item, (range_res > .090));
return;
}
int
player_init(int argc, char *argv[])
{
int csize, usize, i;
if(get_options(argc, argv) < 0)
{
print_usage();
exit(-1);
}
// Create a g_client object and connect to the server; the server must
// be running on "localhost" at port 6665
g_client = playerc_client_create(NULL, g_hostname, g_port);
playerc_client_set_transport(g_client, g_transport);
if (0 != playerc_client_connect(g_client))
{
fprintf(stderr, "error: %s\n", playerc_error_str());
exit(-1);
}
/* if (0 != playerc_client_datafreq(g_client, 20))
{
fprintf(stderr, "error: %s\n", playerc_error_str());
return -1;
}
*/
// Create a camera proxy (device id "camera:index") and susbscribe
g_camera = playerc_camera_create(g_client, g_camera_index);
if (0 != playerc_camera_subscribe(g_camera, PLAYER_OPEN_MODE))
{
fprintf(stderr, "camera error: %s\n", playerc_error_str());
fprintf(stderr, "playercam will attempt to continue without a camera\n");
playerc_camera_destroy(g_camera);
g_camera = NULL;
}
// Create a blobfinder proxy (device id "blobfinder:index") and susbscribe
g_blobfinder = playerc_blobfinder_create(g_client, g_blobfinder_index);
if (0 != playerc_blobfinder_subscribe(g_blobfinder, PLAYER_OPEN_MODE))
{
fprintf(stderr, "blobfinder error: %s\n", playerc_error_str());
fprintf(stderr, "playercam will attempt to continue without a blobfinder\n");
playerc_blobfinder_destroy(g_blobfinder);
g_blobfinder = NULL;
}
if ((NULL == g_camera) && (NULL == g_blobfinder))
{
fprintf(stderr, "we need either a camera or blobfinder! aborting\n");
exit(-1);
}
// Get up to 10 images until we have a valid frame (g_wdith > 0)
for (i=0, g_width=0; i < 10 && g_width==0 && NULL != playerc_client_read(g_client); ++i)
{
if (NULL != g_camera)
{
// Decompress the image
csize = g_camera->image_count;
playerc_camera_decompress(g_camera);
usize = g_camera->image_count;
g_print("camera: [w %d h %d d %d] [%d/%d bytes]\n",
g_camera->width, g_camera->height, g_camera->bpp, csize, usize);
g_width = g_camera->width;
g_height = g_camera->height;
if (allocated_size != usize)
{
g_img = realloc(g_img, usize);
allocated_size = usize;
}
}
else // try the blobfinder
{
g_print("blobfinder: [w %d h %d]\n",
g_blobfinder->width, g_blobfinder->height);
g_width = g_blobfinder->width;
g_height = g_blobfinder->height;
usize = g_width * g_height * 3;
if (allocated_size != usize)
{
g_img = realloc(g_img, usize);
allocated_size = usize;
}
// set the image data to 0 since we don't have a camera
memset(g_img, 128, usize);
}
}
g_window_width = g_width;
g_window_height = g_height;
assert(g_width>0);
assert(g_height>0);
playerc_client_datamode(g_client,PLAYER_DATAMODE_PULL);
playerc_client_set_replace_rule(g_client,-1,-1,PLAYER_MSGTYPE_DATA,-1,1);
return 0;
}
static gint playerout_open(AFormat fmt, gint rate, gint nch)
{
gint pos;
written = 0;
afmt = fmt;
sampleRate = rate;
numChannels = nch;
startTime = 0.0f;
pausedTime = 0.0f;
if (xmms_check_realtime_priority())
{
xmms_show_message("Error",
"You cannot use the Player Output plugin\n"
"when you're running in realtime mode.",
"Ok", FALSE, NULL, NULL);
return 0;
}
/* do player server connection here */
// Create a client object and connect to the server; the server must
// be running on "localhost" at port 6665
client = playerc_client_create(NULL, server_address, server_port);
if (playerc_client_connect(client) != 0)
{
fprintf(stderr, "error: %s\n", playerc_error_str());
client = NULL;
return 0;
}
// Create a audio proxy susbscribe
audio_proxy = playerc_audio_create(client, server_index);
if (playerc_audio_subscribe(audio_proxy, PLAYERC_OPEN_MODE) != 0)
{
fprintf(stderr, "error: %s\n", playerc_error_str());
return 0;
}
// Set to PULL data mode
if (playerc_client_datamode (client, PLAYERC_DATAMODE_PULL) != 0)
{
fprintf(stderr, "error: %s\n", playerc_error_str());
return 0;
}
if (playerc_client_set_replace_rule (client, -1, -1, PLAYER_MSGTYPE_DATA, -1, 1) != 0)
{
fprintf(stderr, "error: %s\n", playerc_error_str());
return 0;
}
bytesPerSecond = rate * nch;
if (fmt == FMT_S16_LE || fmt == FMT_S16_BE || fmt == FMT_S16_NE ||
fmt == FMT_U16_LE || fmt == FMT_U16_BE || fmt == FMT_U16_NE)
bytesPerSecond *= 2;
// Allocate a buffer
bufferLength = (int) (((float) bufferTime / 1000.0f) * bytesPerSecond);
if (buffer != NULL)
free (buffer);
buffer = malloc (bufferLength);
bufferPos = 0;
return 1;
}
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)
{
//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)
{
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;
}
// Update a ranger device
void ranger_update(ranger_t *ranger)
{
int ii;
// Update the device subscription
if (rtk_menuitem_ischecked(ranger->subscribe_item))
{
if (!ranger->proxy->info.subscribed)
{
if (playerc_ranger_subscribe(ranger->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
// Get the ranger geometry
if (playerc_ranger_get_geom(ranger->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
// Request the device config for min angle and resolution
if (playerc_ranger_get_config(ranger->proxy, NULL, NULL, NULL, NULL, NULL, NULL) != 0)
{
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
ranger->start_angle = 0.0f;
ranger->resolution = 0.0f;
}
else
{
ranger->start_angle = ranger->proxy->min_angle;
ranger->resolution = ranger->proxy->resolution;
}
// Delete any current figures
ranger_delete_figures(ranger);
// Create the figures
if ((ranger->scan_fig = malloc(ranger->proxy->sensor_count * sizeof(rtk_fig_t*))) == NULL )
{
PRINT_ERR1("Failed to allocate memory for %d figures to display ranger", ranger->proxy->sensor_count);
return;
}
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
ranger->scan_fig[ii] = rtk_fig_create(ranger->mainwnd->canvas, ranger->mainwnd->robot_fig, 1);
rtk_fig_origin(ranger->scan_fig[ii],
ranger->proxy->sensor_poses[ii].px,
ranger->proxy->sensor_poses[ii].py,
ranger->proxy->sensor_poses[ii].pyaw);
}
}
}
else
{
if (ranger->proxy->info.subscribed)
if (playerc_ranger_unsubscribe(ranger->proxy) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
ranger_delete_figures(ranger);
}
rtk_menuitem_check(ranger->subscribe_item, ranger->proxy->info.subscribed);
if (ranger->proxy->info.subscribed)
{
// Draw in the ranger scan if it has been changed
if (ranger->proxy->info.datatime != ranger->datatime)
{
ranger_draw(ranger);
ranger->datatime = ranger->proxy->info.datatime;
}
}
else
{
// Dont draw the scan
// This causes a segfault, because the ranger figures were already
// deleted above. I don't know whether commenting it out is the right
// thing to do - BPG.
/*
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
rtk_fig_show(ranger->scan_fig[ii], 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;
}
