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