00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "field_view.h"
00024
00025 #include <worldinfo_utils/data_container.h>
00026 #include <cairomm/context.h>
00027
00028 #include <map>
00029 #include <cstdio>
00030
00031 using namespace std;
00032 using namespace fawkes;
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047 FieldView::FieldView( WorldInfoDataContainer* data,
00048 bool show_pose,
00049 bool show_ball,
00050 bool show_opponents )
00051 {
00052 m_show_pose_default = show_pose;
00053 m_show_ball_default = show_ball;
00054 m_show_opponents_default = show_opponents;
00055
00056 m_data_container = data;
00057 }
00058
00059
00060 FieldView::~FieldView()
00061 {
00062 }
00063
00064
00065
00066
00067
00068 bool
00069 FieldView::toggle_show_pose( Glib::ustring name )
00070 {
00071 std::map< Glib::ustring, bool >::iterator iter = m_show_pose.find( name );
00072 if ( iter != m_show_pose.end() )
00073 {
00074 iter->second = iter->second ? false : true;
00075 return iter->second;
00076 }
00077 else
00078 {
00079 m_show_pose[ name ] = m_show_pose_default;
00080 return m_show_pose_default;
00081 }
00082 }
00083
00084
00085
00086
00087
00088 bool
00089 FieldView::toggle_show_ball( Glib::ustring name )
00090 {
00091 std::map< Glib::ustring, bool >::iterator iter = m_show_ball.find( name );
00092 if ( iter != m_show_ball.end() )
00093 {
00094 iter->second = iter->second ? false : true;
00095 return iter->second;
00096 }
00097 else
00098 {
00099 m_show_ball[ name ] = m_show_ball_default;
00100 return m_show_ball_default;
00101 }
00102 }
00103
00104
00105
00106
00107
00108 bool
00109 FieldView::toggle_show_opponents( Glib::ustring name )
00110 {
00111 std::map< Glib::ustring, bool >::iterator iter = m_show_opponents.find( name );
00112 if ( iter != m_show_opponents.end() )
00113 {
00114 iter->second = iter->second ? false : true;
00115 return iter->second;
00116 }
00117 else
00118 {
00119 m_show_opponents[ name ] = m_show_opponents_default;
00120 return m_show_opponents_default;
00121 }
00122 }
00123
00124
00125
00126
00127 void
00128 FieldView::remove_host( Glib::ustring name )
00129 {
00130 m_show_pose.erase( name );
00131 m_show_ball.erase( name );
00132 m_show_opponents.erase( name );
00133 }
00134
00135
00136
00137
00138
00139
00140 bool
00141 FieldView::on_expose_event(GdkEventExpose* event)
00142 {
00143 Glib::RefPtr<Gdk::Window> window = get_window();
00144
00145 if (window)
00146 {
00147 Gtk::Allocation allocation = get_allocation();
00148 const int width = allocation.get_width();
00149 const int height = allocation.get_height();
00150
00151 Cairo::RefPtr<Cairo::Context> context = window->create_cairo_context();
00152
00153 if (event)
00154 {
00155 context->rectangle( event->area.x, event->area.y,
00156 event->area.width, event->area.height );
00157 context->clip();
00158 }
00159
00160
00161 float unit;
00162 if ( (width / 22.0) <= (height / 16.0) )
00163 { unit = width / 22.0; }
00164 else
00165 { unit = height / 16.0; }
00166
00167 context->translate( width / 2.0, height / 2.0 );
00168 context->scale(unit, -unit);
00169
00170 draw_field_msl(context);
00171
00172 list<string> hosts = m_data_container->get_hosts();
00173
00174 for ( list<string>::iterator i = hosts.begin();
00175 i != hosts.end();
00176 ++i )
00177 {
00178 const char* host = i->c_str();
00179
00180 HomPose2d pose;
00181 HomPoint ball_pos;
00182 std::map<unsigned int, HomPoint> opp_positions;
00183
00184 bool show_pose;
00185 bool show_ball;
00186 bool show_opponents;
00187 std::map< Glib::ustring, bool >::iterator iter;
00188
00189 iter = m_show_pose.find( *i );
00190 if ( iter == m_show_pose.end() )
00191 { show_pose = m_show_pose_default; }
00192 else
00193 { show_pose = iter->second; }
00194
00195 iter = m_show_ball.find( *i );
00196 if ( iter == m_show_ball.end() )
00197 { show_ball = m_show_ball_default; }
00198 else
00199 { show_ball = iter->second; }
00200
00201 iter = m_show_opponents.find( *i );
00202 if ( iter == m_show_opponents.end() )
00203 { show_opponents = m_show_opponents_default; }
00204 else
00205 { show_opponents = iter->second; }
00206
00207
00208 if ( m_data_container->get_robot_pose( host, pose ) )
00209 {
00210 if ( show_pose )
00211 { draw_robot( context, pose.x(), pose.y(), pose.yaw(), host ); }
00212
00213 if ( m_data_container->get_ball_pos_global( host, ball_pos ) &&
00214 show_ball )
00215 {
00216 draw_ball( context, ball_pos.x(), ball_pos.y(),
00217 pose.x(), pose.y() );
00218 }
00219
00220
00221
00222
00223
00224
00225 }
00226 else
00227 {
00228 HomPolar ball_pos;
00229 if ( m_data_container->get_ball_pos_relative( host, ball_pos ) &&
00230 show_ball )
00231 {
00232 draw_ball( context, ball_pos.x(), ball_pos.y(), 0.0, 0.0 );
00233 }
00234 }
00235
00236
00237 std::map<unsigned int, HomPoint> opponents;
00238 if ( m_data_container->get_opponent_pos( host, opponents ) &&
00239 show_opponents )
00240 {
00241 for ( std::map<unsigned int, HomPoint>::iterator i = opponents.begin();
00242 i != opponents.end();
00243 ++i )
00244 {
00245 HomPoint p = i->second;
00246 draw_obstacle( context, p.x(), p.y(), 0.2 );
00247 }
00248 }
00249 }
00250 }
00251
00252 return true;
00253 }
00254
00255 void
00256 FieldView::draw_field_msl(Cairo::RefPtr<Cairo::Context> context)
00257 {
00258 context->save();
00259
00260
00261 context->save();
00262 context->set_source_rgb(48.0 / 255.0, 215.0 / 255.0, 31.0 / 255.0);
00263 context->paint();
00264 context->restore();
00265
00266 context->save();
00267 context->set_line_width(0.125);
00268 context->set_source_rgb(1.0, 1.0, 1.0);
00269
00270
00271 context->save();
00272 context->set_source_rgb(237.0 / 255.0, 240.0 / 255.0, 12.0 / 255.0);
00273 context->move_to(9.0, 1.0625);
00274 context->line_to(9.5, 1.0625);
00275 context->line_to(9.5,-1.0625);
00276 context->line_to(9.0,-1.0625);
00277 context->stroke();
00278 context->restore();
00279
00280 context->save();
00281 context->set_source_rgb(12.0 / 255.0, 14.0 / 255.0, 240.0 / 255.0);
00282 context->move_to(-9.0, 1.0625);
00283 context->line_to(-9.5, 1.0625);
00284 context->line_to(-9.5,-1.0625);
00285 context->line_to(-9.0,-1.0625);
00286 context->stroke();
00287 context->restore();
00288
00289
00290 context->arc(0.0, 0.0, 2.0, 0.0, 2 * M_PI);
00291
00292
00293 context->move_to(9.0, 6.0);
00294 context->arc(9.0, 6.0, 0.75, M_PI, -M_PI/2.0);
00295 context->move_to(9.0,-6.0);
00296 context->arc(9.0, -6.0, 0.75, M_PI/2.0, M_PI);
00297 context->move_to(-9.0, -6.0);
00298 context->arc(-9.0, -6.0, 0.75, 0.0, M_PI/2.0);
00299 context->move_to(-9.0, 6.0);
00300 context->arc(-9.0, 6.0, 0.75, -M_PI/2.0, 0.0);
00301
00302
00303 context->save();
00304 context->set_line_cap(Cairo::LINE_CAP_SQUARE);
00305 context->move_to( 0.0, 6.0);
00306 context->line_to( 0.0,-6.0);
00307 context->move_to( 9.0, 6.0);
00308 context->line_to( 9.0,-6.0);
00309 context->line_to(-9.0,-6.0);
00310 context->line_to(-9.0, 6.0);
00311 context->close_path();
00312 context->restore();
00313
00314
00315 context->move_to(9.0, 1.75);
00316 context->line_to(8.25, 1.75);
00317 context->line_to(8.25,-1.75);
00318 context->line_to(9.0,-1.75);
00319 context->move_to(-9.0, 1.75);
00320 context->line_to(-8.25, 1.75);
00321 context->line_to(-8.25,-1.75);
00322 context->line_to(-9.0,-1.75);
00323
00324
00325 context->move_to(9.0, 3.25);
00326 context->line_to(6.75, 3.25);
00327 context->line_to(6.75,-3.25);
00328 context->line_to(9.0,-3.25);
00329 context->move_to(-9.0, 3.25);
00330 context->line_to(-6.75, 3.25);
00331 context->line_to(-6.75,-3.25);
00332 context->line_to(-9.0,-3.25);
00333
00334
00335 context->move_to(0.0, 0.0);
00336 context->arc(0.0, 0.0, 0.05, 0.0, 2.0 * M_PI);
00337 context->move_to(6.0, 0.0);
00338 context->arc(6.0, 0.0, 0.05, 0.0, 2.0 * M_PI);
00339 context->move_to(-6.0, 0.0);
00340 context->arc(-6.0, 0.0, 0.05, 0.0, 2.0 * M_PI);
00341 context->stroke();
00342
00343 context->restore();
00344 }
00345
00346 void
00347 FieldView::draw_robot( Cairo::RefPtr<Cairo::Context> context,
00348 float x,
00349 float y,
00350 float ori,
00351 Glib::ustring name )
00352 {
00353 context->save();
00354 context->set_source_rgb(0.2, 0.2, 0.2);
00355 context->set_line_width(0.05);
00356 context->move_to(x, y);
00357 context->arc(x, y, 0.3, ori, 2*M_PI + ori);
00358 context->stroke();
00359 context->restore();
00360
00361 context->save();
00362 context->select_font_face( "Sans",
00363 Cairo::FONT_SLANT_NORMAL,
00364 Cairo::FONT_WEIGHT_NORMAL );
00365 context->set_font_size( 4 );
00366 context->scale(0.1, -0.1);
00367
00368 Cairo::TextExtents extents;
00369 context->get_text_extents( name.c_str(), extents );
00370
00371 context->move_to( 10 * x - extents.width/2.0 - extents.x_bearing,
00372 -10 * y - extents.height/2.0 - extents.y_bearing + 8 );
00373 context->show_text( name.c_str() );
00374
00375 char* pos;
00376 if (asprintf( &pos, "%.2f, %.2f [%.2f]", x, y, ori ) != -1) {
00377 context->get_text_extents( pos, extents );
00378
00379 context->move_to( 10 * x - extents.width/2.0 - extents.x_bearing,
00380 -10 * y - extents.height/2.0 - extents.y_bearing + 12 );
00381 context->show_text( pos );
00382 }
00383
00384 context->stroke();
00385 context->restore();
00386
00387 free( pos );
00388 }
00389
00390 void
00391 FieldView::draw_obstacle(Cairo::RefPtr<Cairo::Context> context, float x, float y, float extend)
00392 {
00393 context->save();
00394 context->set_source_rgba(0.0, 0.0, 1.0, 0.6);
00395 context->set_line_width(0.05);
00396 context->arc(x, y, 0.25 , 0.0, 2.0 * M_PI);
00397 context->stroke();
00398 context->restore();
00399 }
00400
00401 void
00402 FieldView::draw_ball( Cairo::RefPtr<Cairo::Context> context,
00403 float ball_x, float ball_y,
00404 float bot_x, float bot_y )
00405 {
00406 context->save();
00407 context->set_source_rgb(1.0, 0.3, 0.0);
00408 context->set_line_width(0.05);
00409 context->move_to(bot_x, bot_y);
00410 context->line_to(ball_x, ball_y);
00411 context->stroke();
00412 context->arc(ball_x, ball_y, 0.15, 0.0, 2.0 * M_PI);
00413 context->fill();
00414 context->restore();
00415 }