// 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; }
// 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); } }
// Draw the ptz scan void ptz_draw(ptz_t *ptz) { double ox, oy, d; double ax, ay, bx, by; double fx, fd; // Camera field of view in x-direction (radians) fx = ptz->proxy->zoom; fd = 0.5 / tan(fx/2); rtk_fig_show(ptz->data_fig, 1); rtk_fig_clear(ptz->data_fig); rtk_fig_show(ptz->data_fig_tilt, 1); rtk_fig_clear(ptz->data_fig_tilt); rtk_fig_color_rgb32(ptz->data_fig, COLOR_PTZ_DATA); ox = 100 * cos(ptz->proxy->pan); oy = 100 * sin(ptz->proxy->pan); rtk_fig_line(ptz->data_fig, 0, 0, ox, oy); ox = 100 * cos(ptz->proxy->pan + fx / 2); oy = 100 * sin(ptz->proxy->pan + fx / 2); rtk_fig_line(ptz->data_fig, 0, 0, ox, oy); ox = 100 * cos(ptz->proxy->pan - fx / 2); oy = 100 * sin(ptz->proxy->pan - fx / 2); rtk_fig_line(ptz->data_fig, 0, 0, ox, oy); // Draw in the zoom bar (2 m in length) d = sqrt(fd * fd + 0.5 * 0.5); ax = d * cos(ptz->proxy->pan + fx / 2); ay = d * sin(ptz->proxy->pan + fx / 2); bx = d * cos(ptz->proxy->pan - fx / 2); by = d * sin(ptz->proxy->pan - fx / 2); rtk_fig_line(ptz->data_fig, ax, ay, bx, by); rtk_fig_color_rgb32(ptz->data_fig_tilt, COLOR_PTZ_DATA_TILT); ox = 100 * cos(ptz->proxy->tilt); oy = 100 * sin(ptz->proxy->tilt); rtk_fig_line(ptz->data_fig_tilt, 0, 0, ox, oy); }
// Update the objects void ident_update() { int i, j; double r; char text[64]; double ax, ay, aa, bx, by, ba; mezz_object_t *object; rtk_fig_clear(ident->fig); for (i = 0; i < ident->objectlist->count; i++) { object = ident->objectlist->objects + i; if (!object->valid) continue; r = object->max_sep; dewarp_world2image(object->px, object->py, &ax, &ay); dewarp_world2image(object->px + r * cos(object->pa+M_PI_2), object->py + r * sin(object->pa+M_PI_2), &bx, &by); rtk_fig_color(ident->fig, COLOR_IDENT); rtk_fig_arrow_ex(ident->fig, ax, ay, bx, by, 5); snprintf(text, sizeof(text), "obj [%d] (%.2f, %.2f)\n (%.0f, %.0f)", i, object->px, object->py, (object->ablob.ox + object->bblob.ox) / 2.0, (object->ablob.oy + object->bblob.oy) / 2.0); rtk_fig_text(ident->fig, ax + 10, ay - 20, 0, text); for (j = 0; j < 4; j++) { aa = object->pa + M_PI/4 + j * M_PI/2; ba = object->pa + M_PI/4 + (j + 1) * M_PI/2; dewarp_world2image(object->px + r * cos(aa), object->py + r * sin(aa), &ax, &ay); dewarp_world2image(object->px + r * cos(ba), object->py + r * sin(ba), &bx, &by); rtk_fig_line(ident->fig, ax, ay, bx, by); } } }
// 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; }