Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * field_view.cpp - Draws the field and the robots on it 00004 * 00005 * Created: Wed April 09 21:00:49 2008 00006 * Copyright 2008 Daniel Beck 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 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 /** @class FieldView <tools/worldinfo_viewer/field_view.h> 00035 * Drawing widget that draws an (MSL-) soccer field with robots, 00036 * opponents, and balls. 00037 * @author Daniel Beck 00038 */ 00039 00040 /** Constructor. 00041 * @param show_pose default value for show pose 00042 * @param show_ball default value for show ball 00043 * @param show_opponents default value for show opponents 00044 * @param data pointer to a WorldInfoDataContainer that is used as the 00045 * data source 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 /** Destructor. */ 00060 FieldView::~FieldView() 00061 { 00062 } 00063 00064 /** Toggle whether to show the pose of the specified robot. 00065 * @param name the hostname of the robot 00066 * @return true if the pose wasn't shown before, false otherwise 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 /** Toggle whether to show the ball detected by the specified robot. 00085 * @param name the hostname of the robot 00086 * @return true if the ball wasn't shown before, false otherwise 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 /** Toggle whether to show the opponents seen by the specified robot. 00105 * @param name the hostname of the robot 00106 * @return true if the opponents weren't shown before, false otherwise 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 /** Remove a host. 00125 * @param name the name of the host to be removed. 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 /** Overloaded signal handler. 00136 * This is were the drawing action happens. 00137 * @param context cairo context for drawing 00138 * @return always true 00139 */ 00140 bool 00141 FieldView::on_draw(const Cairo::RefPtr<Cairo::Context> &context) 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 // setup 00152 float unit; 00153 if ( (width / 22.0) <= (height / 16.0) ) 00154 { unit = width / 22.0; } 00155 else 00156 { unit = height / 16.0; } 00157 00158 context->translate( width / 2.0, height / 2.0 ); 00159 context->scale(unit, -unit); 00160 00161 draw_field_msl(context); 00162 00163 list<string> hosts = m_data_container->get_hosts(); 00164 00165 for ( list<string>::iterator i = hosts.begin(); 00166 i != hosts.end(); 00167 ++i ) 00168 { 00169 const char* host = i->c_str(); 00170 00171 HomPose2d pose; 00172 HomPoint ball_pos; 00173 std::map<unsigned int, HomPoint> opp_positions; 00174 00175 bool show_pose; 00176 bool show_ball; 00177 bool show_opponents; 00178 std::map< Glib::ustring, bool >::iterator iter; 00179 00180 iter = m_show_pose.find( *i ); 00181 if ( iter == m_show_pose.end() ) 00182 { show_pose = m_show_pose_default; } 00183 else 00184 { show_pose = iter->second; } 00185 00186 iter = m_show_ball.find( *i ); 00187 if ( iter == m_show_ball.end() ) 00188 { show_ball = m_show_ball_default; } 00189 else 00190 { show_ball = iter->second; } 00191 00192 iter = m_show_opponents.find( *i ); 00193 if ( iter == m_show_opponents.end() ) 00194 { show_opponents = m_show_opponents_default; } 00195 else 00196 { show_opponents = iter->second; } 00197 00198 00199 if ( m_data_container->get_robot_pose( host, pose ) ) 00200 { 00201 if ( show_pose ) 00202 { draw_robot( context, pose.x(), pose.y(), pose.yaw(), host ); } 00203 00204 if ( m_data_container->get_ball_pos_global( host, ball_pos ) && 00205 show_ball ) 00206 { 00207 draw_ball( context, ball_pos.x(), ball_pos.y(), 00208 pose.x(), pose.y() ); 00209 } 00210 00211 // TODO 00212 // if ( m_data_container->get_opponenet_pos( host, opp_positions ) && 00213 // show_opponents ) 00214 // { 00215 // } 00216 } 00217 else 00218 { 00219 HomPolar ball_pos; 00220 if ( m_data_container->get_ball_pos_relative( host, ball_pos ) && 00221 show_ball ) 00222 { 00223 draw_ball( context, ball_pos.x(), ball_pos.y(), 0.0, 0.0 ); 00224 } 00225 } 00226 00227 // opponents are always global 00228 std::map<unsigned int, HomPoint> opponents; 00229 if ( m_data_container->get_opponent_pos( host, opponents ) && 00230 show_opponents ) 00231 { 00232 for ( std::map<unsigned int, HomPoint>::iterator i = opponents.begin(); 00233 i != opponents.end(); 00234 ++i ) 00235 { 00236 HomPoint p = i->second; 00237 draw_obstacle( context, p.x(), p.y(), 0.2 ); 00238 } 00239 } 00240 } 00241 } 00242 00243 return true; 00244 } 00245 00246 void 00247 FieldView::draw_field_msl(Cairo::RefPtr<Cairo::Context> context) 00248 { 00249 context->save(); 00250 00251 // background 00252 context->save(); 00253 context->set_source_rgb(48.0 / 255.0, 215.0 / 255.0, 31.0 / 255.0); 00254 context->paint(); 00255 context->restore(); 00256 00257 context->save(); 00258 context->set_line_width(0.125); 00259 context->set_source_rgb(1.0, 1.0, 1.0); 00260 00261 // goals 00262 context->save(); 00263 context->set_source_rgb(237.0 / 255.0, 240.0 / 255.0, 12.0 / 255.0); 00264 context->move_to(9.0, 1.0625); 00265 context->line_to(9.5, 1.0625); 00266 context->line_to(9.5,-1.0625); 00267 context->line_to(9.0,-1.0625); 00268 context->stroke(); 00269 context->restore(); 00270 00271 context->save(); 00272 context->set_source_rgb(12.0 / 255.0, 14.0 / 255.0, 240.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 // center circle 00281 context->arc(0.0, 0.0, 2.0, 0.0, 2 * M_PI); 00282 00283 // corner arcs 00284 context->move_to(9.0, 6.0); 00285 context->arc(9.0, 6.0, 0.75, M_PI, -M_PI/2.0); 00286 context->move_to(9.0,-6.0); 00287 context->arc(9.0, -6.0, 0.75, M_PI/2.0, M_PI); 00288 context->move_to(-9.0, -6.0); 00289 context->arc(-9.0, -6.0, 0.75, 0.0, M_PI/2.0); 00290 context->move_to(-9.0, 6.0); 00291 context->arc(-9.0, 6.0, 0.75, -M_PI/2.0, 0.0); 00292 00293 // lines 00294 context->save(); 00295 context->set_line_cap(Cairo::LINE_CAP_SQUARE); 00296 context->move_to( 0.0, 6.0); 00297 context->line_to( 0.0,-6.0); 00298 context->move_to( 9.0, 6.0); 00299 context->line_to( 9.0,-6.0); 00300 context->line_to(-9.0,-6.0); 00301 context->line_to(-9.0, 6.0); 00302 context->close_path(); 00303 context->restore(); 00304 00305 // goal areas 00306 context->move_to(9.0, 1.75); 00307 context->line_to(8.25, 1.75); 00308 context->line_to(8.25,-1.75); 00309 context->line_to(9.0,-1.75); 00310 context->move_to(-9.0, 1.75); 00311 context->line_to(-8.25, 1.75); 00312 context->line_to(-8.25,-1.75); 00313 context->line_to(-9.0,-1.75); 00314 00315 // penalty areas 00316 context->move_to(9.0, 3.25); 00317 context->line_to(6.75, 3.25); 00318 context->line_to(6.75,-3.25); 00319 context->line_to(9.0,-3.25); 00320 context->move_to(-9.0, 3.25); 00321 context->line_to(-6.75, 3.25); 00322 context->line_to(-6.75,-3.25); 00323 context->line_to(-9.0,-3.25); 00324 00325 // marks 00326 context->move_to(0.0, 0.0); 00327 context->arc(0.0, 0.0, 0.05, 0.0, 2.0 * M_PI); 00328 context->move_to(6.0, 0.0); 00329 context->arc(6.0, 0.0, 0.05, 0.0, 2.0 * M_PI); 00330 context->move_to(-6.0, 0.0); 00331 context->arc(-6.0, 0.0, 0.05, 0.0, 2.0 * M_PI); 00332 context->stroke(); 00333 00334 context->restore(); 00335 } 00336 00337 void 00338 FieldView::draw_robot( Cairo::RefPtr<Cairo::Context> context, 00339 float x, 00340 float y, 00341 float ori, 00342 Glib::ustring name ) 00343 { 00344 context->save(); 00345 context->set_source_rgb(0.2, 0.2, 0.2); 00346 context->set_line_width(0.05); 00347 context->move_to(x, y); 00348 context->arc(x, y, 0.3, ori, 2*M_PI + ori); 00349 context->stroke(); 00350 context->restore(); 00351 00352 context->save(); 00353 context->select_font_face( "Sans", 00354 Cairo::FONT_SLANT_NORMAL, 00355 Cairo::FONT_WEIGHT_NORMAL ); 00356 context->set_font_size( 4 ); 00357 context->scale(0.1, -0.1); 00358 00359 Cairo::TextExtents extents; 00360 context->get_text_extents( name.c_str(), extents ); 00361 00362 context->move_to( 10 * x - extents.width/2.0 - extents.x_bearing, 00363 -10 * y - extents.height/2.0 - extents.y_bearing + 8 ); 00364 context->show_text( name.c_str() ); 00365 00366 char* pos; 00367 if (asprintf( &pos, "%.2f, %.2f [%.2f]", x, y, ori ) != -1) { 00368 context->get_text_extents( pos, extents ); 00369 00370 context->move_to( 10 * x - extents.width/2.0 - extents.x_bearing, 00371 -10 * y - extents.height/2.0 - extents.y_bearing + 12 ); 00372 context->show_text( pos ); 00373 } 00374 00375 context->stroke(); 00376 context->restore(); 00377 00378 free( pos ); 00379 } 00380 00381 void 00382 FieldView::draw_obstacle(Cairo::RefPtr<Cairo::Context> context, float x, float y, float extend) 00383 { 00384 context->save(); 00385 context->set_source_rgba(0.0, 0.0, 1.0, 0.6); 00386 context->set_line_width(0.05); 00387 context->arc(x, y, 0.25 /*extend*/, 0.0, 2.0 * M_PI); 00388 context->stroke(); 00389 context->restore(); 00390 } 00391 00392 void 00393 FieldView::draw_ball( Cairo::RefPtr<Cairo::Context> context, 00394 float ball_x, float ball_y, 00395 float bot_x, float bot_y ) 00396 { 00397 context->save(); 00398 context->set_source_rgb(1.0, 0.3, 0.0); 00399 context->set_line_width(0.05); 00400 context->move_to(bot_x, bot_y); 00401 context->line_to(ball_x, ball_y); 00402 context->stroke(); 00403 context->arc(ball_x, ball_y, 0.15, 0.0, 2.0 * M_PI); 00404 context->fill(); 00405 context->restore(); 00406 }