22 #include <fvutils/draw/field_drawer.h>
24 #include <core/exceptions/software.h>
25 #include <fvutils/base/roi.h>
26 #include <fvutils/draw/drawer.h>
27 #include <fvutils/ipc/shm_image.h>
33 using namespace fawkes;
35 namespace firevision {
94 __head_yaw = head_yaw;
105 __own_position = own_position;
116 __own_pos_est = own_position_estimate;
126 __own_position.
ori = 12345;
127 __own_pos_est.
ori = 12345;
156 __points_est = points_est;
173 return std::min(img_width / f_width, img_height / f_height);
183 __c_background = color;
213 __c_line_points = color;
223 __c_line_points_est = color;
243 __c_own_pos_est = color;
260 bool draw_background,
bool draw_landscape)
270 if (draw_background) {
271 unsigned int draw_width =
static_cast<unsigned int>(f_width * scale);
272 unsigned int draw_height =
static_cast<unsigned int>(f_height * scale);
274 unsigned int v_offset = u_offset + u_offset / 2;
277 unsigned int offset = (_img_height - draw_height) / 2;
279 memset(
_img_buffer + offset * _img_width, __c_field.
Y, draw_height * _img_width);
280 memset(
_img_buffer + (offset + draw_height) * _img_width, __c_background.
Y, offset * _img_width);
285 memset(
_img_buffer + u_offset, __c_background.
U, offset * _img_width);
286 memset(
_img_buffer + u_offset + offset * _img_width, __c_field.
U, draw_height * _img_width);
287 memset(
_img_buffer + u_offset + (offset + draw_height) * _img_width, __c_background.
U, offset * _img_width);
289 memset(
_img_buffer + v_offset, __c_background.
V, offset * _img_width);
290 memset(
_img_buffer + v_offset + offset * _img_width, __c_field.
V, draw_height * _img_width);
291 memset(
_img_buffer + v_offset + (offset + draw_height) * _img_width, __c_background.
V, offset * _img_width);
294 unsigned int sx = (
_img_width - draw_width) / 2;
295 unsigned int sy = (_img_height - draw_height) / 2;
297 ROI f_roi(sx,sy, draw_width,draw_height,
_img_width,_img_height);
298 for (
unsigned int x = 0; x <
_img_width; ++x) {
302 _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_field.
U;
303 _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_field.
V;
306 _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_background.
U;
307 _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_background.
V;
322 unsigned int center_x = std::max(0, static_cast<int>(
_img_width / 2) + static_cast<int>(f_offs.
x * scale));
323 unsigned int center_y = std::max(0, static_cast<int>(
_img_height / 2) + static_cast<int>(f_offs.
y * scale));
325 if (__own_pos_est.
ori != 12345) {
330 int x =
static_cast<int>(__own_pos_est.
x * scale);
331 int y =
static_cast<int>(__own_pos_est.
y * scale);
332 int dx =
static_cast<int>(r * cosf(__own_pos_est.
ori));
333 int dy =
static_cast<int>(r * sinf(__own_pos_est.
ori));
335 if (draw_landscape) {
347 if(__head_yaw != 12345) {
348 int hx =
static_cast<int>(r * cosf(__own_pos_est.
ori + __head_yaw));
349 int hy =
static_cast<int>(r * sinf(__own_pos_est.
ori + __head_yaw));
350 int hdx =
static_cast<int>((r + 4) * cosf(__own_pos_est.
ori + __head_yaw));
351 int hdy =
static_cast<int>((r + 4) * sinf(__own_pos_est.
ori + __head_yaw));
353 if (draw_landscape) d.
draw_line(x + hx, y - hy, x + hdx, y - hdy);
354 else d.
draw_line(y + hy, x - hx, y + hdy, x - hdx);
358 if (__own_position.
ori != 12345) {
363 int x =
static_cast<int>(__own_position.
x * scale);
364 int y =
static_cast<int>(__own_position.
y * scale);
365 int dx =
static_cast<int>(r * cosf(__own_position.
ori));
366 int dy =
static_cast<int>(r * sinf(__own_position.
ori));
368 if (draw_landscape) {
380 if(__head_yaw != 12345) {
381 int hx =
static_cast<int>(r * cosf(__own_position.
ori + __head_yaw));
382 int hy =
static_cast<int>(r * sinf(__own_position.
ori + __head_yaw));
383 int hdx =
static_cast<int>((r + 4) * cosf(__own_position.
ori + __head_yaw));
384 int hdy =
static_cast<int>((r + 4) * sinf(__own_position.
ori + __head_yaw));
386 if (draw_landscape) d.
draw_line(x + hx, y - hy, x + hdx, y - hdy);
387 else d.
draw_line(y + hy, x - hx, y + hdy, x - hdx);
409 unsigned int center_x = std::max(0, static_cast<int>(
_img_width / 2) + static_cast<int>(f_offs.
x * scale));
410 unsigned int center_y = std::max(0, static_cast<int>(
_img_height / 2) + static_cast<int>(f_offs.
y * scale));
417 for (fld_line_points_t::const_iterator it = __points_est->begin(); it != __points_est->end(); ++it) {
418 unsigned int y =
static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
419 unsigned int x =
static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
427 for (fld_line_points_t::const_iterator it = __points->begin(); it != __points->end(); ++it) {
428 unsigned int y =
static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
429 unsigned int x =
static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
453 int f_off_x =
static_cast<int>(f_offs.
x * scale);
454 int f_off_y =
static_cast<int>(f_offs.
y * scale);
456 unsigned int off_x = std::max(0, static_cast<int>(
_img_width / 2) + f_off_x);
457 unsigned int off_y = std::max(0, static_cast<int>(
_img_height / 2) + f_off_y);
463 for (FieldLines::const_iterator it = __lines.begin(); it != __lines.end(); ++it) {
464 unsigned int sx =
static_cast<unsigned int>((draw_landscape ? (*it).start.x : (*it).start.y) * scale);
465 unsigned int sy =
static_cast<unsigned int>((draw_landscape ? (*it).start.y : (*it).start.x) * scale);
466 unsigned int ex =
static_cast<unsigned int>((draw_landscape ? (*it).end.x : (*it).end.y) * scale);
467 unsigned int ey =
static_cast<unsigned int>((draw_landscape ? (*it).end.y : (*it).end.x) * scale);
469 d.
draw_line(off_x + sx, off_y + sy, off_x + ex, off_y + ey);
472 for (field_circles_t::const_iterator it = __lines.
get_circles().begin(); it != __lines.
get_circles().end(); ++it) {
473 unsigned int cx =
static_cast<unsigned int>((draw_landscape ? it->center.x : it->center.y) * scale);
474 unsigned int cy =
static_cast<unsigned int>((draw_landscape ? it->center.y : it->center.x) * scale);
475 unsigned int r =
static_cast<unsigned int>(it->radius * scale);