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