Fawkes API Fawkes Development Version
|
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 }