00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include <fvutils/draw/field_drawer.h>
00023
00024 #include <core/exceptions/software.h>
00025 #include <fvutils/base/roi.h>
00026 #include <fvutils/draw/drawer.h>
00027 #include <fvutils/ipc/shm_image.h>
00028
00029 #include <cmath>
00030 #include <cstring>
00031 #include <stdio.h>
00032
00033 using namespace fawkes;
00034
00035 namespace firevision {
00036 #if 0
00037 }
00038 #endif
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 FieldDrawer::FieldDrawer(const FieldLines &lines) :
00061 __lines(lines)
00062 {
00063 __points = NULL;
00064 __points_est = NULL;
00065
00066 clear_own_pos();
00067
00068 set_color_background(YUV_t::black());
00069 set_color_field(YUV_t::green());
00070 set_color_lines(YUV_t::white());
00071
00072 set_color_own_pos(YUV_t::cyan());
00073 set_color_line_points(YUV_t::cyan());
00074
00075 set_color_own_pos_est(YUV_t::yellow());
00076 set_color_line_points_est(YUV_t::yellow());
00077 }
00078
00079
00080
00081
00082 FieldDrawer::~FieldDrawer()
00083 {
00084 }
00085
00086
00087
00088
00089
00090
00091 void
00092 FieldDrawer::set_head_yaw(float head_yaw)
00093 {
00094 __head_yaw = head_yaw;
00095 }
00096
00097
00098
00099
00100
00101
00102 void
00103 FieldDrawer::set_own_pos(field_pos_t own_position)
00104 {
00105 __own_position = own_position;
00106 }
00107
00108
00109
00110
00111
00112
00113 void
00114 FieldDrawer::set_own_pos_est(field_pos_t own_position_estimate)
00115 {
00116 __own_pos_est = own_position_estimate;
00117 }
00118
00119
00120
00121
00122
00123 void
00124 FieldDrawer::clear_own_pos()
00125 {
00126 __own_position.ori = 12345;
00127 __own_pos_est.ori = 12345;
00128 __head_yaw = 12345;
00129 __points = NULL;
00130 __points_est = NULL;
00131
00132 _img_buffer = NULL;
00133 _img_width = 0;
00134 _img_height = 0;
00135 }
00136
00137
00138
00139
00140
00141
00142 void
00143 FieldDrawer::set_line_points(const fld_line_points_t *points)
00144 {
00145 __points = points;
00146 }
00147
00148
00149
00150
00151
00152
00153 void
00154 FieldDrawer::set_line_points_est(const fld_line_points_t *points_est)
00155 {
00156 __points_est = points_est;
00157 }
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168 float
00169 FieldDrawer::get_scale(unsigned int img_width, unsigned int img_height, bool draw_landscape) const
00170 {
00171 float f_width = (draw_landscape ? __lines.get_field_length() : __lines.get_field_width());
00172 float f_height = (draw_landscape ? __lines.get_field_width() : __lines.get_field_length());
00173 return std::min(img_width / f_width, img_height / f_height);
00174 }
00175
00176
00177
00178
00179
00180 void
00181 FieldDrawer::set_color_background(YUV_t color)
00182 {
00183 __c_background = color;
00184 }
00185
00186
00187
00188
00189
00190 void
00191 FieldDrawer::set_color_field(YUV_t color)
00192 {
00193 __c_field = color;
00194 }
00195
00196
00197
00198
00199
00200 void
00201 FieldDrawer::set_color_lines(YUV_t color)
00202 {
00203 __c_lines = color;
00204 }
00205
00206
00207
00208
00209
00210 void
00211 FieldDrawer::set_color_line_points(YUV_t color)
00212 {
00213 __c_line_points = color;
00214 }
00215
00216
00217
00218
00219
00220 void
00221 FieldDrawer::set_color_line_points_est(YUV_t color)
00222 {
00223 __c_line_points_est = color;
00224 }
00225
00226
00227
00228
00229
00230 void
00231 FieldDrawer::set_color_own_pos(YUV_t color)
00232 {
00233 __c_own_pos = color;
00234 }
00235
00236
00237
00238
00239
00240 void
00241 FieldDrawer::set_color_own_pos_est(YUV_t color)
00242 {
00243 __c_own_pos_est = color;
00244 }
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258 void
00259 FieldDrawer::draw_field(unsigned char *yuv422_planar, unsigned int img_width, unsigned int img_height,
00260 bool draw_background, bool draw_landscape)
00261 {
00262 _img_buffer = yuv422_planar;
00263 _img_width = img_width;
00264 _img_height = img_height;
00265
00266 float f_width = (draw_landscape ? __lines.get_field_length() : __lines.get_field_width());
00267 float f_height = (draw_landscape ? __lines.get_field_width() : __lines.get_field_length());
00268 float scale = std::min(_img_width / f_width, _img_height / f_height);
00269
00270 if (draw_background) {
00271 unsigned int draw_width = static_cast<unsigned int>(f_width * scale);
00272 unsigned int draw_height = static_cast<unsigned int>(f_height * scale);
00273 unsigned int u_offset = _img_width * _img_height;
00274 unsigned int v_offset = u_offset + u_offset / 2;
00275
00276 if (_img_width == draw_width) {
00277 unsigned int offset = (_img_height - draw_height) / 2;
00278 memset(_img_buffer, __c_background.Y, offset * _img_width);
00279 memset(_img_buffer + offset * _img_width, __c_field.Y, draw_height * _img_width);
00280 memset(_img_buffer + (offset + draw_height) * _img_width, __c_background.Y, offset * _img_width);
00281
00282 offset /= 2;
00283 draw_height /= 2;
00284
00285 memset(_img_buffer + u_offset, __c_background.U, offset * _img_width);
00286 memset(_img_buffer + u_offset + offset * _img_width, __c_field.U, draw_height * _img_width);
00287 memset(_img_buffer + u_offset + (offset + draw_height) * _img_width, __c_background.U, offset * _img_width);
00288
00289 memset(_img_buffer + v_offset, __c_background.V, offset * _img_width);
00290 memset(_img_buffer + v_offset + offset * _img_width, __c_field.V, draw_height * _img_width);
00291 memset(_img_buffer + v_offset + (offset + draw_height) * _img_width, __c_background.V, offset * _img_width);
00292 } else {
00293
00294 unsigned int sx = (_img_width - draw_width) / 2;
00295 unsigned int sy = (_img_height - draw_height) / 2;
00296
00297 ROI f_roi(sx,sy, draw_width,draw_height, _img_width,_img_height);
00298 for (unsigned int x = 0; x < _img_width; ++x) {
00299 for (unsigned int y = 0; y < _img_height; ++y) {
00300 if (f_roi.contains(x, y)) {
00301 _img_buffer[y * _img_width + x] = __c_field.Y;
00302 _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_field.U;
00303 _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_field.V;
00304 } else {
00305 _img_buffer[y * _img_width + x] = __c_background.Y;
00306 _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_background.U;
00307 _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_background.V;
00308 }
00309 }
00310 }
00311 }
00312 } else {
00313 unsigned int size = _img_width * _img_height;
00314 memset(_img_buffer, 0, size);
00315 memset(_img_buffer + size, 128, size);
00316 }
00317
00318
00319 draw_lines(__c_lines, draw_landscape, scale);
00320
00321 cart_coord_2d_t f_offs = __lines.get_field_offsets();
00322 unsigned int center_x = std::max(0, static_cast<int>(_img_width / 2) + static_cast<int>(f_offs.x * scale));
00323 unsigned int center_y = std::max(0, static_cast<int>(_img_height / 2) + static_cast<int>(f_offs.y * scale));
00324
00325 if (__own_pos_est.ori != 12345) {
00326 Drawer d;
00327 d.set_buffer(_img_buffer, _img_width, _img_height);
00328 d.set_color(__c_own_pos_est);
00329 unsigned int r = _img_width / 40;
00330 int x = static_cast<int>(__own_pos_est.x * scale);
00331 int y = static_cast<int>(__own_pos_est.y * scale);
00332 int dx = static_cast<int>(r * cosf(__own_pos_est.ori));
00333 int dy = static_cast<int>(r * sinf(__own_pos_est.ori));
00334
00335 if (draw_landscape) {
00336 x += center_x;
00337 y = center_y - y;
00338 d.draw_circle(x, y, r);
00339 d.draw_line(x, y, x + dx, y - dy);
00340 } else {
00341 x += center_y;
00342 y = center_x - y;
00343 d.draw_circle(y, x, r);
00344 d.draw_line(y, x, y + dy, x - dx);
00345 }
00346
00347 if(__head_yaw != 12345) {
00348 int hx = static_cast<int>(r * cosf(__own_pos_est.ori + __head_yaw));
00349 int hy = static_cast<int>(r * sinf(__own_pos_est.ori + __head_yaw));
00350 int hdx = static_cast<int>((r + 4) * cosf(__own_pos_est.ori + __head_yaw));
00351 int hdy = static_cast<int>((r + 4) * sinf(__own_pos_est.ori + __head_yaw));
00352
00353 if (draw_landscape) d.draw_line(x + hx, y - hy, x + hdx, y - hdy);
00354 else d.draw_line(y + hy, x - hx, y + hdy, x - hdx);
00355 }
00356 }
00357
00358 if (__own_position.ori != 12345) {
00359 Drawer d;
00360 d.set_buffer(_img_buffer, _img_width, _img_height);
00361 d.set_color(__c_own_pos);
00362 unsigned int r = _img_width / 40;
00363 int x = static_cast<int>(__own_position.x * scale);
00364 int y = static_cast<int>(__own_position.y * scale);
00365 int dx = static_cast<int>(r * cosf(__own_position.ori));
00366 int dy = static_cast<int>(r * sinf(__own_position.ori));
00367
00368 if (draw_landscape) {
00369 x += center_x;
00370 y = center_y - y;
00371 d.draw_circle(x, y, r);
00372 d.draw_line(x, y, x + dx, y - dy);
00373 } else {
00374 x += center_y;
00375 y = center_x - y;
00376 d.draw_circle(y, x, r);
00377 d.draw_line(y, x, y + dy, x - dx);
00378 }
00379
00380 if(__head_yaw != 12345) {
00381 int hx = static_cast<int>(r * cosf(__own_position.ori + __head_yaw));
00382 int hy = static_cast<int>(r * sinf(__own_position.ori + __head_yaw));
00383 int hdx = static_cast<int>((r + 4) * cosf(__own_position.ori + __head_yaw));
00384 int hdy = static_cast<int>((r + 4) * sinf(__own_position.ori + __head_yaw));
00385
00386 if (draw_landscape) d.draw_line(x + hx, y - hy, x + hdx, y - hdy);
00387 else d.draw_line(y + hy, x - hx, y + hdy, x - hdx);
00388 }
00389 }
00390
00391 draw_line_points(draw_landscape, scale);
00392 clear_own_pos();
00393 }
00394
00395
00396
00397
00398
00399
00400 void
00401 FieldDrawer::draw_line_points(bool draw_landscape, float scale) const
00402 {
00403 if (!scale) {
00404 if (draw_landscape) scale = std::min(_img_width / __lines.get_field_length(), _img_height / __lines.get_field_width());
00405 else scale = std::min(_img_width / __lines.get_field_width(), _img_height / __lines.get_field_length());
00406 }
00407
00408 cart_coord_2d_t f_offs = __lines.get_field_offsets();
00409 unsigned int center_x = std::max(0, static_cast<int>(_img_width / 2) + static_cast<int>(f_offs.x * scale));
00410 unsigned int center_y = std::max(0, static_cast<int>(_img_height / 2) + static_cast<int>(f_offs.y * scale));
00411
00412 Drawer d;
00413 d.set_buffer(_img_buffer, _img_width, _img_height);
00414
00415 if (__points_est) {
00416 d.set_color(__c_line_points_est);
00417 for (fld_line_points_t::const_iterator it = __points_est->begin(); it != __points_est->end(); ++it) {
00418 unsigned int y = static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
00419 unsigned int x =static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
00420
00421 d.draw_cross(x, y, 4);
00422 }
00423 }
00424
00425 if (__points) {
00426 d.set_color(__c_line_points);
00427 for (fld_line_points_t::const_iterator it = __points->begin(); it != __points->end(); ++it) {
00428 unsigned int y = static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
00429 unsigned int x = static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
00430
00431 d.draw_cross(x, y, 4);
00432 }
00433 }
00434 }
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444 void
00445 FieldDrawer::draw_lines(YUV_t color, bool draw_landscape, float scale) const
00446 {
00447 if (!scale) {
00448 if (draw_landscape) scale = std::min(_img_width / __lines.get_field_length(), _img_height / __lines.get_field_width());
00449 else scale = std::min(_img_width / __lines.get_field_width(), _img_height / __lines.get_field_length());
00450 }
00451
00452 cart_coord_2d_t f_offs = __lines.get_field_offsets();
00453 int f_off_x = static_cast<int>(f_offs.x * scale);
00454 int f_off_y = static_cast<int>(f_offs.y * scale);
00455
00456 unsigned int off_x = std::max(0, static_cast<int>(_img_width / 2) + f_off_x);
00457 unsigned int off_y = std::max(0, static_cast<int>(_img_height / 2) + f_off_y);
00458
00459 Drawer d;
00460 d.set_buffer(_img_buffer, _img_width, _img_height);
00461 d.set_color(color);
00462
00463 for (FieldLines::const_iterator it = __lines.begin(); it != __lines.end(); ++it) {
00464 unsigned int sx = static_cast<unsigned int>((draw_landscape ? (*it).start.x : (*it).start.y) * scale);
00465 unsigned int sy = static_cast<unsigned int>((draw_landscape ? (*it).start.y : (*it).start.x) * scale);
00466 unsigned int ex = static_cast<unsigned int>((draw_landscape ? (*it).end.x : (*it).end.y) * scale);
00467 unsigned int ey = static_cast<unsigned int>((draw_landscape ? (*it).end.y : (*it).end.x) * scale);
00468
00469 d.draw_line(off_x + sx, off_y + sy, off_x + ex, off_y + ey);
00470 }
00471
00472 for (field_circles_t::const_iterator it = __lines.get_circles().begin(); it != __lines.get_circles().end(); ++it) {
00473 unsigned int cx = static_cast<unsigned int>((draw_landscape ? it->center.x : it->center.y) * scale);
00474 unsigned int cy = static_cast<unsigned int>((draw_landscape ? it->center.y : it->center.x) * scale);
00475 unsigned int r = static_cast<unsigned int>(it->radius * scale);
00476
00477 d.draw_circle(off_x + cx, off_y + cy, r);
00478 }
00479 }
00480
00481 }