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 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 }