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