// 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;
}
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;
}
// Draw the actarray scan
void actarray_draw(actarray_t *actarray)
{
double value;
double min, max;
double ax, ay, bx, by;
double fx, fd;
int ii;
actarray_allocate(actarray, actarray->proxy->actuators_count);
for(ii = 0; ii < actarray->proxy->actuators_count; ++ii)
{
value = actarray->proxy->actuators_data[ii].position;
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
if (value > max) value = max;
if (value < min) value = min;
value = 2*(value-min)/(max-min) -1;
rtk_fig_t * fig = actarray->actuator_fig[ii];
rtk_fig_show(fig, 1);
rtk_fig_clear(fig);
rtk_fig_origin(actarray->actuator_fig[ii], ARRAY_SPACING*ii +ARRAY_X_OFFSET, 0, 0);
rtk_fig_color_rgb32(fig, COLOR_ACTARRAY_DATA);
rtk_fig_line(fig, 0, -1, 0, 1);
rtk_fig_ellipse(actarray->actuator_fig[ii], 0, value, 0, 0.2, 0.2, 1);
}
}
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;
}
// 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;
}
// 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 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);
}
}
// 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);
*/
}
}
