Fawkes API Fawkes Development Version

laser_drawing_area.cpp

00001 
00002 /***************************************************************************
00003  *  laser_drawing_area.cpp - Laser drawing area derived from Gtk::DrawingArea
00004  *
00005  *  Created: Thu Oct 09 18:20:21 2008
00006  *  Copyright  2008-2010  Tim Niemueller [www.niemueller.de]
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 "laser_drawing_area.h"
00024 #include "visdisplay.h"
00025 #include <interfaces/Laser720Interface.h>
00026 #include <interfaces/Laser360Interface.h>
00027 #include <interfaces/ObjectPositionInterface.h>
00028 #include <interfaces/VisualDisplay2DInterface.h>
00029 #include <utils/math/angle.h>
00030 #include <gui_utils/robot/drawer.h>
00031 #include <algorithm>
00032 #include <utils/misc/string_conversions.h>
00033 
00034 //#define LASERGUI_DEBUG_PRINT_TRACKS
00035 #define CFG_PRINT_NR_TRACKELEMENTS 5
00036 
00037 using namespace fawkes;
00038 
00039 /** @class LaserDrawingArea "laser_drawing_area.h"
00040  * Laser drawing area.
00041  * Derived version of Gtk::DrawingArea that renders values of a laser interface.
00042  * @author Tim Niemueller
00043  */
00044 
00045 #ifdef HAVE_GLADEMM
00046 /** Constructor.
00047  * Special ctor to be used with Glade's get_widget_derived().
00048  * @param cobject Gtk C object
00049  * @param refxml Glade's XML reference
00050  */
00051 LaserDrawingArea::LaserDrawingArea(BaseObjectType* cobject,
00052                                    const Glib::RefPtr<Gnome::Glade::Xml>& refxml)
00053   : Gtk::DrawingArea(cobject)
00054 {
00055   __draw_mode = MODE_LINES;
00056   __zoom_factor = 50;
00057   __laser360_if = NULL;
00058   __laser720_if = NULL;
00059   __l_objpos_if_persons = NULL;
00060   __l_objpos_if_legs = NULL;
00061   __l_objpos_if_misc = NULL;
00062   __laser_segmentation_if = NULL;
00063   __l_track_if = NULL;
00064   __target_if = NULL;
00065   __switch_if = NULL;
00066   __line_if   = NULL;
00067   __visdisp_if = NULL;
00068   __robot_drawer = NULL;
00069   __resolution = 1;
00070   __rotation = 0;
00071   __break_drawing = false;
00072   __first_draw = true;
00073 
00074   __visdisp = new VisualDisplay2D();
00075 
00076   add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK |
00077              Gdk::BUTTON_PRESS_MASK );
00078 
00079 #ifndef GLIBMM_DEFAULT_SIGNAL_HANDLERS_ENABLED
00080   signal_expose_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_expose_event));
00081   signal_button_press_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_button_press_event));
00082   signal_motion_notify_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_motion_notify_event));
00083 #endif
00084 
00085   //Glib::RefPtr<Gdk::Window> window = get_window();
00086 }
00087 #endif
00088 
00089 /** Constructor. */
00090 LaserDrawingArea::LaserDrawingArea()
00091 {
00092   __draw_mode = MODE_LINES;
00093   __zoom_factor = 50;
00094   __laser360_if = NULL;
00095   __laser720_if = NULL;
00096   __l_objpos_if_persons = NULL;
00097   __l_objpos_if_legs = NULL;
00098   __l_objpos_if_misc = NULL;
00099   __laser_segmentation_if = NULL;
00100   __l_track_if = NULL;
00101   __target_if = NULL;
00102   __switch_if = NULL;
00103   __line_if   = NULL;
00104   __visdisp_if = NULL;
00105   __robot_drawer = NULL;
00106   __resolution = 1;
00107   __rotation = 0;
00108   __break_drawing = false;
00109 
00110   __visdisp = new VisualDisplay2D();
00111 
00112   add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK);
00113 
00114 #ifndef GLIBMM_DEFAULT_SIGNAL_HANDLERS_ENABLED
00115   signal_expose_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_expose_event));
00116   signal_button_press_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_button_press_event));
00117   signal_motion_notify_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_motion_notify_event));
00118 #endif
00119 }
00120 
00121 
00122 /** Destructor. */
00123 LaserDrawingArea::~LaserDrawingArea()
00124 {
00125   delete __visdisp;
00126 }
00127 
00128 /** Set ObjectPosition interfaces.
00129  * @param  l_objpos_if_persons list of objectposition interfaces for persons
00130  * @param  l_objpos_if_legs list of objectposition interfaces for legs
00131  * @param l_objpos_if_misc list of objectposition interfaces for miscellanous objects
00132  * @param laser_segmentation_if Laser interface indicating the segmentation-borfers of the legtracker
00133  * @param l_track_if list of track interfaces
00134  * @param target_if the current target
00135  * @param switch_if used to indicate that a drawing-run is finish (so e.g. new data can be sent)
00136  */
00137 void
00138 LaserDrawingArea::set_objpos_if(std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_persons,
00139                                 std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_legs,
00140                                 std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_misc,
00141                                 fawkes::Laser720Interface* laser_segmentation_if ,
00142                                 std::list<fawkes::Position2DTrackInterface*>* l_track_if,
00143                                 fawkes::ObjectPositionInterface* target_if,
00144                                 fawkes::SwitchInterface* switch_if){
00145   __l_objpos_if_persons = l_objpos_if_persons;
00146   __l_objpos_if_legs = l_objpos_if_legs;
00147   __l_objpos_if_misc = l_objpos_if_misc;
00148   __laser_segmentation_if=laser_segmentation_if;
00149   __l_track_if = l_track_if;
00150   __target_if = target_if;
00151   __switch_if = switch_if;
00152 }
00153 
00154 
00155 
00156 /** Set 360 degree laser interface.
00157  * @param laser_if laser interface
00158  */
00159 void
00160 LaserDrawingArea::set_laser360_if(Laser360Interface *laser_if)
00161 {
00162   __laser360_if = laser_if;
00163   __laser720_if = NULL;
00164 }
00165 
00166 
00167 /** Set 720 degree laser interface.
00168  * @param laser_if laser interface
00169  */
00170 void
00171 LaserDrawingArea::set_laser720_if(Laser720Interface *laser_if)
00172 {
00173   __laser720_if = laser_if;
00174   __laser360_if = NULL;
00175 }
00176 
00177 
00178 /** Reset laser interfaces to "no laser available". */
00179 void
00180 LaserDrawingArea::reset_laser_ifs()
00181 {
00182   __laser360_if = NULL;
00183   __laser720_if = NULL;
00184   __l_objpos_if_persons = NULL;
00185   __l_objpos_if_legs = NULL;
00186   __l_objpos_if_misc = NULL;
00187   __laser_segmentation_if = NULL;
00188   __l_track_if = NULL;
00189   __target_if = NULL;
00190   __switch_if = NULL;
00191 
00192   Gtk::Allocation allocation = get_allocation();
00193   const int width  = allocation.get_width();
00194   const int height = allocation.get_height();
00195 
00196   __xc = width / 2;
00197   __yc = height / 2;
00198   __zoom_factor = 50;
00199 
00200 }
00201 
00202 /** Set line interface.
00203  * @param line_if interface to use for line data to draw.
00204  */
00205 void
00206 LaserDrawingArea::set_line_if(ObjectPositionInterface *line_if)
00207 {
00208   __line_if = line_if;
00209 }
00210 
00211 
00212 /** Set visual display interface.
00213  * @param visdisp_if interface to query for drawing ops
00214  */
00215 void
00216 LaserDrawingArea::set_visdisp_if(VisualDisplay2DInterface *visdisp_if)
00217 {
00218   __visdisp_if = visdisp_if;
00219   __visdisp->set_interface(__visdisp_if);
00220 }
00221 
00222 
00223 /** Set robot drawer.
00224  * @param robot_drawer new robot drawer to use
00225  */
00226 void
00227 LaserDrawingArea::set_robot_drawer(fawkes::CairoRobotDrawer *robot_drawer)
00228 {
00229   __robot_drawer = robot_drawer;
00230 }
00231 
00232 /** Set resolution.
00233  * Every n'th beam will be drawn where n is the resolution.
00234  * @param resolution new resolution
00235  */
00236 void
00237 LaserDrawingArea::set_resolution(unsigned int resolution)
00238 {
00239   __resolution = resolution;
00240 }
00241 
00242 
00243 /** Set the drawing mode.
00244  * @param mode the new drawing mode
00245  */
00246 void
00247 LaserDrawingArea::set_draw_mode(draw_mode_t mode)
00248 {
00249   __draw_mode = mode;
00250   queue_draw();
00251 }
00252 
00253 /** Zoom in.
00254  * Increases zoom factor by 20, no upper limit.
00255  */
00256 void
00257 LaserDrawingArea::zoom_in()
00258 {
00259   __zoom_factor += 20;
00260   queue_draw();
00261 }
00262 
00263 /** Zoom out.
00264  * Decreases zoom factor by 20 with a minimum of 1.
00265  */
00266 void
00267 LaserDrawingArea::zoom_out()
00268 {
00269   if ( __zoom_factor > 20 ) {
00270     __zoom_factor -= 20;
00271   } else {
00272     __zoom_factor = 1;
00273   }
00274   queue_draw();
00275 }
00276 
00277 
00278 /** Set rotation.
00279  * @param rot_rad rotation angle in rad
00280  */
00281 void
00282 LaserDrawingArea::set_rotation(float rot_rad)
00283 {
00284   __rotation = rot_rad;
00285 }
00286 
00287 
00288 /** Expose event handler.
00289  * @param event event info structure.
00290  * @return signal return value
00291  */
00292 bool
00293 LaserDrawingArea::on_expose_event(GdkEventExpose* event)
00294 {
00295   // This is where we draw on the window
00296   Glib::RefPtr<Gdk::Window> window = get_window();
00297   if(window) {
00298     Gtk::Allocation allocation = get_allocation();
00299 
00300     if(__first_draw)
00301     {
00302       __first_draw = false;
00303       const int width = allocation.get_width();
00304       const int height = allocation.get_height();
00305     
00306       // coordinates for the center of the window
00307       __xc = width / 2;
00308       __yc = height / 2;
00309     }
00310     Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
00311     cr->set_line_width(1.0);
00312 
00313     // clip to the area indicated by the expose event so that we only redraw
00314     // the portion of the window that needs to be redrawn
00315     cr->rectangle(event->area.x, event->area.y,
00316                   event->area.width, event->area.height);
00317     cr->set_source_rgb(1, 1, 1);
00318     cr->fill_preserve();
00319     cr->clip();
00320     cr->set_source_rgb(0, 0, 0);
00321     //cr->set_source_rgba(0,0,0,1);
00322 
00323     //    __last_xc += __translation_x;
00324     //    __last_yc += __translation_y;
00325     cr->translate(__xc, __yc);
00326   
00327     cr->save();
00328     if ( (__laser360_if == NULL) && (__laser720_if == NULL) ) {
00329       Cairo::TextExtents te;
00330       std::string t = "Not connected to BlackBoard";
00331       cr->set_source_rgb(1, 0, 0);
00332       cr->set_font_size(20);
00333       cr->get_text_extents(t, te);
00334       cr->move_to(- te.width / 2, -te.height / 2);
00335       cr->show_text(t);
00336     } else if ( (__laser360_if && ! __laser360_if->has_writer()) ||
00337                 (__laser720_if && ! __laser720_if->has_writer()) ) {
00338       Cairo::TextExtents te;
00339       std::string t = "No writer for 360° laser interface";
00340       if (__laser720_if) t = "No writer for 720° laser interface";
00341       cr->set_source_rgb(1, 0, 0);
00342       cr->set_font_size(20);
00343       cr->get_text_extents(t, te);
00344       cr->move_to(- te.width / 2, -te.height / 2);
00345       cr->show_text(t);
00346     } else {
00347 
00348       if (! __break_drawing) {
00349         if (__laser360_if)  __laser360_if->read();
00350         if (__laser720_if)  __laser720_if->read();
00351       }
00352 
00353       draw_beams(window, cr);
00354       if (__robot_drawer)  __robot_drawer->draw_robot(window, cr);
00355       draw_segments(window, cr);
00356       draw_persons_legs(window, cr);
00357 
00358       if(__switch_if != NULL && __switch_if->has_writer()){
00359         SwitchInterface::EnableSwitchMessage *esm = new SwitchInterface::EnableSwitchMessage();
00360         __switch_if->msgq_enqueue(esm);
00361       }
00362     }
00363     cr->restore();
00364 
00365     cr->save();
00366     cr->rotate(0.5 * M_PI + __rotation);
00367     cr->scale(-__zoom_factor, __zoom_factor);
00368     cr->set_line_width(1. / __zoom_factor);
00369     if (__visdisp_if) {
00370       __visdisp->process_messages();
00371       __visdisp->draw(cr);
00372     }
00373 
00374     const float radius = 0.01;
00375     if (__line_if) {
00376       __line_if->read();
00377       if (__line_if->has_writer() &&
00378           __line_if->is_valid() && __line_if->is_visible()) {
00379 
00380         cr->set_source_rgb(1, 0, 0);
00381         /*
00382         std::vector<double> dashes(1);
00383         dashes[0] = 0.1;
00384         cr->set_dash(dashes, 0);
00385         */
00386         cr->rectangle(__line_if->world_x() - radius * 0.5, __line_if->world_y() - radius * 0.5, radius, radius);
00387         cr->rectangle(__line_if->relative_x() - radius * 0.5, __line_if->relative_y() - radius * 0.5, radius, radius);
00388         cr->fill_preserve();
00389         cr->stroke();
00390         cr->move_to(__line_if->world_x(), __line_if->world_y());
00391         cr->line_to(__line_if->relative_x(), __line_if->relative_y());
00392         cr->stroke();
00393       }
00394     }
00395     cr->restore();
00396   }
00397 
00398   return true;
00399 }
00400 
00401 
00402 /** Draw scale box.
00403  * Draws a circle with a radius of 1m around the robot.
00404  * @param window Gdk window
00405  * @param cr Cairo context to draw to. It is assumed that possible transformations
00406  * have been setup before.
00407  */
00408 void
00409 LaserDrawingArea::draw_scalebox(Glib::RefPtr<Gdk::Window> &window,
00410                                 Cairo::RefPtr<Cairo::Context> &cr)
00411 {
00412   cr->save();
00413   cr->set_source_rgba(0, 0, 0.8, 0.2);
00414   cr->arc(0, 0, 1.0, 0, 2 * M_PI);
00415   cr->stroke();
00416   cr->restore();
00417 }
00418 
00419 
00420 /** Draw Beams.
00421  * Draws the beams as lines, circles or hull, depending on draw mode.
00422  * @param window Gdk window
00423  * @param cr Cairo context to draw to. It is assumed that possible transformations
00424  * have been setup before.
00425  */
00426 void
00427 LaserDrawingArea::draw_beams(Glib::RefPtr<Gdk::Window> &window,
00428                              Cairo::RefPtr<Cairo::Context> &cr)
00429 {
00430   float *distances = __laser360_if ? __laser360_if->distances() : __laser720_if->distances();
00431   size_t nd = __laser360_if ? __laser360_if->maxlenof_distances() : __laser720_if->maxlenof_distances();
00432   const float nd_factor = 360.0 / nd;
00433 
00434   bool clockwise = __laser360_if ? __laser360_if->is_clockwise_angle() : __laser720_if->is_clockwise_angle();
00435 
00436   float *revdists = NULL;
00437   if (! clockwise) {
00438     // re-arrange to clockwise
00439     revdists = (float *)new float[nd];
00440     for (size_t i = 0; i < nd; ++i) {
00441       revdists[nd - i] = distances[i];
00442     }
00443     distances = revdists;
00444   }
00445 
00446   cr->scale(__zoom_factor, __zoom_factor);
00447   cr->rotate(__rotation);
00448   cr->set_line_width(1. / __zoom_factor);
00449 
00450   draw_scalebox(window, cr);
00451 
00452   if ( __draw_mode == MODE_LINES ) {
00453     for (size_t i = 0; i < nd; i += __resolution) {
00454       if ( distances[i] == 0 )  continue;
00455       const float anglerad = deg2rad(i * nd_factor);
00456       cr->move_to(0, 0);
00457       cr->line_to(distances[i] *  sin(anglerad),
00458                   distances[i] * -cos(anglerad));
00459     }
00460     cr->stroke();
00461   } else if ( __draw_mode == MODE_POINTS ) {
00462     const float radius = 4 / __zoom_factor;
00463     for (size_t i = 0; i < nd; i += __resolution) {
00464       if ( distances[i] == 0 )  continue;
00465       float anglerad = deg2rad(i * nd_factor);
00466       float x = distances[i] *  sin(anglerad);
00467       float y = distances[i] * -cos(anglerad);
00468       // circles replaced by rectangles, they are a *lot* faster
00469       //cr->move_to(x, y);
00470       //cr->arc(x, y, radius, 0, 2*M_PI);
00471       cr->rectangle(x, y, radius, radius);
00472     }
00473     cr->fill_preserve();
00474     cr->stroke();
00475   } else {
00476     cr->move_to(0, - distances[0]);
00477     for (size_t i = __resolution; i <= nd + __resolution; i += __resolution) {
00478       if ( distances[i] == 0 )  continue;
00479       const float anglerad    = normalize_rad(deg2rad(i * nd_factor));
00480       cr->line_to(distances[i % nd] *  sin(anglerad),
00481                   distances[i % nd] * -cos(anglerad));
00482     }
00483     cr->stroke();
00484   }
00485 
00486   if (revdists) delete[] revdists;
00487 }
00488 
00489 
00490 /** Draw person legs.
00491  * Draws the legs of persons
00492  * @param window Gdk window
00493  * @param cr Cairo context to draw to. It is assumed that possible transformations
00494  * have been setup before.
00495  */
00496 void
00497 LaserDrawingArea::draw_persons_legs(Glib::RefPtr<Gdk::Window> &window,
00498                                     Cairo::RefPtr<Cairo::Context> &cr)
00499 {
00500   std::list<ObjectPositionInterface*>::iterator objpos_if_itt;;
00501 
00502   cr->save();
00503   if (__l_objpos_if_persons) {
00504     cr->set_source_rgb(0,0,1);
00505     for( objpos_if_itt = __l_objpos_if_persons->begin(); 
00506          objpos_if_itt != __l_objpos_if_persons->end()  && (*objpos_if_itt)->has_writer();
00507          objpos_if_itt++ ) {
00508       if(!__break_drawing)
00509         (*objpos_if_itt)->read();
00510       if ((*objpos_if_itt)->is_valid()){
00511         std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->relative_x(), (*objpos_if_itt)->relative_y());
00512         float x=pos.first;
00513         float y=pos.second;
00514         cr->move_to(x, y);
00515         //      cr->arc(x, y, std::max((*objpos_if_itt)->extent_x(),(*objpos_if_itt)->extent_y()), 0, 2*M_PI);
00516         cr->arc(x, y, 0.2, 0, 2*M_PI);
00517       }
00518     }
00519     cr->stroke();
00520   }
00521 
00522   if (__l_objpos_if_legs) {
00523     cr->set_source_rgb(0,1,0);
00524     for( objpos_if_itt = __l_objpos_if_legs->begin(); 
00525          objpos_if_itt != __l_objpos_if_legs->end() && (*objpos_if_itt)->has_writer() ; 
00526          objpos_if_itt++ ) {
00527       if(!__break_drawing)
00528         (*objpos_if_itt)->read();
00529       if ((*objpos_if_itt)->is_valid()){
00530         std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->relative_x(), (*objpos_if_itt)->relative_y());
00531         float x=pos.first;
00532         float y=pos.second;
00533         cr->move_to(x, y);
00534         cr->arc(x, y, 0.1, 0, 2*M_PI);
00535       }
00536     }
00537     cr->stroke();
00538   }
00539   
00540   if (__l_objpos_if_misc) {
00541     cr->set_source_rgb(0,1,1);
00542     for( objpos_if_itt = __l_objpos_if_misc->begin(); 
00543          objpos_if_itt != __l_objpos_if_misc->end() && (*objpos_if_itt)->has_writer() ; 
00544          objpos_if_itt++ ) {
00545       if(!__break_drawing)
00546         (*objpos_if_itt)->read();
00547       if ((*objpos_if_itt)->is_valid()){
00548         //      switch( (*objpos_if_itt)->object_type() ){
00549         //      case ObjectPositionInterface::TYPE_BALL:
00550         //TYPE_OPPONENT
00551         if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_BALL){
00552           std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->relative_x(), (*objpos_if_itt)->relative_y());
00553           float x=pos.first;
00554           float y=pos.second;
00555           pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x(), (*objpos_if_itt)->world_y());
00556           float begin_x=pos.first;
00557           float begin_y=pos.second;
00558           pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x_velocity(), (*objpos_if_itt)->world_y_velocity());
00559           float end_x= pos.first;
00560           float end_y= pos.first;
00561           float angle1=atan2(begin_y- y, begin_x - x);
00562           float angle2=atan2(end_y- y, end_x - x);
00563           float radius=(*objpos_if_itt)->relative_x_velocity();
00564           float probability = (*objpos_if_itt)->relative_z_velocity();
00565           cr->move_to(begin_x, begin_y);
00566           cr->arc(x, y, radius, angle2, angle1);
00567 
00568           //    Cairo::TextExtents te;
00569           std::string t = StringConversions::to_string(probability);
00570           t.erase(5);
00571           //      cr->set_source_rgb(0,1 ,1);
00572           cr->set_font_size(0.08);
00573           //    cr->get_text_extents(t, te);
00574           //    cr->move_to(- te.width / 2, -te.height / 2);
00575           cr->move_to(begin_x, begin_y);
00576           cr->show_text(t);
00577           //      cr->set_source_rgb(0,0,1);
00578         
00579           //    break;
00580           //      case ObjectPositionInterface::TYPE_LINE:
00581         }else if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_LINE){
00582           std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x(), (*objpos_if_itt)->world_y());
00583           float begin_x=pos.first;
00584           float begin_y=pos.second;
00585           pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x_velocity(), (*objpos_if_itt)->world_y_velocity());
00586           float end_x= pos.first;
00587           float end_y= pos.first;
00588           cr->move_to(begin_x, begin_y);
00589           cr->line_to(end_x, end_y);
00590           //break;
00591         }
00592       }
00593     }
00594     //  cr->fill_preserve();
00595     cr->stroke();
00596   }
00597 
00598   cr->set_source_rgb(1,0,1);
00599 
00600   float r,g,b;
00601   r=g=b=0.0;
00602   int color_it=0;
00603   float delta = 0.25;
00604 
00605 
00606   if (__l_track_if) {
00607 
00608     std::list<Position2DTrackInterface*>::iterator track_if_itt;;  
00609     const float radius (0.1);
00610     float* x_positions1;
00611     float* y_positions1;
00612     int* timestamps1;
00613     float* x_positions2 = NULL;
00614     float* y_positions2 = NULL;
00615     unsigned int track_length1 = 0;
00616     unsigned int track_length2 = 0;
00617     int* timestamps2 = NULL;
00618     unsigned int id;
00619     cr->set_font_size(0.03);
00620 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
00621     printf("\n\n################################\n");
00622 #endif
00623     for( track_if_itt = __l_track_if->begin(); 
00624          track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer();) {
00625       bool b_compound_track(false);
00626       if(!__break_drawing)
00627         (*track_if_itt)->read();
00628       if ((*track_if_itt)->is_valid()){
00629         x_positions1=(*track_if_itt)->track_x_positions();
00630         y_positions1=(*track_if_itt)->track_y_positions();
00631         timestamps1=(*track_if_itt)->track_timestamps();
00632         track_length1 = (*track_if_itt)->length();
00633         id = (*track_if_itt)->track_id();
00634         ++track_if_itt;
00635         if( track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer()){
00636           if(!__break_drawing)
00637             (*track_if_itt)->read();
00638           if( (*track_if_itt)->is_valid() && (*track_if_itt)->track_id()==id ){
00639             b_compound_track = true;
00640             x_positions2=(*track_if_itt)->track_x_positions();
00641             y_positions2=(*track_if_itt)->track_y_positions();
00642             timestamps2=(*track_if_itt)->track_timestamps();
00643             track_length2 = (*track_if_itt)->length();
00644             ++track_if_itt;
00645           }
00646         }
00647 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
00648         printf("\n    trackid %d\n", id);
00649 #endif
00650         unsigned int i(0);
00651         unsigned int j(0);
00652         float x = x_positions1[i];
00653         float y = y_positions1[i];
00654         if(b_compound_track){
00655           while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
00656             ++j;
00657           }
00658           if(timestamps2[j] == timestamps1[i]){
00659             x += x_positions2[i];
00660             x /= 2;
00661             y += y_positions2[i];
00662             y /=2;
00663           }
00664         }
00665         std::pair<float,float> pos = transform_coords_from_fawkes(x,y);
00666         cr->move_to(pos.first,pos.second);
00667         for (; i < track_length1; ++i){
00668           x = x_positions1[i];
00669           y = y_positions1[i];
00670           if(b_compound_track){
00671             while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
00672               ++j;
00673             }
00674             if(timestamps2[j] == timestamps1[i]){
00675               x += x_positions2[i];
00676               x /= 2;
00677               y += y_positions2[i];
00678               y /=2;
00679             }
00680           }
00681           std::pair<float,float> pos = transform_coords_from_fawkes(x,y);
00682           //cr->move_to(pos.first - radius, pos.second);
00683           //      cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
00684           cr->line_to(pos.first, pos.second);
00685           //    cr->rectangle(x_positions[i], y_positions[i], 4 / __zoom_factor, 4 / __zoom_factor);
00686         
00687           //    std::string t = StringConversions::toString(id) + "-" + StringConversions::toString(timestamps[i]);
00688           std::string t = StringConversions::to_string(timestamps1[i]);
00689           //      cr->move_to(begin_x, begin_y);
00690           cr->show_text(t);
00691           cr->move_to(pos.first, pos.second);
00692 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
00693           printf("( %f,%f,[%d] )", pos.first, pos.second, timestamps1[i] );
00694 #endif
00695         }
00696 
00697         // chose color    
00698         if (div(color_it,3).rem == 0) r+= delta;
00699         if (div(color_it,3).rem == 1) g+= delta;
00700         if (div(color_it,3).rem == 2) b+= delta;
00701         cr->set_source_rgb(r,g,b);
00702         color_it++;
00703 
00704         cr->stroke();
00705         
00706         
00707         i = std::max(0, (int) track_length1 - CFG_PRINT_NR_TRACKELEMENTS);
00708         j = 0;
00709         for (; i < track_length1; ++i){
00710           x = x_positions1[i];
00711           y = y_positions1[i];
00712           if(b_compound_track){
00713             while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
00714               ++j;
00715             }
00716           }
00717           
00718           std::pair<float,float> pos = transform_coords_from_fawkes(x_positions1[i],y_positions1[i]);
00719           cr->move_to(pos.first - radius, pos.second);
00720           cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
00721           
00722           if(b_compound_track && timestamps2[j] == timestamps1[i]){
00723             cr->move_to(pos.first, pos.second);
00724             
00725             std::pair<float,float> pos = transform_coords_from_fawkes(x_positions2[j],y_positions2[j]);
00726             cr->line_to(pos.first, pos.second);
00727             cr->move_to(pos.first - radius, pos.second);
00728             cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
00729           }
00730         }
00731         cr->set_source_rgb(0,0,1);
00732         cr->stroke();
00733         
00734       }
00735       else{
00736         break;
00737       }
00738     }
00739   }
00740   
00741   /*  DRAW TARGET */
00742   if(__target_if && __target_if->has_writer()){
00743     __target_if->read();
00744     if(__target_if->is_valid()){
00745       cr->set_source_rgb(1,0,0);
00746       std::pair<float,float> pos = transform_coords_from_fawkes(__target_if->relative_x(), __target_if->relative_y());
00747       float x=pos.first;
00748       float y=pos.second;
00749       float radius = 0.1;
00750 
00751       cr->move_to(x, y);
00752       cr->arc(x, y, radius, 0, 2*M_PI);
00753       cr->move_to(x - radius, y);
00754       cr->line_to(x + radius, y);
00755       cr->move_to(x, y - radius );
00756       cr->line_to(x, y + radius);
00757       cr->stroke();
00758     }
00759   }
00760 
00761 
00762   /*
00763   float r,g,b;
00764   r=g=b=0.0;
00765   float delta = 0.2;
00766   for (int i = 0; i< 15 ; i++){
00767     
00768     if (div(i,3).rem == 0) r+= delta;
00769     if (div(i,3).rem == 1) g+= delta;
00770     if (div(i,3).rem == 2) b+= delta;
00771     //    printf("i %d rem %d| r %f, g %f, b %f\n", i, div(i,3).rem,r,g,b);
00772     cr->move_to(0, (i+1)*0.125);
00773     cr->set_source_rgb(r,g,b);
00774     cr->rectangle(0, (i+1)*0.125, 0.1 , 0.1 );
00775     cr->fill_preserve();
00776     cr->stroke();
00777   }
00778   */
00779   //  cr->stroke();
00780 
00781   cr->restore();
00782 }
00783 
00784 
00785 /** Draw laser segments as produced by leg tracker application.
00786  * @param window Gdk window
00787  * @param cr Cairo context to draw to. It is assumed that possible transformations
00788  * have been setup before.
00789  */
00790 void
00791 LaserDrawingArea::draw_segments(Glib::RefPtr<Gdk::Window> &window,
00792                                 Cairo::RefPtr<Cairo::Context> &cr)
00793 {
00794   float *distances = __laser360_if ? __laser360_if->distances() : __laser720_if->distances();
00795   size_t nd = __laser_segmentation_if->maxlenof_distances();
00796   const float nd_factor = 360.0 / nd;
00797 
00798   cr->save();
00799   /* DRAW SEGMENTS (draw the segment interiors again with other color*/
00800   if( __laser_segmentation_if && __laser_segmentation_if->has_writer()){
00801     if(!__break_drawing)
00802       __laser_segmentation_if->read();
00803     float * segmentations = __laser_segmentation_if->distances();
00804     size_t nd = __laser_segmentation_if->maxlenof_distances();
00805     //  cr->set_source_rgba(0,0,0,0.5);
00806     cr->set_source_rgb(1,1,0);
00807 
00808     if ( __draw_mode == MODE_POINTS ) {
00809       for (size_t i = 0; i < nd; i += __resolution) {
00810         if( segmentations[i]==0) continue;  // dont draw the segment borders
00811         if ( distances[i] == 0 )  continue;
00812         float anglerad = deg2rad(i * nd_factor);
00813         cr->move_to(0, 0);
00814         cr->line_to(distances[i] *  sin(anglerad),
00815                     distances[i] * -cos(anglerad));
00816       }
00817       cr->stroke();
00818     } else {//if ( __draw_mode == MODE_LINES ) {
00819       float radius = 4 / __zoom_factor;
00820       for (size_t i = 0; i < nd; i += __resolution) {
00821         if( segmentations[i]==0) continue;  // dont draw the segment borders
00822         if ( distances[i] == 0 )  continue;
00823         float anglerad = deg2rad(i * nd_factor);
00824         float x = distances[i] *  sin(anglerad);
00825         float y = distances[i] * -cos(anglerad);
00826         // circles replaced by rectangles, they are a *lot* faster
00827         //cr->move_to(x, y);
00828         //cr->arc(x, y, radius, 0, 2*M_PI);
00829         cr->rectangle(x, y, radius, radius);
00830       }
00831       cr->fill_preserve();
00832       cr->stroke();
00833     } 
00834     /*else {
00835       cr->move_to(0, - distances[0]);
00836       for (size_t i = __resolution; i <= nd + __resolution; i += __resolution) {
00837       if ( distances[i] == 0 )  continue;
00838       float anglerad    = deg2rad(i % 360);
00839       cr->line_to(distances[i % 360] *  sin(anglerad),
00840       distances[i % 360] * -cos(anglerad));
00841       }
00842       cr->stroke();
00843       }
00844     */
00845   }
00846   cr->restore();
00847 }
00848 
00849 
00850 /** Scroll event handler.
00851  * @param event event structure
00852  * @return signal return value
00853  */
00854 bool
00855 LaserDrawingArea::on_scroll_event(GdkEventScroll *event)
00856 {
00857   if (event->direction == GDK_SCROLL_UP) {
00858     zoom_in();
00859   } else if (event->direction == GDK_SCROLL_DOWN) {
00860     zoom_out();
00861   }
00862   return true;
00863 }
00864 
00865 /** Set a member for breaking the drawing. */
00866 void
00867 LaserDrawingArea::toggle_break_drawing()
00868 {
00869   __break_drawing = ! __break_drawing;
00870 }
00871 
00872 
00873 /** Button press event handler.
00874  * @param event event data
00875  * @return true
00876  */
00877 bool
00878 LaserDrawingArea::on_button_press_event(GdkEventButton *event)
00879 {
00880   __last_mouse_x = event->x;
00881   __last_mouse_y = event->y;
00882   return true;
00883 }
00884 
00885 
00886 /** Mouse motion notify event handler.
00887  * @param event event data
00888  * @return true
00889  */
00890 bool
00891 LaserDrawingArea::on_motion_notify_event(GdkEventMotion *event)
00892 {
00893   //  d__translation_x -= __last_mouse_x - event->x;
00894   //  double __translation_y -= __last_mouse_y - event->y;
00895   __xc -= __last_mouse_x - event->x;
00896   __yc -= __last_mouse_y - event->y;
00897 
00898   __last_mouse_x = event->x;
00899   __last_mouse_y = event->y;
00900   queue_draw();
00901   return true;
00902 }
00903 
00904 
00905 
00906 /**
00907  * Transform a position from the fawkes coordinate system to the Cairo
00908  * coordinate system.
00909  * @param p_x input x 
00910  * @param p_y input y
00911  * @return the transformed position
00912  */
00913 std::pair<float,float>
00914 LaserDrawingArea::transform_coords_from_fawkes(float p_x, float p_y){
00915   std::pair<float,float> pos;
00916   pos.first =  -p_y ;
00917   pos.second=  -p_x ;
00918   return pos;
00919 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends