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> 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);
void set_color_lines(YUV_t color)
Sets the lines color.
void set_color_own_pos_est(YUV_t color)
Sets the own position estimates color.
float get_field_length() const
Field length getter.
unsigned char V
V component.
static YUV_t_struct yellow()
virtual void draw_lines(YUV_t color, bool draw_landscape=true, float scale=0) const
Draws the field lines to a SharedMemoryImageBuffer.
void clear_own_pos()
Clears the own position.
unsigned int _img_width
The width of the target image buffer.
Cartesian coordinates (2D).
void draw_cross(unsigned int x_center, unsigned int y_center, unsigned int width)
Draws a cross.
Fawkes library namespace.
float get_scale(unsigned int img_width, unsigned int img_height, bool draw_landscape=true) const
Calculates the conversion factor between field size and image size.
virtual void draw_field(unsigned char *yuv422_planar, unsigned int img_width, unsigned int img_height, bool draw_background=true, bool draw_landscape=true)
Draws the field (including the own position [est]).
virtual ~FieldDrawer()
Destructor.
unsigned char Y
Y component.
static YUV_t_struct cyan()
void draw_line(unsigned int x_start, unsigned int y_start, unsigned int x_end, unsigned int y_end)
Draw line.
static YUV_t_struct black()
const field_circles_t & get_circles() const
Get circles.
void set_color_background(YUV_t color)
Sets the background color (outside the field)
void set_buffer(unsigned char *buffer, unsigned int width, unsigned int height)
Set the buffer to draw to.
void set_line_points(const fld_line_points_t *points)
Setter for detected line points.
float x
x coordinate in meters
unsigned char * _img_buffer
The pointer to the target image buffer.
bool contains(unsigned int x, unsigned int y)
Check if this ROI contains the given coordinates.
float get_field_width() const
Field width getter.
static YUV_t_struct green()
void set_color_line_points(YUV_t color)
Sets the line points color.
void set_head_yaw(float head_yaw)
Sets the angular offset between body and head (along the body axis)
void set_color_own_pos(YUV_t color)
Sets the own position color.
unsigned char U
U component.
unsigned int _img_height
The height of the target image buffer.
void draw_circle(int center_x, int center_y, unsigned int radius)
Draw circle.
This class acts as a container for lines on a soccer field.
fawkes::cart_coord_2d_t get_field_offsets() const
Offset getter.
void set_own_pos(fawkes::field_pos_t own_position)
Own position setter.
void set_color_field(YUV_t color)
Sets the field color.
float y
y coordinate in meters
virtual void draw_line_points(bool draw_landscape=true, float scale=0) const
Draws the line points.
static YUV_t_struct white()
void set_color_line_points_est(YUV_t color)
Sets the line points color.
void set_own_pos_est(fawkes::field_pos_t own_position_estimate)
Own position estimate setter.
void set_color(unsigned char y, unsigned char u, unsigned char v)
Set drawing color.
void set_line_points_est(const fld_line_points_t *points_est)
Setter for detected line points.