22 #include <core/exceptions/software.h>
23 #include <fvutils/base/roi.h>
24 #include <fvutils/draw/drawer.h>
25 #include <fvutils/draw/field_drawer.h>
26 #include <fvutils/ipc/shm_image.h>
34 namespace firevision {
56 FieldDrawer::FieldDrawer(
const FieldLines &lines) : lines_(lines)
99 own_position_ = own_position;
110 own_pos_est_ = own_position_estimate;
120 own_position_.
ori = 12345;
121 own_pos_est_.
ori = 12345;
150 points_est_ = points_est;
166 return std::min(img_width / f_width, img_height / f_height);
176 c_background_ = color;
206 c_line_points_ = color;
216 c_line_points_est_ = color;
236 c_own_pos_est_ = color;
251 unsigned int img_width,
252 unsigned int img_height,
253 bool draw_background,
264 if (draw_background) {
265 unsigned int draw_width =
static_cast<unsigned int>(f_width * scale);
266 unsigned int draw_height =
static_cast<unsigned int>(f_height * scale);
268 unsigned int v_offset = u_offset + u_offset / 2;
271 unsigned int offset = (
_img_height - draw_height) / 2;
298 unsigned int sx = (
_img_width - draw_width) / 2;
302 for (
unsigned int x = 0; x <
_img_width; ++x) {
325 unsigned int center_x =
326 std::max(0,
static_cast<int>(
_img_width / 2) +
static_cast<int>(f_offs.
x * scale));
327 unsigned int center_y =
328 std::max(0,
static_cast<int>(
_img_height / 2) +
static_cast<int>(f_offs.
y * scale));
330 if (own_pos_est_.
ori != 12345) {
335 int x =
static_cast<int>(own_pos_est_.
x * scale);
336 int y =
static_cast<int>(own_pos_est_.
y * scale);
337 int dx =
static_cast<int>(r * cosf(own_pos_est_.
ori));
338 int dy =
static_cast<int>(r * sinf(own_pos_est_.
ori));
340 if (draw_landscape) {
352 if (head_yaw_ != 12345) {
353 int hx =
static_cast<int>(r * cosf(own_pos_est_.
ori + head_yaw_));
354 int hy =
static_cast<int>(r * sinf(own_pos_est_.
ori + head_yaw_));
355 int hdx =
static_cast<int>((r + 4) * cosf(own_pos_est_.
ori + head_yaw_));
356 int hdy =
static_cast<int>((r + 4) * sinf(own_pos_est_.
ori + head_yaw_));
359 d.
draw_line(x + hx, y - hy, x + hdx, y - hdy);
361 d.
draw_line(y + hy, x - hx, y + hdy, x - hdx);
365 if (own_position_.
ori != 12345) {
370 int x =
static_cast<int>(own_position_.
x * scale);
371 int y =
static_cast<int>(own_position_.
y * scale);
372 int dx =
static_cast<int>(r * cosf(own_position_.
ori));
373 int dy =
static_cast<int>(r * sinf(own_position_.
ori));
375 if (draw_landscape) {
387 if (head_yaw_ != 12345) {
388 int hx =
static_cast<int>(r * cosf(own_position_.
ori + head_yaw_));
389 int hy =
static_cast<int>(r * sinf(own_position_.
ori + head_yaw_));
390 int hdx =
static_cast<int>((r + 4) * cosf(own_position_.
ori + head_yaw_));
391 int hdy =
static_cast<int>((r + 4) * sinf(own_position_.
ori + head_yaw_));
394 d.
draw_line(x + hx, y - hy, x + hdx, y - hdy);
396 d.
draw_line(y + hy, x - hx, y + hdy, x - hdx);
422 unsigned int center_x =
423 std::max(0,
static_cast<int>(
_img_width / 2) +
static_cast<int>(f_offs.
x * scale));
424 unsigned int center_y =
425 std::max(0,
static_cast<int>(
_img_height / 2) +
static_cast<int>(f_offs.
y * scale));
432 for (fld_line_points_t::const_iterator it = points_est_->begin(); it != points_est_->end();
435 static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
437 static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
445 for (fld_line_points_t::const_iterator it = points_->begin(); it != points_->end(); ++it) {
447 static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
449 static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
476 int f_off_x =
static_cast<int>(f_offs.
x * scale);
477 int f_off_y =
static_cast<int>(f_offs.
y * scale);
479 unsigned int off_x = std::max(0,
static_cast<int>(
_img_width / 2) + f_off_x);
480 unsigned int off_y = std::max(0,
static_cast<int>(
_img_height / 2) + f_off_y);
486 for (FieldLines::const_iterator it = lines_.begin(); it != lines_.end(); ++it) {
488 static_cast<unsigned int>((draw_landscape ? (*it).start.x : (*it).start.y) * scale);
490 static_cast<unsigned int>((draw_landscape ? (*it).start.y : (*it).start.x) * scale);
492 static_cast<unsigned int>((draw_landscape ? (*it).end.x : (*it).end.y) * scale);
494 static_cast<unsigned int>((draw_landscape ? (*it).end.y : (*it).end.x) * scale);
496 d.
draw_line(off_x + sx, off_y + sy, off_x + ex, off_y + ey);
499 for (field_circles_t::const_iterator it = lines_.
get_circles().begin();
503 static_cast<unsigned int>((draw_landscape ? it->center.x : it->center.y) * scale);
505 static_cast<unsigned int>((draw_landscape ? it->center.y : it->center.x) * scale);
506 unsigned int r =
static_cast<unsigned int>(it->radius * scale);
void set_color(unsigned char y, unsigned char u, unsigned char v)
Set drawing color.
void draw_cross(unsigned int x_center, unsigned int y_center, unsigned int width)
Draws a cross.
void draw_line(unsigned int x_start, unsigned int y_start, unsigned int x_end, unsigned int y_end)
Draw line.
void set_buffer(unsigned char *buffer, unsigned int width, unsigned int height)
Set the buffer to draw to.
void draw_circle(int center_x, int center_y, unsigned int radius)
Draw circle.
void set_line_points(const fld_line_points_t *points)
Setter for detected line points.
void set_line_points_est(const fld_line_points_t *points_est)
Setter for detected line points.
void set_color_own_pos(YUV_t color)
Sets the own position color.
void set_own_pos(fawkes::field_pos_t own_position)
Own position setter.
void clear_own_pos()
Clears the own position.
void set_color_own_pos_est(YUV_t color)
Sets the own position estimates color.
void set_own_pos_est(fawkes::field_pos_t own_position_estimate)
Own position estimate setter.
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]).
void set_color_field(YUV_t color)
Sets the field color.
void set_color_line_points(YUV_t color)
Sets the line points color.
void set_color_lines(YUV_t color)
Sets the lines color.
void set_head_yaw(float head_yaw)
Sets the angular offset between body and head (along the body axis)
void set_color_line_points_est(YUV_t color)
Sets the line points color.
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.
unsigned int _img_width
The width of the target image buffer.
virtual ~FieldDrawer()
Destructor.
unsigned char * _img_buffer
The pointer to the target image buffer.
unsigned int _img_height
The height of the target image buffer.
virtual void draw_lines(YUV_t color, bool draw_landscape=true, float scale=0) const
Draws the field lines to a SharedMemoryImageBuffer.
void set_color_background(YUV_t color)
Sets the background color (outside the field)
virtual void draw_line_points(bool draw_landscape=true, float scale=0) const
Draws the line points.
This class acts as a container for lines on a soccer field.
fawkes::cart_coord_2d_t get_field_offsets() const
Offset getter.
float get_field_length() const
Field length getter.
const field_circles_t & get_circles() const
Get circles.
float get_field_width() const
Field width getter.
bool contains(unsigned int x, unsigned int y)
Check if this ROI contains the given coordinates.
Fawkes library namespace.
Cartesian coordinates (2D).
float y
y coordinate in meters
float x
x coordinate in meters
unsigned char V
V component.
static YUV_t_struct cyan()
static YUV_t_struct green()
static YUV_t_struct white()
unsigned char U
U component.
static YUV_t_struct yellow()
static YUV_t_struct black()
unsigned char Y
Y component.