Fawkes API  Fawkes Development Version
laser_drawing_area.cpp
1 
2 /***************************************************************************
3  * laser_drawing_area.cpp - Laser drawing area derived from Gtk::DrawingArea
4  *
5  * Created: Thu Oct 09 18:20:21 2008
6  * Copyright 2008-2010 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "laser_drawing_area.h"
24 #include "visdisplay.h"
25 #include <interfaces/Laser720Interface.h>
26 #include <interfaces/Laser360Interface.h>
27 #include <interfaces/Laser1080Interface.h>
28 #include <interfaces/ObjectPositionInterface.h>
29 #include <interfaces/VisualDisplay2DInterface.h>
30 #include <utils/math/angle.h>
31 #include <gui_utils/robot/drawer.h>
32 #include <algorithm>
33 #include <utils/misc/string_conversions.h>
34 #include <cstdio>
35 #include <cmath>
36 
37 //#define LASERGUI_DEBUG_PRINT_TRACKS
38 #define CFG_PRINT_NR_TRACKELEMENTS 5
39 
40 using namespace fawkes;
41 
42 /** @class LaserDrawingArea "laser_drawing_area.h"
43  * Laser drawing area.
44  * Derived version of Gtk::DrawingArea that renders values of a laser interface.
45  * @author Tim Niemueller
46  */
47 
48 /** Constructor.
49  * Special ctor to be used with Gtk::Builder's get_widget_derived().
50  * @param cobject Gtk C object
51  * @param builder Gtk Builder
52  */
53 LaserDrawingArea::LaserDrawingArea(BaseObjectType* cobject,
54  const Glib::RefPtr<Gtk::Builder> &builder)
55  : Gtk::DrawingArea(cobject)
56 {
57  __draw_mode = MODE_LINES;
58  __zoom_factor = 50;
59  __l_objpos_if_persons = NULL;
60  __l_objpos_if_legs = NULL;
61  __l_objpos_if_misc = NULL;
62  __laser_segmentation_if = NULL;
63  __l_track_if = NULL;
64  __target_if = NULL;
65  __switch_if = NULL;
66  __line_if = NULL;
67  __visdisp_if = NULL;
68  __robot_drawer = NULL;
69  __resolution = 1;
70  __rotation = 0;
71  __break_drawing = false;
72  __first_draw = true;
73  __connected = false;
74 
75  __visdisp = new VisualDisplay2D();
76 
77  add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK |
78  Gdk::BUTTON_PRESS_MASK );
79 
80 #if GTK_VERSION_LT(3,0)
81  signal_expose_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_expose_event));
82  signal_button_press_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_button_press_event));
83  signal_motion_notify_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_motion_notify_event));
84 #endif
85  //Glib::RefPtr<Gdk::Window> window = get_window();
86 }
87 
88 /** Constructor. */
90 {
91  __draw_mode = MODE_LINES;
92  __zoom_factor = 50;
93  __l_objpos_if_persons = NULL;
94  __l_objpos_if_legs = NULL;
95  __l_objpos_if_misc = NULL;
96  __laser_segmentation_if = NULL;
97  __l_track_if = NULL;
98  __target_if = NULL;
99  __switch_if = NULL;
100  __line_if = NULL;
101  __visdisp_if = NULL;
102  __robot_drawer = NULL;
103  __resolution = 1;
104  __rotation = 0;
105  __break_drawing = false;
106 
107  __visdisp = new VisualDisplay2D();
108 
109  add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK);
110 
111 #if GTK_VERSION_LT(3,0)
112  signal_expose_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_expose_event));
113  signal_button_press_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_button_press_event));
114  signal_motion_notify_event().connect(sigc::mem_fun(*this, &LaserDrawingArea::on_motion_notify_event));
115 #endif
116 }
117 
118 
119 /** Destructor. */
121 {
122  delete __visdisp;
123 }
124 
125 /** Set ObjectPosition interfaces.
126  * @param l_objpos_if_persons list of objectposition interfaces for persons
127  * @param l_objpos_if_legs list of objectposition interfaces for legs
128  * @param l_objpos_if_misc list of objectposition interfaces for miscellanous objects
129  * @param laser_segmentation_if Laser interface indicating the segmentation-borfers of the legtracker
130  * @param l_track_if list of track interfaces
131  * @param target_if the current target
132  * @param switch_if used to indicate that a drawing-run is finish (so e.g. new data can be sent)
133  */
134 void
135 LaserDrawingArea::set_objpos_if(std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_persons,
136  std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_legs,
137  std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_misc,
138  fawkes::Laser720Interface* laser_segmentation_if ,
139  std::list<fawkes::Position2DTrackInterface*>* l_track_if,
141  fawkes::SwitchInterface* switch_if){
142  __l_objpos_if_persons = l_objpos_if_persons;
143  __l_objpos_if_legs = l_objpos_if_legs;
144  __l_objpos_if_misc = l_objpos_if_misc;
145  __laser_segmentation_if=laser_segmentation_if;
146  __l_track_if = l_track_if;
147  __target_if = target_if;
148  __switch_if = switch_if;
149 }
150 
151 /** Set connection status.
152  * @param connected true if connected, false otherwise
153  */
154 void
156 {
157  __connected = connected;
158  queue_draw();
159 }
160 
161 
162 /** Set new laser interfaces.
163  *
164  * This is also the place where colors are determined the following way:
165  * <pre>
166  * 1 000 -> 0 0 0
167  * 2 001 -> 255 0 0
168  * 3 010 -> 0 255 0
169  * 4 011 -> 255 255 0
170  * 5 100 -> 0 0 255
171  * 6 101 -> 255 0 255
172  * 7 110 -> 255 255 0
173  * 8 000 -> 0 0 0
174  * 9 001 -> 127 0 0
175  * 10 010 -> 0 127 0
176  * 11 011 -> 127 127 0
177  * 12 100 -> 0 0 127
178  * 13 101 -> 127 0 127
179  * 14 110 -> 127 127 0
180  * ...
181  * </pre>
182  *
183  * @param ifs The interfaces of the lasers that should be visualized.
184  */
185 void
186 LaserDrawingArea::set_laser_ifs(const std::list<fawkes::Interface*>& ifs)
187 {
188  __laser_ifs.clear();
189  unsigned char color_counter = 0;
190  unsigned char intensity = 255;
191  for (std::list<fawkes::Interface*>::const_iterator it = ifs.begin();
192  it != ifs.end(); ++it) {
193  if ((color_counter & 0x1 & 0x2 & 0x4) != 0) {
194  intensity /= 2;
195  }
196  Color c;
197  c.r = ((color_counter & 0x1) != 0) ? intensity : 0;
198  c.g = ((color_counter & 0x2) != 0) ? intensity : 0;
199  c.b = ((color_counter & 0x4) != 0) ? intensity : 0;
200  const InterfaceColorPair p = std::make_pair(*it, c);
201  __laser_ifs.push_back(p);
202  ++color_counter;
203  }
204  queue_draw();
205 }
206 
207 
208 /** Reset laser interfaces to "no laser available". */
209 void
211 {
212  __laser_ifs.clear();
213  __l_objpos_if_persons = NULL;
214  __l_objpos_if_legs = NULL;
215  __l_objpos_if_misc = NULL;
216  __laser_segmentation_if = NULL;
217  __l_track_if = NULL;
218  __target_if = NULL;
219  __switch_if = NULL;
220 
221  Gtk::Allocation allocation = get_allocation();
222  const int width = allocation.get_width();
223  const int height = allocation.get_height();
224 
225  __xc = width / 2;
226  __yc = height / 2;
227  __zoom_factor = 50;
228  queue_draw();
229 }
230 
231 /** Set line interface.
232  * @param line_if interface to use for line data to draw.
233  */
234 void
236 {
237  __line_if = line_if;
238 }
239 
240 
241 /** Set visual display interface.
242  * @param visdisp_if interface to query for drawing ops
243  */
244 void
246 {
247  __visdisp_if = visdisp_if;
248  __visdisp->set_interface(__visdisp_if);
249 }
250 
251 
252 /** Set robot drawer.
253  * @param robot_drawer new robot drawer to use
254  */
255 void
257 {
258  __robot_drawer = robot_drawer;
259 }
260 
261 /** Set resolution.
262  * Every n'th beam will be drawn where n is the resolution.
263  * @param resolution new resolution
264  */
265 void
266 LaserDrawingArea::set_resolution(unsigned int resolution)
267 {
268  __resolution = resolution;
269 }
270 
271 
272 /** Set the drawing mode.
273  * @param mode the new drawing mode
274  */
275 void
277 {
278  __draw_mode = mode;
279  queue_draw();
280 }
281 
282 /** Zoom in.
283  * Increases zoom factor by 20, no upper limit.
284  */
285 void
287 {
288  __zoom_factor += 20;
289  queue_draw();
290 }
291 
292 /** Zoom out.
293  * Decreases zoom factor by 20 with a minimum of 1.
294  */
295 void
297 {
298  if ( __zoom_factor > 20 ) {
299  __zoom_factor -= 20;
300  } else {
301  __zoom_factor = 1;
302  }
303  queue_draw();
304 }
305 
306 
307 /** Set rotation.
308  * @param rot_rad rotation angle in rad
309  */
310 void
312 {
313  __rotation = rot_rad;
314 }
315 
316 
317 bool
318 LaserDrawingArea::all_laser_ifs_have_writer() const
319 {
320  for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
321  it != __laser_ifs.end(); ++it) {
322  fawkes::Interface* itf = it->first;
323  if (!itf->has_writer()) {
324  return false;
325  }
326  }
327  return true;
328 }
329 
330 
331 #if GTK_VERSION_GE(3,0)
332 /** Expose event handler.
333  * @param cr Cairo context for drawing
334  * @return signal return value
335  */
336 bool
337 LaserDrawingArea::on_draw(const Cairo::RefPtr<Cairo::Context> &cr)
338 #else
339 /** Expose event handler.
340  * @param event event info structure.
341  * @return signal return value
342  */
343 bool
344 LaserDrawingArea::on_expose_event(GdkEventExpose* event)
345 #endif
346 {
347  // This is where we draw on the window
348  Glib::RefPtr<Gdk::Window> window = get_window();
349  if(window) {
350  Gtk::Allocation allocation = get_allocation();
351 
352  if(__first_draw)
353  {
354  __first_draw = false;
355  const int width = allocation.get_width();
356  const int height = allocation.get_height();
357 
358  // coordinates for the center of the window
359  __xc = width / 2;
360  __yc = height / 2;
361  }
362 #if GTK_VERSION_LT(3,0)
363  Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
364 #endif
365  cr->set_line_width(1.0);
366 
367  cr->set_source_rgb(1, 1, 1);
368 #if GTK_VERSION_LT(3,0)
369  // clip to the area indicated by the expose event so that we only
370  // redraw the portion of the window that needs to be redrawn
371  cr->rectangle(event->area.x, event->area.y,
372  event->area.width, event->area.height);
373  cr->fill_preserve();
374  cr->clip();
375 #else
376  cr->paint();
377 #endif
378  cr->set_source_rgb(0, 0, 0);
379  //cr->set_source_rgba(0,0,0,1);
380 
381  // __last_xc += __translation_x;
382  // __last_yc += __translation_y;
383  cr->translate(__xc, __yc);
384 
385  cr->save();
386  if (! __connected) {
387  Cairo::TextExtents te;
388  std::string t = "Not connected to BlackBoard";
389  cr->set_source_rgb(1, 0, 0);
390  cr->set_font_size(20);
391  cr->get_text_extents(t, te);
392  cr->move_to(- te.width / 2, -te.height / 2);
393  cr->show_text(t);
394  } else if ( __laser_ifs.empty() ) {
395  Cairo::TextExtents te;
396  std::string t = "No interface opened";
397  cr->set_source_rgb(1, 0, 0);
398  cr->set_font_size(20);
399  cr->get_text_extents(t, te);
400  cr->move_to(- te.width / 2, -te.height / 2);
401  cr->show_text(t);
402  } else if (! all_laser_ifs_have_writer() ) {
403  Cairo::TextExtents te;
404  std::string t = "No writer for ";
405  for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
406  it != __laser_ifs.end(); ++it) {
407  fawkes::Interface* itf = it->first;
408  if (!itf->has_writer()) {
409  t += itf->uid();
410  t += ' ';
411  }
412  }
413  cr->set_source_rgb(1, 0, 0);
414  cr->set_font_size(20);
415  cr->get_text_extents(t, te);
416  cr->move_to(- te.width / 2, -te.height / 2);
417  cr->show_text(t);
418  } else {
419  if (! __break_drawing) {
420  for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
421  it != __laser_ifs.end(); ++it) {
422  fawkes::Interface* laser_if = it->first;
423  laser_if->read();
424  }
425  }
426 
427  for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
428  it != __laser_ifs.end(); ++it) {
429  const fawkes::Interface* laser_if = it->first;
430  const Color& color = it->second;
431  cr->save();
432  cr->set_source_rgb(color.r, color.g, color.b);
433  draw_beams(laser_if, window, cr);
434  cr->restore();
435  }
436  if (__robot_drawer) __robot_drawer->draw_robot(window, cr);
437  for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
438  it != __laser_ifs.end(); ++it) {
439  const fawkes::Interface* laser_if = it->first;
440  const Color& color = it->second;
441  cr->save();
442  cr->set_source_rgb(color.r, color.g, color.b);
443  draw_segments(laser_if, window, cr);
444  cr->restore();
445  }
446  draw_persons_legs(window, cr);
447 
448  if(__switch_if != NULL && __switch_if->has_writer()){
450  __switch_if->msgq_enqueue(esm);
451  }
452  }
453  cr->restore();
454 
455  cr->save();
456  cr->rotate(0.5 * M_PI + __rotation);
457  cr->scale(-__zoom_factor, __zoom_factor);
458  cr->set_line_width(1. / __zoom_factor);
459  if (__visdisp_if) {
460  __visdisp->process_messages();
461  __visdisp->draw(cr);
462  }
463 
464  const float radius = 0.01;
465  if (__line_if) {
466  __line_if->read();
467  if (__line_if->has_writer() &&
468  __line_if->is_valid() && __line_if->is_visible()) {
469 
470  cr->set_source_rgb(1, 0, 0);
471  /*
472  std::vector<double> dashes(1);
473  dashes[0] = 0.1;
474  cr->set_dash(dashes, 0);
475  */
476  cr->rectangle(__line_if->world_x() - radius * 0.5, __line_if->world_y() - radius * 0.5, radius, radius);
477  cr->rectangle(__line_if->relative_x() - radius * 0.5, __line_if->relative_y() - radius * 0.5, radius, radius);
478  cr->fill_preserve();
479  cr->stroke();
480  cr->move_to(__line_if->world_x(), __line_if->world_y());
481  cr->line_to(__line_if->relative_x(), __line_if->relative_y());
482  cr->stroke();
483  }
484  }
485  cr->restore();
486  }
487 
488  return true;
489 }
490 
491 
492 /** Draw scale box.
493  * Draws a circle with a radius of 1m around the robot.
494  * @param window Gdk window
495  * @param cr Cairo context to draw to. It is assumed that possible transformations
496  * have been setup before.
497  */
498 void
499 LaserDrawingArea::draw_scalebox(Glib::RefPtr<Gdk::Window> &window,
500  const Cairo::RefPtr<Cairo::Context> &cr)
501 {
502  cr->save();
503  cr->set_source_rgba(0, 0, 0.8, 0.2);
504  cr->arc(0, 0, 1.0, 0, 2 * M_PI);
505  cr->stroke();
506  cr->restore();
507 }
508 
509 
510 /** Draw Beams of an interface.
511  * Draws the beams as lines, circles or hull, depending on draw mode.
512  * @param itf either Laser360Interface or Laser720Interface
513  * @param window Gdk window
514  * @param cr Cairo context to draw to. It is assumed that possible transformations
515  * have been setup before.
516  */
517 void
519  Glib::RefPtr<Gdk::Window> &window,
520  const Cairo::RefPtr<Cairo::Context> &cr)
521 {
522  float *distances;
523  size_t nd;
524  bool clockwise;
525  const fawkes::Laser360Interface* itf360 = NULL;
526  const fawkes::Laser720Interface* itf720 = NULL;
527  const fawkes::Laser1080Interface* itf1080 = NULL;
528  if ((itf360 = dynamic_cast<const fawkes::Laser360Interface*>(itf))) {
529  distances = itf360->distances();
530  nd = itf360->maxlenof_distances();
531  clockwise = itf360->is_clockwise_angle();
532  } else if ((itf720 = dynamic_cast<const fawkes::Laser720Interface*>(itf))) {
533  distances = itf720->distances();
534  nd = itf720->maxlenof_distances();
535  clockwise = itf720->is_clockwise_angle();
536  } else if ((itf1080 = dynamic_cast<const fawkes::Laser1080Interface*>(itf))) {
537  distances = itf1080->distances();
538  nd = itf1080->maxlenof_distances();
539  clockwise = itf1080->is_clockwise_angle();
540  } else {
541  throw fawkes::Exception("Interface is neither Laser360Interface nor Laser720Interface");
542  }
543 
544  const float nd_factor = 360.0 / nd;
545 
546 
547  float *revdists = NULL;
548  if (! clockwise) {
549  // re-arrange to clockwise
550  revdists = (float *)new float[nd];
551  for (size_t i = 0; i < nd; ++i) {
552  revdists[nd - i - 1] = distances[i];
553  }
554  distances = revdists;
555  }
556 
557  cr->scale(__zoom_factor, __zoom_factor);
558  cr->rotate(__rotation);
559  cr->set_line_width(1. / __zoom_factor);
560 
561  draw_scalebox(window, cr);
562 
563  if ( __draw_mode == MODE_LINES ) {
564  for (size_t i = 0; i < nd; i += __resolution) {
565  if ( distances[i] == 0 || ! std::isfinite(distances[i]) ) continue;
566  const float anglerad = deg2rad(i * nd_factor);
567  cr->move_to(0, 0);
568  cr->line_to(distances[i] * sin(anglerad),
569  distances[i] * -cos(anglerad));
570  }
571  cr->stroke();
572  } else if ( __draw_mode == MODE_POINTS ) {
573  const float radius = 4 / __zoom_factor;
574  for (size_t i = 0; i < nd; i += __resolution) {
575  if ( distances[i] == 0 ) continue;
576  float anglerad = deg2rad(i * nd_factor);
577  float x = distances[i] * sin(anglerad);
578  float y = distances[i] * -cos(anglerad);
579  // circles replaced by rectangles, they are a *lot* faster
580  //cr->move_to(x, y);
581  //cr->arc(x, y, radius, 0, 2*M_PI);
582  cr->rectangle(x, y, radius, radius);
583  }
584  cr->fill_preserve();
585  cr->stroke();
586  } else {
587  cr->move_to(0, - distances[0]);
588  for (size_t i = __resolution; i <= nd + __resolution; i += __resolution) {
589  if ( distances[i] == 0 ) continue;
590  const float anglerad = normalize_rad(deg2rad(i * nd_factor));
591  cr->line_to(distances[i % nd] * sin(anglerad),
592  distances[i % nd] * -cos(anglerad));
593  }
594  cr->stroke();
595  }
596 
597  if (revdists) delete[] revdists;
598 }
599 
600 
601 /** Draw person legs.
602  * Draws the legs of persons
603  * @param window Gdk window
604  * @param cr Cairo context to draw to. It is assumed that possible transformations
605  * have been setup before.
606  */
607 void
608 LaserDrawingArea::draw_persons_legs(Glib::RefPtr<Gdk::Window> &window,
609  const Cairo::RefPtr<Cairo::Context> &cr)
610 {
611  std::list<ObjectPositionInterface*>::iterator objpos_if_itt;;
612 
613  cr->save();
614  if (__l_objpos_if_persons) {
615  cr->set_source_rgb(0,0,1);
616  for( objpos_if_itt = __l_objpos_if_persons->begin();
617  objpos_if_itt != __l_objpos_if_persons->end() && (*objpos_if_itt)->has_writer();
618  objpos_if_itt++ ) {
619  if(!__break_drawing)
620  (*objpos_if_itt)->read();
621  if ((*objpos_if_itt)->is_valid()){
622  std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->relative_x(), (*objpos_if_itt)->relative_y());
623  float x=pos.first;
624  float y=pos.second;
625  cr->move_to(x, y);
626  // cr->arc(x, y, std::max((*objpos_if_itt)->extent_x(),(*objpos_if_itt)->extent_y()), 0, 2*M_PI);
627  cr->arc(x, y, 0.2, 0, 2*M_PI);
628  }
629  }
630  cr->stroke();
631  }
632 
633  if (__l_objpos_if_legs) {
634  cr->set_source_rgb(0,1,0);
635  for( objpos_if_itt = __l_objpos_if_legs->begin();
636  objpos_if_itt != __l_objpos_if_legs->end() && (*objpos_if_itt)->has_writer() ;
637  objpos_if_itt++ ) {
638  if(!__break_drawing)
639  (*objpos_if_itt)->read();
640  if ((*objpos_if_itt)->is_valid()){
641  std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->relative_x(), (*objpos_if_itt)->relative_y());
642  float x=pos.first;
643  float y=pos.second;
644  cr->move_to(x, y);
645  cr->arc(x, y, 0.1, 0, 2*M_PI);
646  }
647  }
648  cr->stroke();
649  }
650 
651  if (__l_objpos_if_misc) {
652  cr->set_source_rgb(0,1,1);
653  for( objpos_if_itt = __l_objpos_if_misc->begin();
654  objpos_if_itt != __l_objpos_if_misc->end() && (*objpos_if_itt)->has_writer() ;
655  objpos_if_itt++ ) {
656  if(!__break_drawing)
657  (*objpos_if_itt)->read();
658  if ((*objpos_if_itt)->is_valid()){
659  // switch( (*objpos_if_itt)->object_type() ){
660  // case ObjectPositionInterface::TYPE_BALL:
661  //TYPE_OPPONENT
662  if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_BALL){
663  std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->relative_x(), (*objpos_if_itt)->relative_y());
664  float x=pos.first;
665  float y=pos.second;
666  pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x(), (*objpos_if_itt)->world_y());
667  float begin_x=pos.first;
668  float begin_y=pos.second;
669  pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x_velocity(), (*objpos_if_itt)->world_y_velocity());
670  float end_x= pos.first;
671  float end_y= pos.first;
672  float angle1=atan2(begin_y- y, begin_x - x);
673  float angle2=atan2(end_y- y, end_x - x);
674  float radius=(*objpos_if_itt)->relative_x_velocity();
675  float probability = (*objpos_if_itt)->relative_z_velocity();
676  cr->move_to(begin_x, begin_y);
677  cr->arc(x, y, radius, angle2, angle1);
678 
679  // Cairo::TextExtents te;
680  std::string t = StringConversions::to_string(probability);
681  t.erase(5);
682  // cr->set_source_rgb(0,1 ,1);
683  cr->set_font_size(0.08);
684  // cr->get_text_extents(t, te);
685  // cr->move_to(- te.width / 2, -te.height / 2);
686  cr->move_to(begin_x, begin_y);
687  cr->show_text(t);
688  // cr->set_source_rgb(0,0,1);
689 
690  // break;
691  // case ObjectPositionInterface::TYPE_LINE:
692  }else if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_LINE){
693  std::pair<float,float> pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x(), (*objpos_if_itt)->world_y());
694  float begin_x=pos.first;
695  float begin_y=pos.second;
696  pos = transform_coords_from_fawkes((*objpos_if_itt)->world_x_velocity(), (*objpos_if_itt)->world_y_velocity());
697  float end_x= pos.first;
698  float end_y= pos.first;
699  cr->move_to(begin_x, begin_y);
700  cr->line_to(end_x, end_y);
701  //break;
702  }
703  }
704  }
705  // cr->fill_preserve();
706  cr->stroke();
707  }
708 
709  cr->set_source_rgb(1,0,1);
710 
711  float r,g,b;
712  r=g=b=0.0;
713  int color_it=0;
714  float delta = 0.25;
715 
716 
717  if (__l_track_if) {
718 
719  std::list<Position2DTrackInterface*>::iterator track_if_itt;;
720  const float radius (0.1);
721  float* x_positions1;
722  float* y_positions1;
723  int* timestamps1;
724  float* x_positions2 = NULL;
725  float* y_positions2 = NULL;
726  unsigned int track_length1 = 0;
727  unsigned int track_length2 = 0;
728  int* timestamps2 = NULL;
729  unsigned int id;
730  cr->set_font_size(0.03);
731 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
732  printf("\n\n################################\n");
733 #endif
734  for( track_if_itt = __l_track_if->begin();
735  track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer();) {
736  bool b_compound_track(false);
737  if(!__break_drawing)
738  (*track_if_itt)->read();
739  if ((*track_if_itt)->is_valid()){
740  x_positions1=(*track_if_itt)->track_x_positions();
741  y_positions1=(*track_if_itt)->track_y_positions();
742  timestamps1=(*track_if_itt)->track_timestamps();
743  track_length1 = (*track_if_itt)->length();
744  id = (*track_if_itt)->track_id();
745  ++track_if_itt;
746  if( track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer()){
747  if(!__break_drawing)
748  (*track_if_itt)->read();
749  if( (*track_if_itt)->is_valid() && (*track_if_itt)->track_id()==id ){
750  b_compound_track = true;
751  x_positions2=(*track_if_itt)->track_x_positions();
752  y_positions2=(*track_if_itt)->track_y_positions();
753  timestamps2=(*track_if_itt)->track_timestamps();
754  track_length2 = (*track_if_itt)->length();
755  ++track_if_itt;
756  }
757  }
758 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
759  printf("\n trackid %d\n", id);
760 #endif
761  unsigned int i(0);
762  unsigned int j(0);
763  float x = x_positions1[i];
764  float y = y_positions1[i];
765  if(b_compound_track){
766  while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
767  ++j;
768  }
769  if(timestamps2[j] == timestamps1[i]){
770  x += x_positions2[i];
771  x /= 2;
772  y += y_positions2[i];
773  y /=2;
774  }
775  }
776  std::pair<float,float> pos = transform_coords_from_fawkes(x,y);
777  cr->move_to(pos.first,pos.second);
778  for (; i < track_length1; ++i){
779  x = x_positions1[i];
780  y = y_positions1[i];
781  if(b_compound_track){
782  while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
783  ++j;
784  }
785  if(timestamps2[j] == timestamps1[i]){
786  x += x_positions2[i];
787  x /= 2;
788  y += y_positions2[i];
789  y /=2;
790  }
791  }
792  std::pair<float,float> pos = transform_coords_from_fawkes(x,y);
793  //cr->move_to(pos.first - radius, pos.second);
794  // cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
795  cr->line_to(pos.first, pos.second);
796  // cr->rectangle(x_positions[i], y_positions[i], 4 / __zoom_factor, 4 / __zoom_factor);
797 
798  // std::string t = StringConversions::toString(id) + "-" + StringConversions::toString(timestamps[i]);
799  std::string t = StringConversions::to_string(timestamps1[i]);
800  // cr->move_to(begin_x, begin_y);
801  cr->show_text(t);
802  cr->move_to(pos.first, pos.second);
803 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
804  printf("( %f,%f,[%d] )", pos.first, pos.second, timestamps1[i] );
805 #endif
806  }
807 
808  // chose color
809  if (div(color_it,3).rem == 0) r+= delta;
810  if (div(color_it,3).rem == 1) g+= delta;
811  if (div(color_it,3).rem == 2) b+= delta;
812  cr->set_source_rgb(r,g,b);
813  color_it++;
814 
815  cr->stroke();
816 
817 
818  i = std::max(0, (int) track_length1 - CFG_PRINT_NR_TRACKELEMENTS);
819  j = 0;
820  for (; i < track_length1; ++i){
821  x = x_positions1[i];
822  y = y_positions1[i];
823  if(b_compound_track){
824  while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
825  ++j;
826  }
827  }
828 
829  std::pair<float,float> pos = transform_coords_from_fawkes(x_positions1[i],y_positions1[i]);
830  cr->move_to(pos.first - radius, pos.second);
831  cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
832 
833  if(b_compound_track && timestamps2[j] == timestamps1[i]){
834  cr->move_to(pos.first, pos.second);
835 
836  std::pair<float,float> pos = transform_coords_from_fawkes(x_positions2[j],y_positions2[j]);
837  cr->line_to(pos.first, pos.second);
838  cr->move_to(pos.first - radius, pos.second);
839  cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
840  }
841  }
842  cr->set_source_rgb(0,0,1);
843  cr->stroke();
844 
845  }
846  else{
847  break;
848  }
849  }
850  }
851 
852  /* DRAW TARGET */
853  if(__target_if && __target_if->has_writer()){
854  __target_if->read();
855  if(__target_if->is_valid()){
856  cr->set_source_rgb(1,0,0);
857  std::pair<float,float> pos = transform_coords_from_fawkes(__target_if->relative_x(), __target_if->relative_y());
858  float x=pos.first;
859  float y=pos.second;
860  float radius = 0.1;
861 
862  cr->move_to(x, y);
863  cr->arc(x, y, radius, 0, 2*M_PI);
864  cr->move_to(x - radius, y);
865  cr->line_to(x + radius, y);
866  cr->move_to(x, y - radius );
867  cr->line_to(x, y + radius);
868  cr->stroke();
869  }
870  }
871 
872 
873  /*
874  float r,g,b;
875  r=g=b=0.0;
876  float delta = 0.2;
877  for (int i = 0; i< 15 ; i++){
878 
879  if (div(i,3).rem == 0) r+= delta;
880  if (div(i,3).rem == 1) g+= delta;
881  if (div(i,3).rem == 2) b+= delta;
882  // printf("i %d rem %d| r %f, g %f, b %f\n", i, div(i,3).rem,r,g,b);
883  cr->move_to(0, (i+1)*0.125);
884  cr->set_source_rgb(r,g,b);
885  cr->rectangle(0, (i+1)*0.125, 0.1 , 0.1 );
886  cr->fill_preserve();
887  cr->stroke();
888  }
889  */
890  // cr->stroke();
891 
892  cr->restore();
893 }
894 
895 
896 /** Draw laser segments as produced by leg tracker application.
897  * @param itf either Laser360Interface or Laser720Interface
898  * @param window Gdk window
899  * @param cr Cairo context to draw to. It is assumed that possible transformations
900  * have been setup before.
901  */
902 void
904  Glib::RefPtr<Gdk::Window> &window,
905  const Cairo::RefPtr<Cairo::Context> &cr)
906 {
907  size_t nd = __laser_segmentation_if->maxlenof_distances();
908  const float nd_factor = 360.0 / nd;
909 
910  float *distances;
911  const fawkes::Laser360Interface* itf360 = NULL;
912  const fawkes::Laser720Interface* itf720 = NULL;
913  const fawkes::Laser1080Interface* itf1080 = NULL;
914  if ((itf360 = dynamic_cast<const fawkes::Laser360Interface*>(itf))) {
915  distances = itf360->distances();
916  } else if ((itf720 = dynamic_cast<const fawkes::Laser720Interface*>(itf))) {
917  distances = itf720->distances();
918  } else if ((itf1080 = dynamic_cast<const fawkes::Laser1080Interface*>(itf))) {
919  distances = itf1080->distances();
920  } else {
921  throw fawkes::Exception("Interface is neither Laser360Interface nor Laser720Interface");
922  }
923 
924  cr->save();
925  /* DRAW SEGMENTS (draw the segment interiors again with other color*/
926  if( __laser_segmentation_if && __laser_segmentation_if->has_writer()){
927  if(!__break_drawing)
928  __laser_segmentation_if->read();
929  float * segmentations = __laser_segmentation_if->distances();
930  size_t nd = __laser_segmentation_if->maxlenof_distances();
931  // cr->set_source_rgba(0,0,0,0.5);
932  cr->set_source_rgb(1,1,0);
933 
934  if ( __draw_mode == MODE_POINTS ) {
935  for (size_t i = 0; i < nd; i += __resolution) {
936  if( segmentations[i]==0) continue; // dont draw the segment borders
937  if ( distances[i] == 0 || ! std::isfinite(distances[i])) continue;
938  float anglerad = deg2rad(i * nd_factor);
939  cr->move_to(0, 0);
940  cr->line_to(distances[i] * sin(anglerad),
941  distances[i] * -cos(anglerad));
942  }
943  cr->stroke();
944  } else {//if ( __draw_mode == MODE_LINES ) {
945  float radius = 4 / __zoom_factor;
946  for (size_t i = 0; i < nd; i += __resolution) {
947  if( segmentations[i]==0) continue; // dont draw the segment borders
948  if ( distances[i] == 0 ) continue;
949  float anglerad = deg2rad(i * nd_factor);
950  float x = distances[i] * sin(anglerad);
951  float y = distances[i] * -cos(anglerad);
952  // circles replaced by rectangles, they are a *lot* faster
953  //cr->move_to(x, y);
954  //cr->arc(x, y, radius, 0, 2*M_PI);
955  cr->rectangle(x, y, radius, radius);
956  }
957  cr->fill_preserve();
958  cr->stroke();
959  }
960  /*else {
961  cr->move_to(0, - distances[0]);
962  for (size_t i = __resolution; i <= nd + __resolution; i += __resolution) {
963  if ( distances[i] == 0 ) continue;
964  float anglerad = deg2rad(i % 360);
965  cr->line_to(distances[i % 360] * sin(anglerad),
966  distances[i % 360] * -cos(anglerad));
967  }
968  cr->stroke();
969  }
970  */
971  }
972  cr->restore();
973 }
974 
975 
976 /** Scroll event handler.
977  * @param event event structure
978  * @return signal return value
979  */
980 bool
981 LaserDrawingArea::on_scroll_event(GdkEventScroll *event)
982 {
983  if (event->direction == GDK_SCROLL_UP) {
984  zoom_in();
985  } else if (event->direction == GDK_SCROLL_DOWN) {
986  zoom_out();
987  }
988  return true;
989 }
990 
991 /** Set a member for breaking the drawing. */
992 void
994 {
995  __break_drawing = ! __break_drawing;
996 }
997 
998 
999 /** Button press event handler.
1000  * @param event event data
1001  * @return true
1002  */
1003 bool
1005 {
1006  __last_mouse_x = event->x;
1007  __last_mouse_y = event->y;
1008 
1009  double user_x = event->x;
1010  double user_y = event->y;
1011  Glib::RefPtr<Gdk::Window> window = get_window();
1012  Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
1013  cr->save();
1014  cr->translate(__xc, __yc);
1015  cr->rotate(0.5 * M_PI + __rotation);
1016  cr->scale(-__zoom_factor, __zoom_factor);
1017  cr->device_to_user(user_x, user_y);
1018  printf("Clicked at (%.3lf, %.3lf)\n", user_x, user_y);
1019  cr->restore();
1020  return true;
1021 }
1022 
1023 
1024 /** Mouse motion notify event handler.
1025  * @param event event data
1026  * @return true
1027  */
1028 bool
1030 {
1031  // d__translation_x -= __last_mouse_x - event->x;
1032  // double __translation_y -= __last_mouse_y - event->y;
1033  __xc -= __last_mouse_x - event->x;
1034  __yc -= __last_mouse_y - event->y;
1035 
1036  __last_mouse_x = event->x;
1037  __last_mouse_y = event->y;
1038  queue_draw();
1039  return true;
1040 }
1041 
1042 
1043 
1044 /**
1045  * Transform a position from the fawkes coordinate system to the Cairo
1046  * coordinate system.
1047  * @param p_x input x
1048  * @param p_y input y
1049  * @return the transformed position
1050  */
1051 std::pair<float,float>
1053  std::pair<float,float> pos;
1054  pos.first = -p_y ;
1055  pos.second= -p_x ;
1056  return pos;
1057 }
Laser360Interface Fawkes BlackBoard Interface.
size_t maxlenof_distances() const
Get maximum length of distances value.
bool is_clockwise_angle() const
Get clockwise_angle value.
void draw_scalebox(Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw scale box.
Only draw beam end points.
virtual bool on_expose_event(GdkEventExpose *event)
Expose event handler.
bool is_valid() const
Get valid value.
bool is_clockwise_angle() const
Get clockwise_angle value.
Laser1080Interface Fawkes BlackBoard Interface.
void reset_laser_ifs()
Reset laser interfaces to "no laser available".
ObjectPositionInterface Fawkes BlackBoard Interface.
2D visualization processor for VisualDisplay2DInterface.
Definition: visdisplay.h:32
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:93
void set_robot_drawer(fawkes::CairoRobotDrawer *robot_drawer)
Set robot drawer.
draw_mode_t
Draw modes.
Fawkes library namespace.
void set_laser_ifs(const std::list< fawkes::Interface *> &laser_if)
Set new laser interfaces.
void set_interface(fawkes::VisualDisplay2DInterface *interface)
Set interface.
Definition: visdisplay.cpp:57
std::pair< float, float > transform_coords_from_fawkes(float p_x, float p_y)
Transform a position from the fawkes coordinate system to the Cairo coordinate system.
void zoom_in()
Zoom in.
void set_visdisp_if(fawkes::VisualDisplay2DInterface *visdisp_if)
Set visual display interface.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
Robot drawing interface.
Definition: drawer.h:34
void set_line_if(fawkes::ObjectPositionInterface *line_if)
Set line interface.
void process_messages()
Process messages.
Definition: visdisplay.cpp:68
float world_y() const
Get world_y value.
virtual bool on_scroll_event(GdkEventScroll *event)
Scroll event handler.
void toggle_break_drawing()
Set a member for breaking the drawing.
void set_draw_mode(draw_mode_t mode)
Set the drawing mode.
~LaserDrawingArea()
Destructor.
SwitchInterface Fawkes BlackBoard Interface.
bool is_visible() const
Get visible value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
void set_resolution(unsigned int resolution)
Set resolution.
virtual void draw_robot(Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)=0
Draw robot.
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
const char * uid() const
Get unique identifier of interface.
Definition: interface.cpp:687
void set_rotation(float rot_rad)
Set rotation.
float * distances() const
Get distances value.
void draw_beams(const fawkes::Interface *itf, Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw Beams of an interface.
void zoom_out()
Zoom out.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
void set_objpos_if(std::list< fawkes::ObjectPositionInterface *> *l_objpos_if_persons, std::list< fawkes::ObjectPositionInterface *> *l_objpos_if_legs, std::list< fawkes::ObjectPositionInterface *> *l_objpos_if_misc, fawkes::Laser720Interface *laser_segmentation_if, std::list< fawkes::Position2DTrackInterface *> *l_track_if, fawkes::ObjectPositionInterface *target_if, fawkes::SwitchInterface *switch_if)
Set ObjectPosition interfaces.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
virtual bool on_button_press_event(GdkEventButton *event)
Button press event handler.
float * distances() const
Get distances value.
void draw(Cairo::RefPtr< Cairo::Context > cr)
Draw objects.
Definition: visdisplay.cpp:117
void draw_segments(const fawkes::Interface *itf, Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw laser segments as produced by leg tracker application.
float world_x() const
Get world_x value.
void draw_persons_legs(Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw person legs.
float * distances() const
Get distances value.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
size_t maxlenof_distances() const
Get maximum length of distances value.
VisualDisplay2DInterface Fawkes BlackBoard Interface.
virtual bool on_motion_notify_event(GdkEventMotion *event)
Mouse motion notify event handler.
void set_connected(bool connected)
Set connection status.
Laser720Interface Fawkes BlackBoard Interface.
size_t maxlenof_distances() const
Get maximum length of distances value.
bool is_clockwise_angle() const
Get clockwise_angle value.
float relative_y() const
Get relative_y value.
LaserDrawingArea()
Constructor.
float relative_x() const
Get relative_x value.