// 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;
}
// Draw the map scan
void map_draw(map_t *map)
{
int x,y;
double scale = map->proxy->resolution;
rtk_fig_show(map->fig, 1);
rtk_fig_clear(map->fig);
puts( "map draw" );
rtk_fig_color_rgb32(map->fig, 0xFF0000 );
rtk_fig_rectangle(map->fig,
0,0,0,
map->proxy->width * scale,
map->proxy->height * scale,
0 );
// TODO - combine contiguous cells to minimize the number of
// rectangles we have to draw - performance is pretty nasty right
// now on big maps.
for( x=0; x<map->proxy->width; x++ )
for( y=0; y<map->proxy->height; y++ )
{
switch( map->proxy->cells[ x + y * map->proxy->width ] )
{
case -1:
// empty: draw nothing
break;
case 0:
// unknown: draw grey square
rtk_fig_color_rgb32(map->fig, 0x808080 );
rtk_fig_rectangle(map->fig,
(x - map->proxy->width/2.0) * scale + scale/2.0,
(y - map->proxy->height/2.0) * scale + scale/2.0,
0,
scale, scale, 1);
break;
case +1:
// occupied: draw black square
rtk_fig_color_rgb32(map->fig, 0x0 );
rtk_fig_rectangle(map->fig,
(x - map->proxy->width/2.0) * scale + scale/2.0,
(y - map->proxy->height/2.0) * scale + scale/2.0,
0,
scale, scale, 1);
break;
default:
puts( "Warning: invalid occupancy value." );
break;
}
}
return;
}
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 ir scan
void ir_draw(ir_t *ir)
{
int i;
double dr, da;
double points[3][2];
for (i = 0; i < ir->proxy->data.ranges_count; i++)
{
rtk_fig_show(ir->scan_fig[i], 1);
rtk_fig_clear(ir->scan_fig[i]);
// Draw in the ir itself
rtk_fig_color_rgb32(ir->scan_fig[i], COLOR_IR);
rtk_fig_rectangle(ir->scan_fig[i], 0, 0, 0, 0.01, 0.05, 0);
// Draw in the range scan
rtk_fig_color_rgb32(ir->scan_fig[i], COLOR_IR_SCAN);
dr = ((double)ir->proxy->data.ranges[i]);
da = 20 * M_PI / 180 / 2;
points[0][0] = 0;
points[0][1] = 0;
points[1][0] = dr * cos(-da);
points[1][1] = dr * sin(-da);
points[2][0] = dr * cos(+da);
points[2][1] = dr * sin(+da);
rtk_fig_polygon(ir->scan_fig[i], 0, 0, 0, 3, points, 1);
}
}
// 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;
}
// Update the YUV plots
void sample_update_yuv()
{
int i;
int pixel, r, g, b;
rtk_fig_clear(sample->vufig);
rtk_fig_clear(sample->yufig);
rtk_fig_clear(sample->vyfig);
for (i = 0; i < sample->pixel_count; i++)
{
pixel = sample->pixels[i];
if (sample->mmap->depth == 16)
{
r = R_RGB16(pixel);
g = G_RGB16(pixel);
b = B_RGB16(pixel);
}
else if (sample->mmap->depth == 32)
{
r = R_RGB32(pixel);
g = G_RGB32(pixel);
b = B_RGB32(pixel);
}
rtk_fig_color(sample->vufig, r / 256.0, g / 256.0, b / 256.0);
rtk_fig_rectangle(sample->vufig, V_RGB(r, g, b), U_RGB(r, g, b), 0, 2, 2, 1);
rtk_fig_color(sample->yufig, r / 256.0, g / 256.0, b / 256.0);
rtk_fig_rectangle(sample->yufig, Y_RGB(r, g, b), U_RGB(r, g, b), 0, 2, 2, 1);
rtk_fig_color(sample->vyfig, r / 256.0, g / 256.0, b / 256.0);
rtk_fig_rectangle(sample->vyfig, V_RGB(r, g, b), Y_RGB(r, g, b), 0, 2, 2, 1);
}
}
// Draw the map scan
void vectormap_draw(vectormap_t *map)
{
unsigned ii, jj;
uint32_t colour = 0xFF0000;
double xCenter, yCenter;
const uint8_t * feature;
size_t feature_count;
rtk_fig_show(map->fig, 1);
rtk_fig_clear(map->fig);
// draw map data
for (ii = 0; ii < map->proxy->layers_count; ++ii)
{
// get a different colour for each layer the quick way, will duplicate after 6 layers
colour = colour >> 4 & colour << 24;
rtk_fig_color_rgb32(map->fig, colour);
// render the features
for (jj = 0; jj < map->proxy->layers_data[ii]->features_count; ++jj)
{
feature = playerc_vectormap_get_feature_data( map->proxy, ii, jj );
feature_count = playerc_vectormap_get_feature_data_count( map->proxy, ii, jj );
if ((feature) && (feature_count > 0)) player_wkb_process_wkb(map->proxy->wkbprocessor, feature, feature_count, (playerwkbcallback_t)(rtk_fig_line), map->fig);
}
}
// draw map extent
rtk_fig_color_rgb32( map->fig, 0xFF0000 );
xCenter = map->proxy->extent.x1 - (map->proxy->extent.x1 - map->proxy->extent.x0)/2;
yCenter = map->proxy->extent.y1 - (map->proxy->extent.y1 - map->proxy->extent.y0)/2;
rtk_fig_rectangle(
map->fig,
xCenter,
yCenter,
0,
map->proxy->extent.x1 - map->proxy->extent.x0,
map->proxy->extent.y1 - map->proxy->extent.y0,
0
);
return;
}
// Draw the map scan
void map_draw(map_t *map)
{
double scale = map->proxy->resolution;
double color;
char val;
char* cellsDrawn=(char*)calloc(map->proxy->width*map->proxy->height,sizeof(char));
int mapWidth=map->proxy->width;
int mapHeight=map->proxy->height;
int x,y;
double ox,oy;
double rectangleWidth,rectangleHeight;
int startx, starty;
int endx,endy;
rtk_fig_show(map->fig, 1);
rtk_fig_clear(map->fig);
puts( "map draw" );
rtk_fig_color(map->fig, 0.5, 0.5, 0.5 );
rtk_fig_rectangle(map->fig, 0,0,0, mapWidth * scale, mapHeight* scale, 1 );
for( y=0; y<mapHeight; y++ )
for( x=0; x<mapWidth; x++)
{
if(cellsDrawn[x+y*mapWidth]==TRUE)
continue;
startx=x;
starty=y;
endy=mapHeight-1;
val = map->proxy->cells[ x + y * mapWidth ];
if(val==0)
continue;
while(x < mapWidth)
{
int yy = y;
while(yy+1 < mapHeight)
{
if(map->proxy->cells[x+(yy+1)*mapWidth]==val)
yy++;
else
break;
}
if( yy < endy )
endy=yy;
if(x+1<mapWidth &&
map->proxy->cells[(x+1)+y*mapWidth]==val &&
cellsDrawn[(x+1)+y*mapWidth]==FALSE)
x++;
else
break;
}
endx=x;
map_mark_cells(cellsDrawn,mapWidth,mapHeight,startx,starty,endx,endy);
rectangleWidth=(endx-startx+1)*scale;
rectangleHeight=(endy-starty+1)*scale;
ox=((startx-mapWidth/2.0f)*scale+rectangleWidth/2.0f);
oy=((starty-mapHeight/2.0f)*scale+rectangleHeight/2.0f);
color = (double)val/map->proxy->data_range; // scale to[-1,1]
color *= -1.0; //flip sign for coloring occupied to black
color = (color + 1.0)/2.0; // scale to [0,1]
rtk_fig_color(map->fig, color, color, color );
rtk_fig_rectangle(map->fig, ox, oy, 0, rectangleWidth, rectangleHeight,1);
}
free(cellsDrawn);
return;
}
// Draw the ranger scan
void ranger_draw(ranger_t *ranger)
{
int ii = 0, jj = 0;
int point_count;
double point1[2], point2[2];
double (*points)[2];
double current_angle = 0.0f, temp = 0.0f;
unsigned int ranges_per_sensor = 0;
// Drawing type depends on the selected sensor type
// Singular sensors (e.g. sonar sensors):
// Draw a cone for the first range scan of each sensor
// Non-singular sensors (e.g. laser scanner):
// Draw the edge of the scan and empty space
// Calculate the number of ranges per sensor
if (ranger->proxy->sensor_count == 0)
ranges_per_sensor = ranger->proxy->ranges_count;
else
ranges_per_sensor = ranger->proxy->ranges_count / ranger->proxy->sensor_count;
if (rtk_menuitem_ischecked(ranger->device_item))
{
// Draw sonar-like
points = calloc(3, sizeof(double)*2);
temp = 20.0f * M_PI / 180.0f / 2.0f;
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_SONAR_SCAN);
// Draw a cone for the first range for each sensor
// Assume the range is straight ahead (ignore min_angle and resolution properties)
points[0][0] = 0.0f;
points[0][1] = 0.0f;
points[1][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(-temp);
points[1][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(-temp);
points[2][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(temp);
points[2][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(temp);
rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, 3, points, 1);
// Draw the sensor itself
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER);
rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0);
}
free(points);
points=NULL;
}
else
{
// Draw laser-like
if (rtk_menuitem_ischecked(ranger->style_item))
{
// Draw each sensor in turn
points = calloc(ranger->proxy->sensor_count, sizeof(double)*2);
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
// Draw empty space
points[0][0] = ranger->proxy->sensor_poses[ii].px;
points[0][1] = ranger->proxy->sensor_poses[ii].py;
point_count = 1;
current_angle = ranger->start_angle;
// Loop over the ranges
for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++)
{
range_to_point(ranger, jj, ii, current_angle, points[point_count]);
// Move round to the angle of the next range
current_angle += ranger->resolution;
point_count++;
}
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_EMP);
rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, point_count, points, 1);
// Draw occupied space
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
current_angle = ranger->start_angle;
for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++)
{
range_to_point(ranger, jj, ii, current_angle - ranger->resolution, point1);
range_to_point(ranger, jj, ii, current_angle + ranger->resolution, point2);
rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0], point2[1]);
current_angle += ranger->resolution;
}
}
free(points);
points = NULL;
}
else
{
// Draw a range scan for each individual sensor in the device
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
current_angle = ranger->start_angle;
// Get the first point
range_to_point(ranger, ii * ranges_per_sensor, ii, ranger->start_angle, point1);
// Loop over the rest of the ranges
for (jj = ii * ranges_per_sensor + 1; jj < (ii + 1) * ranges_per_sensor; jj++)
{
range_to_point(ranger, jj, ii, current_angle, point2);
// Draw a line from point 1 (previous point) to point 2 (current point)
rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0], point2[1]);
point1[0] = point2[0];
point1[1] = point2[1];
// Move round to the angle of the next range
current_angle += ranger->resolution;
}
}
}
// For each sensor...
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
if (rtk_menuitem_ischecked(ranger->intns_item))
{
// Draw an intensity scan
if (ranger->proxy->intensities_count > 0)
{
current_angle = ranger->start_angle;
for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++)
{
if (ranger->proxy->intensities[0] != 0)
{
range_to_point(ranger, jj, ii, current_angle, point1);
rtk_fig_rectangle(ranger->scan_fig[ii], point1[0], point1[1], 0, 0.05, 0.05, 1);
}
current_angle += ranger->resolution;
}
}
}
// Draw the sensor itself
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER);
rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0);
}
}
}
// Draw the laser scan
void laser_draw(laser_t *laser)
{
int i;
int style;
double ax, ay, bx, by;
double r, b, res;
int point_count;
double (*points)[2];
rtk_fig_show(laser->scan_fig, 1);
rtk_fig_clear(laser->scan_fig);
if (!rtk_menuitem_ischecked(laser->style_item))
{
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC);
// Draw in the range scan
for (i = 0; i < laser->proxy->scan_count; i++)
{
bx = laser->proxy->point[i].px;
by = laser->proxy->point[i].py;
if (i == 0)
{
ax = bx;
ay = by;
}
rtk_fig_line(laser->scan_fig, ax, ay, bx, by);
ax = bx;
ay = by;
}
}
else
{
res = laser->proxy->scan_res / 2;
// Draw in the range scan (empty space)
points = calloc(laser->proxy->scan_count,sizeof(double)*2);
for (i = 0; i < laser->proxy->scan_count; i++)
{
r = laser->proxy->scan[i][0];
b = laser->proxy->scan[i][1];
points[i][0] = r * cos(b - res);
points[i][1] = r * sin(b - res);
}
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_EMP);
rtk_fig_polygon(laser->scan_fig, 0, 0, 0, laser->proxy->scan_count, points, 1);
free(points);
points = NULL;
// Draw in the range scan (occupied space)
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC);
for (i = 0; i < laser->proxy->scan_count; i++)
{
r = laser->proxy->scan[i][0];
b = laser->proxy->scan[i][1];
ax = r * cos(b - res);
ay = r * sin(b - res);
bx = r * cos(b + res);
by = r * sin(b + res);
rtk_fig_line(laser->scan_fig, ax, ay, bx, by);
}
}
// Draw in the intensity scan
for (i = 0; i < laser->proxy->scan_count; i++)
{
if (laser->proxy->intensity[i] == 0)
continue;
ax = laser->proxy->point[i].px;
ay = laser->proxy->point[i].py;
rtk_fig_rectangle(laser->scan_fig, ax, ay, 0, 0.05, 0.05, 1);
}
// Draw in the laser itself
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER);
rtk_fig_rectangle(laser->scan_fig, 0, 0, 0,
laser->proxy->size[0], laser->proxy->size[1], 0);
return;
}
