Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
field_view.cpp
1 
2 /***************************************************************************
3  * field_view.cpp - Draws the field and the robots on it
4  *
5  * Created: Wed April 09 21:00:49 2008
6  * Copyright 2008 Daniel Beck
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "field_view.h"
24 
25 #include <worldinfo_utils/data_container.h>
26 #include <cairomm/context.h>
27 
28 #include <map>
29 #include <cstdio>
30 
31 using namespace std;
32 using namespace fawkes;
33 
34 /** @class FieldView <tools/worldinfo_viewer/field_view.h>
35  * Drawing widget that draws an (MSL-) soccer field with robots,
36  * opponents, and balls.
37  * @author Daniel Beck
38  */
39 
40 /** Constructor.
41  * @param show_pose default value for show pose
42  * @param show_ball default value for show ball
43  * @param show_opponents default value for show opponents
44  * @param data pointer to a WorldInfoDataContainer that is used as the
45  * data source
46  */
48  bool show_pose,
49  bool show_ball,
50  bool show_opponents )
51 {
52  m_show_pose_default = show_pose;
53  m_show_ball_default = show_ball;
54  m_show_opponents_default = show_opponents;
55 
56  m_data_container = data;
57 }
58 
59 /** Destructor. */
61 {
62 }
63 
64 /** Toggle whether to show the pose of the specified robot.
65  * @param name the hostname of the robot
66  * @return true if the pose wasn't shown before, false otherwise
67  */
68 bool
69 FieldView::toggle_show_pose( Glib::ustring name )
70 {
71  std::map< Glib::ustring, bool >::iterator iter = m_show_pose.find( name );
72  if ( iter != m_show_pose.end() )
73  {
74  iter->second = iter->second ? false : true;
75  return iter->second;
76  }
77  else
78  {
79  m_show_pose[ name ] = m_show_pose_default;
80  return m_show_pose_default;
81  }
82 }
83 
84 /** Toggle whether to show the ball detected by the specified robot.
85  * @param name the hostname of the robot
86  * @return true if the ball wasn't shown before, false otherwise
87  */
88 bool
89 FieldView::toggle_show_ball( Glib::ustring name )
90 {
91  std::map< Glib::ustring, bool >::iterator iter = m_show_ball.find( name );
92  if ( iter != m_show_ball.end() )
93  {
94  iter->second = iter->second ? false : true;
95  return iter->second;
96  }
97  else
98  {
99  m_show_ball[ name ] = m_show_ball_default;
100  return m_show_ball_default;
101  }
102 }
103 
104 /** Toggle whether to show the opponents seen by the specified robot.
105  * @param name the hostname of the robot
106  * @return true if the opponents weren't shown before, false otherwise
107  */
108 bool
109 FieldView::toggle_show_opponents( Glib::ustring name )
110 {
111  std::map< Glib::ustring, bool >::iterator iter = m_show_opponents.find( name );
112  if ( iter != m_show_opponents.end() )
113  {
114  iter->second = iter->second ? false : true;
115  return iter->second;
116  }
117  else
118  {
119  m_show_opponents[ name ] = m_show_opponents_default;
120  return m_show_opponents_default;
121  }
122 }
123 
124 /** Remove a host.
125  * @param name the name of the host to be removed.
126  */
127 void
128 FieldView::remove_host( Glib::ustring name )
129 {
130  m_show_pose.erase( name );
131  m_show_ball.erase( name );
132  m_show_opponents.erase( name );
133 }
134 
135 /** Overloaded signal handler.
136  * This is were the drawing action happens.
137  * @param context cairo context for drawing
138  * @return always true
139  */
140 bool
141 FieldView::on_draw(const Cairo::RefPtr<Cairo::Context> &context)
142 {
143  Glib::RefPtr<Gdk::Window> window = get_window();
144 
145  if (window)
146  {
147  Gtk::Allocation allocation = get_allocation();
148  const int width = allocation.get_width();
149  const int height = allocation.get_height();
150 
151  // setup
152  float unit;
153  if ( (width / 22.0) <= (height / 16.0) )
154  { unit = width / 22.0; }
155  else
156  { unit = height / 16.0; }
157 
158  context->translate( width / 2.0, height / 2.0 );
159  context->scale(unit, -unit);
160 
161  draw_field_msl(context);
162 
163  list<string> hosts = m_data_container->get_hosts();
164 
165  for ( list<string>::iterator i = hosts.begin();
166  i != hosts.end();
167  ++i )
168  {
169  const char* host = i->c_str();
170 
171  HomPose2d pose;
172  HomPoint ball_pos;
173  std::map<unsigned int, HomPoint> opp_positions;
174 
175  bool show_pose;
176  bool show_ball;
177  bool show_opponents;
178  std::map< Glib::ustring, bool >::iterator iter;
179 
180  iter = m_show_pose.find( *i );
181  if ( iter == m_show_pose.end() )
182  { show_pose = m_show_pose_default; }
183  else
184  { show_pose = iter->second; }
185 
186  iter = m_show_ball.find( *i );
187  if ( iter == m_show_ball.end() )
188  { show_ball = m_show_ball_default; }
189  else
190  { show_ball = iter->second; }
191 
192  iter = m_show_opponents.find( *i );
193  if ( iter == m_show_opponents.end() )
194  { show_opponents = m_show_opponents_default; }
195  else
196  { show_opponents = iter->second; }
197 
198 
199  if ( m_data_container->get_robot_pose( host, pose ) )
200  {
201  if ( show_pose )
202  { draw_robot( context, pose.x(), pose.y(), pose.yaw(), host ); }
203 
204  if ( m_data_container->get_ball_pos_global( host, ball_pos ) &&
205  show_ball )
206  {
207  draw_ball( context, ball_pos.x(), ball_pos.y(),
208  pose.x(), pose.y() );
209  }
210 
211  // TODO
212 // if ( m_data_container->get_opponenet_pos( host, opp_positions ) &&
213 // show_opponents )
214 // {
215 // }
216  }
217  else
218  {
219  HomPolar ball_pos;
220  if ( m_data_container->get_ball_pos_relative( host, ball_pos ) &&
221  show_ball )
222  {
223  draw_ball( context, ball_pos.x(), ball_pos.y(), 0.0, 0.0 );
224  }
225  }
226 
227  // opponents are always global
228  std::map<unsigned int, HomPoint> opponents;
229  if ( m_data_container->get_opponent_pos( host, opponents ) &&
230  show_opponents )
231  {
232  for ( std::map<unsigned int, HomPoint>::iterator i = opponents.begin();
233  i != opponents.end();
234  ++i )
235  {
236  HomPoint p = i->second;
237  draw_obstacle( context, p.x(), p.y(), 0.2 );
238  }
239  }
240  }
241  }
242 
243  return true;
244 }
245 
246 void
247 FieldView::draw_field_msl(Cairo::RefPtr<Cairo::Context> context)
248 {
249  context->save();
250 
251  // background
252  context->save();
253  context->set_source_rgb(48.0 / 255.0, 215.0 / 255.0, 31.0 / 255.0);
254  context->paint();
255  context->restore();
256 
257  context->save();
258  context->set_line_width(0.125);
259  context->set_source_rgb(1.0, 1.0, 1.0);
260 
261  // goals
262  context->save();
263  context->set_source_rgb(237.0 / 255.0, 240.0 / 255.0, 12.0 / 255.0);
264  context->move_to(9.0, 1.0625);
265  context->line_to(9.5, 1.0625);
266  context->line_to(9.5,-1.0625);
267  context->line_to(9.0,-1.0625);
268  context->stroke();
269  context->restore();
270 
271  context->save();
272  context->set_source_rgb(12.0 / 255.0, 14.0 / 255.0, 240.0 / 255.0);
273  context->move_to(-9.0, 1.0625);
274  context->line_to(-9.5, 1.0625);
275  context->line_to(-9.5,-1.0625);
276  context->line_to(-9.0,-1.0625);
277  context->stroke();
278  context->restore();
279 
280  // center circle
281  context->arc(0.0, 0.0, 2.0, 0.0, 2 * M_PI);
282 
283  // corner arcs
284  context->move_to(9.0, 6.0);
285  context->arc(9.0, 6.0, 0.75, M_PI, -M_PI/2.0);
286  context->move_to(9.0,-6.0);
287  context->arc(9.0, -6.0, 0.75, M_PI/2.0, M_PI);
288  context->move_to(-9.0, -6.0);
289  context->arc(-9.0, -6.0, 0.75, 0.0, M_PI/2.0);
290  context->move_to(-9.0, 6.0);
291  context->arc(-9.0, 6.0, 0.75, -M_PI/2.0, 0.0);
292 
293  // lines
294  context->save();
295  context->set_line_cap(Cairo::LINE_CAP_SQUARE);
296  context->move_to( 0.0, 6.0);
297  context->line_to( 0.0,-6.0);
298  context->move_to( 9.0, 6.0);
299  context->line_to( 9.0,-6.0);
300  context->line_to(-9.0,-6.0);
301  context->line_to(-9.0, 6.0);
302  context->close_path();
303  context->restore();
304 
305  // goal areas
306  context->move_to(9.0, 1.75);
307  context->line_to(8.25, 1.75);
308  context->line_to(8.25,-1.75);
309  context->line_to(9.0,-1.75);
310  context->move_to(-9.0, 1.75);
311  context->line_to(-8.25, 1.75);
312  context->line_to(-8.25,-1.75);
313  context->line_to(-9.0,-1.75);
314 
315  // penalty areas
316  context->move_to(9.0, 3.25);
317  context->line_to(6.75, 3.25);
318  context->line_to(6.75,-3.25);
319  context->line_to(9.0,-3.25);
320  context->move_to(-9.0, 3.25);
321  context->line_to(-6.75, 3.25);
322  context->line_to(-6.75,-3.25);
323  context->line_to(-9.0,-3.25);
324 
325  // marks
326  context->move_to(0.0, 0.0);
327  context->arc(0.0, 0.0, 0.05, 0.0, 2.0 * M_PI);
328  context->move_to(6.0, 0.0);
329  context->arc(6.0, 0.0, 0.05, 0.0, 2.0 * M_PI);
330  context->move_to(-6.0, 0.0);
331  context->arc(-6.0, 0.0, 0.05, 0.0, 2.0 * M_PI);
332  context->stroke();
333 
334  context->restore();
335 }
336 
337 void
338 FieldView::draw_robot( Cairo::RefPtr<Cairo::Context> context,
339  float x,
340  float y,
341  float ori,
342  Glib::ustring name )
343 {
344  context->save();
345  context->set_source_rgb(0.2, 0.2, 0.2);
346  context->set_line_width(0.05);
347  context->move_to(x, y);
348  context->arc(x, y, 0.3, ori, 2*M_PI + ori);
349  context->stroke();
350  context->restore();
351 
352  context->save();
353  context->select_font_face( "Sans",
354  Cairo::FONT_SLANT_NORMAL,
355  Cairo::FONT_WEIGHT_NORMAL );
356  context->set_font_size( 4 );
357  context->scale(0.1, -0.1);
358 
359  Cairo::TextExtents extents;
360  context->get_text_extents( name.c_str(), extents );
361 
362  context->move_to( 10 * x - extents.width/2.0 - extents.x_bearing,
363  -10 * y - extents.height/2.0 - extents.y_bearing + 8 );
364  context->show_text( name.c_str() );
365 
366  char* pos;
367  if (asprintf( &pos, "%.2f, %.2f [%.2f]", x, y, ori ) != -1) {
368  context->get_text_extents( pos, extents );
369 
370  context->move_to( 10 * x - extents.width/2.0 - extents.x_bearing,
371  -10 * y - extents.height/2.0 - extents.y_bearing + 12 );
372  context->show_text( pos );
373  }
374 
375  context->stroke();
376  context->restore();
377 
378  free( pos );
379 }
380 
381 void
382 FieldView::draw_obstacle(Cairo::RefPtr<Cairo::Context> context, float x, float y, float extend)
383 {
384  context->save();
385  context->set_source_rgba(0.0, 0.0, 1.0, 0.6);
386  context->set_line_width(0.05);
387  context->arc(x, y, 0.25 /*extend*/, 0.0, 2.0 * M_PI);
388  context->stroke();
389  context->restore();
390 }
391 
392 void
393 FieldView::draw_ball( Cairo::RefPtr<Cairo::Context> context,
394  float ball_x, float ball_y,
395  float bot_x, float bot_y )
396 {
397  context->save();
398  context->set_source_rgb(1.0, 0.3, 0.0);
399  context->set_line_width(0.05);
400  context->move_to(bot_x, bot_y);
401  context->line_to(ball_x, ball_y);
402  context->stroke();
403  context->arc(ball_x, ball_y, 0.15, 0.0, 2.0 * M_PI);
404  context->fill();
405  context->restore();
406 }
Data container to store and exchange worldinfo data.
virtual ~FieldView()
Destructor.
Definition: field_view.cpp:60
virtual bool on_draw(const Cairo::RefPtr< Cairo::Context > &context)
Overloaded signal handler.
Definition: field_view.cpp:141
virtual float y() const
RO-getter for y.
Definition: hom_coord.cpp:115
A homogeneous representation of a polar coordinate.
Definition: hom_polar.h:31
A 2-dimensional pose, i.e.
Definition: hom_pose_2d.h:33
A homogeneous point.
Definition: hom_point.h:33
bool toggle_show_pose(Glib::ustring name)
Toggle whether to show the pose of the specified robot.
Definition: field_view.cpp:69
float yaw() const
Get the angle of the current orientation [0...2pi].
void remove_host(Glib::ustring name)
Remove a host.
Definition: field_view.cpp:128
float x() const
Get the x-coordinate of the position.
bool toggle_show_opponents(Glib::ustring name)
Toggle whether to show the opponents seen by the specified robot.
Definition: field_view.cpp:109
virtual float x() const
RO-getter for x.
Definition: hom_coord.cpp:85
bool toggle_show_ball(Glib::ustring name)
Toggle whether to show the ball detected by the specified robot.
Definition: field_view.cpp:89
float y() const
Get the y-coordinate of the position.
FieldView(fawkes::WorldInfoDataContainer *data, bool show_pose=true, bool show_ball=true, bool show_opponents=false)
Constructor.
Definition: field_view.cpp:47