00001 /***************************************************************************
00002  *  field.cpp - Drawer for a soccer field
00003  *
00004  *  Created:  23.09.2008
00005  *  Copyright 2008 Christof Rath <christof.rath@gmail.com>
00006  *
00007  ****************************************************************************/
00008
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021
00022 #include "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 /** @class FieldDrawer field_drawer.h
00036  * This class is used to draw a soccer field.
00037  *
00038  * @author Christof Rath
00039  */
00040 /** @var float FieldDrawer::_img_buffer
00041  * The pointer to the target image buffer
00042  */
00043 /** @var float FieldDrawer::_img_width
00044  * The width of the target image buffer
00045  */
00046 /** @var float FieldDrawer::_img_height
00047  * The height of the target image buffer
00048  */
00049 
00050 /**
00051  * Created a new field object
00052  *
00053  * @param lines the field lines container
00054  */
00055 FieldDrawer::FieldDrawer(const FieldLines &lines) :
00056   __lines(lines)
00057 {
00058   __points     = NULL;
00059   __points_est = NULL;
00060
00061   clear_own_pos();
00062
00063   set_color_background(YUV_t::black());
00064   set_color_field(YUV_t::green());
00065   set_color_lines(YUV_t::white());
00066
00067   set_color_own_pos(YUV_t::cyan());
00068   set_color_line_points(YUV_t::cyan());
00069
00070   set_color_own_pos_est(YUV_t::yellow()); //yellowish
00071   set_color_line_points_est(YUV_t::yellow());
00072 }
00073 
00074 /**
00075  * Destructor.
00076  */
00077 FieldDrawer::~FieldDrawer()
00078 {
00079 }
00080
00081 
00082 /**
00083  * Sets the angular offset between body and head (along the body axis)
00084  * @param head_yaw angular offset
00085  */
00086 void
00087 FieldDrawer::set_head_yaw(float head_yaw)
00088 {
00089   __head_yaw = head_yaw;
00090 }
00091 
00092 /**
00093  * Own position setter.
00094  * Sets the (calculated) own position on the field
00095  * @param own_position as calculated by the localization
00096  */
00097 void
00098 FieldDrawer::set_own_pos(field_pos_t own_position)
00099 {
00100   __own_position = own_position;
00101 }
00102 
00103 /**
00104  * Own position estimate setter.
00105  * Sets the position estimate (e.g. by triangulation, odometry, ...)
00106  * @param own_position_estimate as estimated
00107  */
00108 void
00109 FieldDrawer::set_own_pos_est(field_pos_t own_position_estimate)
00110 {
00111   __own_pos_est     = own_position_estimate;
00112 }
00113 
00114 /**
00115  * Clears the own position.
00116  * Used (e.g.) if the own position couldn't be calculated
00117  */
00118 void
00119 FieldDrawer::clear_own_pos()
00120 {
00121   __own_position.ori  = 12345;
00122   __own_pos_est.ori   = 12345;
00123   __head_yaw          = 12345;
00124   __points            = NULL;
00125   __points_est        = NULL;
00126
00127   _img_buffer = NULL;
00128   _img_width  = 0;
00129   _img_height = 0;
00130 }
00131 
00132 /**
00133  * Setter for detected line points
00134  *
00135  * @param points a list of line points (relative to the center of the field!)
00136  */
00137 void
00138 FieldDrawer::set_line_points(const fld_line_points_t *points)
00139 {
00140   __points = points;
00141 }
00142 
00143 /**
00144  * Setter for detected line points
00145  *
00146  * @param points_est a list of line points (relative to the center of the field!)
00147  */
00148 void
00149 FieldDrawer::set_line_points_est(const fld_line_points_t *points_est)
00150 {
00151   __points_est = points_est;
00152 }
00153
00154 
00155 /**
00156  * Calculates the conversion factor between field size and image size
00157  *
00158  * @param img_width      of the target image
00159  * @param img_height     of the target image
00160  * @param draw_landscape true if the image should be drawn landscape
00161  * @return the conversion factor
00162  */
00163 float
00164 FieldDrawer::get_scale(unsigned int img_width, unsigned int img_height, bool draw_landscape) const
00165 {
00166   float f_width  = (draw_landscape ? __lines.get_field_length() : __lines.get_field_width());
00167   float f_height = (draw_landscape ? __lines.get_field_width() : __lines.get_field_length());
00168   return std::min(img_width / f_width, img_height / f_height);
00169 }
00170 
00171 /**
00172  * Sets the background color (outside the field)
00173  * @param color to be used
00174  */
00175 void
00176 FieldDrawer::set_color_background(YUV_t color)
00177 {
00178   __c_background = color;
00179 }
00180 
00181 /**
00182  * Sets the field color
00183  * @param color to be used
00184  */
00185 void
00186 FieldDrawer::set_color_field(YUV_t color)
00187 {
00188   __c_field = color;
00189 }
00190 
00191 /**
00192  * Sets the lines color
00193  * @param color to be used
00194  */
00195 void
00196 FieldDrawer::set_color_lines(YUV_t color)
00197 {
00198   __c_lines = color;
00199 }
00200 
00201 /**
00202  * Sets the line points color
00203  * @param color to be used
00204  */
00205 void
00206 FieldDrawer::set_color_line_points(YUV_t color)
00207 {
00208   __c_line_points = color;
00209 }
00210 
00211 /**
00212  * Sets the line points color
00213  * @param color to be used
00214  */
00215 void
00216 FieldDrawer::set_color_line_points_est(YUV_t color)
00217 {
00218   __c_line_points_est = color;
00219 }
00220 
00221 /**
00222  * Sets the own position color
00223  * @param color to be used
00224  */
00225 void
00226 FieldDrawer::set_color_own_pos(YUV_t color)
00227 {
00228   __c_own_pos = color;
00229 }
00230 
00231 /**
00232  * Sets the own position estimates color
00233  * @param color to be used
00234  */
00235 void
00236 FieldDrawer::set_color_own_pos_est(YUV_t color)
00237 {
00238   __c_own_pos_est = color;
00239 }
00240
00241
00242 
00243 /**
00244  * Draws the field (including the own position [est]).
00245  * The position [est] and line points [est] gets reseted after drawing
00246  *
00247  * @param yuv422_planar the image buffer
00248  * @param img_width the image width
00249  * @param img_height the image height
00250  * @param draw_background true if the background (field and border) should be drawn
00251  * @param draw_landscape true if the field should be drawn landscape
00252  */
00253 void
00254 FieldDrawer::draw_field(unsigned char *yuv422_planar, unsigned int img_width, unsigned int img_height,
00255                         bool draw_background, bool draw_landscape)
00256 {
00257   _img_buffer = yuv422_planar;
00258   _img_width  = img_width;
00259   _img_height = img_height;
00260
00261   float f_width  = (draw_landscape ? __lines.get_field_length() : __lines.get_field_width());
00262   float f_height = (draw_landscape ? __lines.get_field_width() : __lines.get_field_length());
00263   float scale = std::min(_img_width / f_width, _img_height / f_height);
00264
00265   if (draw_background) {
00266     unsigned int draw_width  = static_cast<unsigned int>(f_width * scale);
00267     unsigned int draw_height = static_cast<unsigned int>(f_height * scale);
00268     unsigned int u_offset = _img_width * _img_height;
00269     unsigned int v_offset = u_offset + u_offset / 2;
00270
00271     if (_img_width == draw_width) {//use memcpy
00272       unsigned int offset = (_img_height - draw_height) / 2;
00273       memset(_img_buffer, __c_background.Y, offset * _img_width);
00274       memset(_img_buffer + offset * _img_width, __c_field.Y, draw_height * _img_width);
00275       memset(_img_buffer + (offset + draw_height) * _img_width, __c_background.Y, offset * _img_width);
00276
00277       offset /= 2;
00278       draw_height /= 2;
00279
00280       memset(_img_buffer + u_offset, __c_background.U, offset * _img_width);
00281       memset(_img_buffer + u_offset + offset * _img_width, __c_field.U, draw_height * _img_width);
00282       memset(_img_buffer + u_offset + (offset + draw_height) * _img_width, __c_background.U, offset * _img_width);
00283
00284       memset(_img_buffer + v_offset, __c_background.V, offset * _img_width);
00285       memset(_img_buffer + v_offset + offset * _img_width, __c_field.V, draw_height * _img_width);
00286       memset(_img_buffer + v_offset + (offset + draw_height) * _img_width, __c_background.V, offset * _img_width);
00287     } else {
00288       //center the field
00289       unsigned int sx = (_img_width - draw_width) / 2;
00290       unsigned int sy = (_img_height - draw_height) / 2;
00291
00292       ROI f_roi(sx,sy, draw_width,draw_height, _img_width,_img_height);
00293       for (unsigned int x = 0; x < _img_width; ++x) {
00294         for (unsigned int y = 0; y < _img_height; ++y) {
00295           if (f_roi.contains(x, y)) {
00296             _img_buffer[y * _img_width + x] = __c_field.Y;
00297             _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_field.U;
00298             _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_field.V;
00299           } else {
00300             _img_buffer[y * _img_width + x] = __c_background.Y;
00301             _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_background.U;
00302             _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_background.V;
00303           }
00304         }
00305       }
00306     }
00307   } else {
00308     unsigned int size = _img_width * _img_height;
00309     memset(_img_buffer, 0, size);
00310     memset(_img_buffer + size, 128, size);
00311   } //END: if (draw_background)
00312
00313
00314   draw_lines(__c_lines, draw_landscape, scale);
00315
00316   cart_coord_2d_t f_offs = __lines.get_field_offsets();
00317   unsigned int center_x = std::max(0, static_cast<int>(_img_width / 2) + static_cast<int>(f_offs.x * scale));
00318   unsigned int center_y = std::max(0, static_cast<int>(_img_height / 2) + static_cast<int>(f_offs.y * scale));
00319
00320   if (__own_pos_est.ori != 12345) {
00321     Drawer d;
00322     d.set_buffer(_img_buffer, _img_width, _img_height);
00323     d.set_color(__c_own_pos_est);
00324     unsigned int r = _img_width / 40;
00325     int x = static_cast<int>(__own_pos_est.x * scale);
00326     int y = static_cast<int>(__own_pos_est.y * scale);
00327     int dx = static_cast<int>(r * cosf(__own_pos_est.ori));
00328     int dy = static_cast<int>(r * sinf(__own_pos_est.ori));
00329
00330     if (draw_landscape) {
00331       x += center_x;
00332       y = center_y - y;
00333       d.draw_circle(x, y, r);
00334       d.draw_line(x, y, x + dx, y - dy);
00335     } else {
00336       x += center_y;
00337       y = center_x - y;
00338       d.draw_circle(y, x, r);
00339       d.draw_line(y, x, y + dy, x - dx);
00340     }
00341
00342     if(__head_yaw != 12345) {
00343       int hx = static_cast<int>(r * cosf(__own_pos_est.ori + __head_yaw));
00344       int hy = static_cast<int>(r * sinf(__own_pos_est.ori + __head_yaw));
00345       int hdx = static_cast<int>((r + 4) * cosf(__own_pos_est.ori + __head_yaw));
00346       int hdy = static_cast<int>((r + 4) * sinf(__own_pos_est.ori + __head_yaw));
00347
00348       if (draw_landscape) d.draw_line(x + hx, y - hy, x + hdx, y - hdy);
00349       else d.draw_line(y + hy, x - hx, y + hdy, x - hdx);
00350     }
00351   }
00352
00353   if (__own_position.ori != 12345) {
00354     Drawer d;
00355     d.set_buffer(_img_buffer, _img_width, _img_height);
00356     d.set_color(__c_own_pos);
00357     unsigned int r = _img_width / 40;
00358     int x   = static_cast<int>(__own_position.x * scale);
00359     int y   = static_cast<int>(__own_position.y * scale);
00360     int dx  = static_cast<int>(r * cosf(__own_position.ori));
00361     int dy  = static_cast<int>(r * sinf(__own_position.ori));
00362
00363     if (draw_landscape) {
00364       x += center_x;
00365       y = center_y - y;
00366       d.draw_circle(x, y, r);
00367       d.draw_line(x, y, x + dx, y - dy);
00368     } else {
00369       x += center_y;
00370       y = center_x - y;
00371       d.draw_circle(y, x, r);
00372       d.draw_line(y, x, y + dy, x - dx);
00373     }
00374
00375     if(__head_yaw != 12345) {
00376       int hx = static_cast<int>(r * cosf(__own_position.ori + __head_yaw));
00377       int hy = static_cast<int>(r * sinf(__own_position.ori + __head_yaw));
00378       int hdx = static_cast<int>((r + 4) * cosf(__own_position.ori + __head_yaw));
00379       int hdy = static_cast<int>((r + 4) * sinf(__own_position.ori + __head_yaw));
00380
00381       if (draw_landscape) d.draw_line(x + hx, y - hy, x + hdx, y - hdy);
00382       else d.draw_line(y + hy, x - hx, y + hdy, x - hdx);
00383     }
00384   }
00385
00386   draw_line_points(draw_landscape, scale);
00387   clear_own_pos();
00388 }
00389 
00390 /**
00391  * Draws the line points
00392  * @param draw_landscape true if the field should be drawn landscape
00393  * @param scale the pre calculated scale (conversion factor between image size and field size - if 0 the value gets calculated)
00394  */
00395 void
00396 FieldDrawer::draw_line_points(bool draw_landscape, float scale) const
00397 {
00398   if (!scale) {
00399     if (draw_landscape) scale = std::min(_img_width / __lines.get_field_length(), _img_height / __lines.get_field_width());
00400     else scale = std::min(_img_width / __lines.get_field_width(), _img_height / __lines.get_field_length());
00401   }
00402
00403   cart_coord_2d_t f_offs = __lines.get_field_offsets();
00404   unsigned int center_x = std::max(0, static_cast<int>(_img_width / 2) + static_cast<int>(f_offs.x * scale));
00405   unsigned int center_y = std::max(0, static_cast<int>(_img_height / 2) + static_cast<int>(f_offs.y * scale));
00406
00407   Drawer d;
00408   d.set_buffer(_img_buffer, _img_width, _img_height);
00409
00410   if (__points_est) {
00411     d.set_color(__c_line_points_est);
00412     for (fld_line_points_t::const_iterator it = __points_est->begin(); it != __points_est->end(); ++it) {
00413       unsigned int y = static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
00414       unsigned int x =static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
00415
00416       d.draw_cross(x, y, 4);
00417     }
00418   }
00419
00420   if (__points) {
00421     d.set_color(__c_line_points);
00422     for (fld_line_points_t::const_iterator it = __points->begin(); it != __points->end(); ++it) {
00423       unsigned int y = static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
00424       unsigned int x = static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
00425
00426       d.draw_cross(x, y, 4);
00427     }
00428   }
00429 }
00430
00431 
00432 /**
00433  * Draws the field lines to a SharedMemoryImageBuffer
00434  *
00435  * @param color of the lines
00436  * @param draw_landscape if true (default) the field is supposed to be landscape
00437  * @param scale the conversation factor between [m] and [px] (if 0 this value gets calculated)
00438  */
00439 void
00440 FieldDrawer::draw_lines(YUV_t color, bool draw_landscape, float scale) const
00441 {
00442   if (!scale) {
00443     if (draw_landscape) scale = std::min(_img_width / __lines.get_field_length(), _img_height / __lines.get_field_width());
00444     else scale = std::min(_img_width / __lines.get_field_width(), _img_height / __lines.get_field_length());
00445   }
00446
00447   cart_coord_2d_t f_offs = __lines.get_field_offsets();
00448   int f_off_x = static_cast<int>(f_offs.x * scale);
00449   int f_off_y = static_cast<int>(f_offs.y * scale);
00450
00451   unsigned int off_x = std::max(0, static_cast<int>(_img_width / 2) + f_off_x);
00452   unsigned int off_y = std::max(0, static_cast<int>(_img_height / 2) + f_off_y);
00453
00454   Drawer d;
00455   d.set_buffer(_img_buffer, _img_width, _img_height);
00456   d.set_color(color);
00457
00458   for (FieldLines::const_iterator it = __lines.begin(); it != __lines.end(); ++it) {
00459     unsigned int sx = static_cast<unsigned int>((draw_landscape ? (*it).start.x : (*it).start.y) * scale);
00460     unsigned int sy = static_cast<unsigned int>((draw_landscape ? (*it).start.y : (*it).start.x) * scale);
00461     unsigned int ex = static_cast<unsigned int>((draw_landscape ? (*it).end.x : (*it).end.y) * scale);
00462     unsigned int ey = static_cast<unsigned int>((draw_landscape ? (*it).end.y : (*it).end.x) * scale);
00463
00464     d.draw_line(off_x + sx, off_y + sy, off_x + ex, off_y + ey);
00465   }
00466
00467   for (field_circles_t::const_iterator it = __lines.get_circles().begin(); it != __lines.get_circles().end(); ++it) {
00468     unsigned int cx = static_cast<unsigned int>((draw_landscape ? it->center.x : it->center.y) * scale);
00469     unsigned int cy = static_cast<unsigned int>((draw_landscape ? it->center.y : it->center.x) * scale);
00470     unsigned int r  = static_cast<unsigned int>(it->radius * scale);
00471     //TODO: Draw only arcs for corner circle, etc.
00472     d.draw_circle(off_x + cx, off_y + cy, r);
00473   }
00474 }
00475
00476