Fawkes API Fawkes Development Version

field_view.cpp

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 event the event that triggered the signal
00138  * @return always true
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     // setup
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         // TODO
00221 //      if ( m_data_container->get_opponenet_pos( host, opp_positions ) &&
00222 //           show_opponents )
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       // opponents are always global
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   // background
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   // goals
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   // center circle
00290   context->arc(0.0, 0.0, 2.0, 0.0, 2 * M_PI);
00291   
00292   // corner arcs
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   // lines
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   // goal areas
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   // penalty areas
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   // marks
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 /*extend*/, 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 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends