// Create the color-space window
colorwnd_t *colorwnd_init(rtk_app_t *app, mezz_mmap_t *mmap)
{
colorwnd_t *wnd;
wnd = malloc(sizeof(colorwnd_t));
wnd->canvas = rtk_canvas_create(app);
// Set up the canvas
rtk_canvas_movemask(wnd->canvas, 0);
rtk_canvas_size(wnd->canvas, 512, 512);
rtk_canvas_scale(wnd->canvas, 1, 1);
rtk_canvas_origin(wnd->canvas, 0, 0);
wnd->vufig = rtk_fig_create(wnd->canvas, NULL, 0);
rtk_fig_origin(wnd->vufig, -256, -256, 0);
rtk_fig_color(wnd->vufig, 0, 0, 0);
rtk_fig_rectangle(wnd->vufig, 128, 128, 0, 256, 256, 1);
wnd->yufig = rtk_fig_create(wnd->canvas, NULL, 0);
rtk_fig_origin(wnd->yufig, 0, -256, 0);
rtk_fig_color(wnd->yufig, 0, 0, 0);
rtk_fig_rectangle(wnd->yufig, 128, 128, 0, 256, 256, 1);
wnd->vyfig = rtk_fig_create(wnd->canvas, NULL, 0);
rtk_fig_origin(wnd->vyfig, -256, 0, 0);
rtk_fig_color(wnd->vyfig, 0, 0, 0);
rtk_fig_rectangle(wnd->vyfig, 128, 128, 0, 256, 256, 1);
return wnd;
}
// Initialise the sample interface
int sample_init(imagewnd_t *imagewnd, colorwnd_t *colorwnd, mezz_mmap_t *mmap)
{
int i;
double r;
sample = malloc(sizeof(sample_t));
sample->mmap = mmap;
sample->radius = 5;
// Create sample point figures on the image
for (i = 0; i < ARRAYSIZE(sample->figs); i++)
{
sample->figs[i] = rtk_fig_create(imagewnd->canvas, imagewnd->imagefig, 1);
rtk_fig_movemask(sample->figs[i], RTK_MOVE_TRANS);
rtk_fig_origin(sample->figs[i], sample->mmap->width/2, sample->mmap->height/2, 0);
rtk_fig_color(sample->figs[i], COLOR_SAMPLE);
r = sample->radius + 2;
rtk_fig_rectangle(sample->figs[i], 0, 0, 0, r, r, 0);
rtk_fig_line(sample->figs[i], -2 * r, 0, -r, 0);
rtk_fig_line(sample->figs[i], +2 * r, 0, +r, 0);
rtk_fig_line(sample->figs[i], 0, -2 * r, 0, -r);
rtk_fig_line(sample->figs[i], 0, +2 * r, 0, +r);
}
// Create figures to put on the color space
sample->vufig = rtk_fig_create(colorwnd->canvas, colorwnd->vufig, 2);
sample->yufig = rtk_fig_create(colorwnd->canvas, colorwnd->yufig, 2);
sample->vyfig = rtk_fig_create(colorwnd->canvas, colorwnd->vyfig, 2);
return 0;
}
void actarray_allocate(actarray_t *actarray, int size)
{
int ii;
if (size == actarray->fig_count)
return;
if (size < actarray->fig_count)
{
for (ii=size; ii < actarray->fig_count; ++ii)
{
rtk_fig_destroy(actarray->actuator_fig[ii]);
rtk_fig_destroy(actarray->actuator_fig_cmd[ii]);
}
}
actarray->actuator_fig = realloc(actarray->actuator_fig, size * sizeof(rtk_fig_t*));
actarray->actuator_fig_cmd = realloc(actarray->actuator_fig_cmd, size * sizeof(rtk_fig_t*));
actarray->lastvalue = realloc(actarray->lastvalue, size*sizeof(double));
if (size > actarray->fig_count)
{
for (ii=actarray->fig_count; ii < size; ++ii)
{
actarray->lastvalue[ii] = 1e10;
actarray->actuator_fig[ii] = rtk_fig_create(actarray->mainwnd->canvas, actarray->mainwnd->robot_fig, 10);
actarray->actuator_fig_cmd[ii] = rtk_fig_create(actarray->mainwnd->canvas, actarray->mainwnd->robot_fig, 11);
rtk_fig_movemask(actarray->actuator_fig_cmd[ii], RTK_MOVE_TRANS);
rtk_fig_origin(actarray->actuator_fig_cmd[ii], ARRAY_SPACING*ii+ARRAY_X_OFFSET, 0, 0);
rtk_fig_color_rgb32(actarray->actuator_fig_cmd[ii], COLOR_ACTARRAY_CMD);
rtk_fig_ellipse(actarray->actuator_fig_cmd[ii], 0, 0, 0, 0.2, 0.2, 0);
}
}
actarray->fig_count = size;
}
int main(int argc, char **argv)
{
rtk_app_t *app;
rtk_canvas_t *canvas;
rtk_fig_t *fig1, *fig2, *fig3;
int i;
app = rtk_app_create();
canvas = rtk_canvas_create(app);
rtk_app_start(app);
fig1 = rtk_fig_create(canvas, NULL);
rtk_fig_rectangle(fig1, 0, 0, 0, 1.0, 0.5);
rtk_fig_ellipse(fig1, 0, 0, 0, 1.0, 0.5);
rtk_fig_arrow(fig1, 0, 0, M_PI / 4, 1.0, 0.2);
fig2 = rtk_fig_create(canvas, fig1);
rtk_fig_origin(fig2, 1, 0, 0);
rtk_fig_scale(fig2, 0.5);
rtk_fig_rectangle(fig2, 0, 0, 0, 1.0, 0.5);
rtk_fig_ellipse(fig2, 0, 0, 0, 1.0, 0.5);
rtk_fig_arrow(fig2, 0, 0, M_PI / 4, 1.0, 0.2);
fig3 = rtk_fig_create(canvas, NULL);
i = 0;
while (!rtk_app_quit(app))
{
//rtk_fig_origin(fig1, 0, 0, i * 0.012);
rtk_fig_clear(fig3);
rtk_fig_origin(fig3, sin(i * 0.07), -1.0, 0);
rtk_fig_rectangle(fig3, 0, 0, i * 0.1, 0.5, 1.0);
rtk_fig_arrow(fig3, 0, 0, -i * 0.1, 0.8, 0.20);
rtk_fig_text(fig3, 0.0, 0.2, 0, "some text");
i++;
usleep(20000);
}
rtk_app_stop(app);
rtk_canvas_export(canvas, "test.fig");
//rtk_canvas_destroy(canvas);
rtk_app_destroy(app);
return 0;
}
// Create a figure, setting the user data (rtv)
rtk_fig_t *rtk_fig_create_ex(rtk_canvas_t *canvas, rtk_fig_t *parent,
int layer, void* userdata )
{
rtk_fig_t* fig = rtk_fig_create(canvas, parent, layer );
fig->userdata = userdata;
return fig;
}
// 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;
}
// 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;
}
void ir_allocate_figures(ir_t * ir, int fig_count)
{
int i;
if (fig_count <= ir->fig_count)
return;
ir->scan_fig = realloc(ir->scan_fig,fig_count*sizeof(ir->scan_fig[0]));
// Construct figures
for (i = ir->fig_count; i < fig_count; i++)
ir->scan_fig[i] = rtk_fig_create(ir->mainwnd->canvas, ir->mainwnd->robot_fig, 1);
ir->fig_count = fig_count;
}
// Initialise the ident interface
int ident_init(imagewnd_t *imagewnd, mezz_mmap_t *mmap)
{
ident = malloc(sizeof(ident_t));
ident->imagewnd = imagewnd;
ident->objectlist = &mmap->objectlist;
// Create figure for drawing objects
ident->fig = rtk_fig_create(imagewnd->canvas, imagewnd->imagefig, 10);
return 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 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);
*/
}
}
