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