// Create a map device
map_t *map_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
int index, const char *drivername, int subscribe)
{
char label[64];
char section[64];
map_t *map;
map = malloc(sizeof(map_t));
map->proxy = playerc_map_create(client, index);
map->drivername = strdup(drivername);
map->datatime = 0;
snprintf(section, sizeof(section), "map:%d", index);
// Construct the menu
snprintf(label, sizeof(label), "map:%d (%s)", index, map->drivername);
map->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
map->subscribe_item = rtk_menuitem_create(map->menu, "Subscribe", 1);
map->continuous_item = rtk_menuitem_create(map->menu, "continuous update", 1);
// Set the initial menu state
rtk_menuitem_check(map->subscribe_item, subscribe);
// Construct figures
map->fig = rtk_fig_create(mainwnd->canvas, NULL, 1);
return map;
}
// Create a dio device
dio_t *dio_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
int index, const char *drivername, int subscribe)
{
char label[64];
char section[64];
dio_t *dio;
dio = malloc(sizeof(dio_t));
dio->proxy = playerc_dio_create(client, index);
dio->drivername = strdup(drivername);
dio->datatime = 0;
snprintf(section, sizeof(section), "dio:%d", index);
// Construct the menu
snprintf(label, sizeof(label), "dio:%d (%s)", index, dio->drivername);
dio->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
dio->subscribe_item = rtk_menuitem_create(dio->menu, "Subscribe", 1);
// Set the initial menu state
rtk_menuitem_check(dio->subscribe_item, subscribe);
// Construct figures
dio->fig = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 50);
return dio;
}
// 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);
}
}
// Create an ir device
ir_t *ir_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
int index, const char *drivername, int subscribe)
{
char label[64];
char section[64];
ir_t *ir;
ir = malloc(sizeof(ir_t));
ir->proxy = playerc_ir_create(client, index);
ir->drivername = strdup(drivername);
ir->datatime = 0;
ir->mainwnd = mainwnd;
snprintf(section, sizeof(section), "ir:%d", index);
// Construct the menu
snprintf(label, sizeof(label), "ir:%d (%s)", index, ir->drivername);
ir->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
ir->subscribe_item = rtk_menuitem_create(ir->menu, "Subscribe", 1);
// Set the initial menu state
// Set initial device state
rtk_menuitem_check(ir->subscribe_item, subscribe);
ir->fig_count = 0;
ir->scan_fig = NULL;
return ir;
}
// 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);
}
}
}
// 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;
}
// 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);
}
}
// 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);
}
}
// 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;
}
// 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);
}
}
// 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;
}
// 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);
*/
}
}
// 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;
}