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