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
43using 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 */
56LaserDrawingArea::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 */
139void
140LaserDrawingArea::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 */
160void
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 */
190void
191LaserDrawingArea::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". */
212void
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 */
237void
239{
240 line_if_ = line_if;
241}
242
243/** Set visual display interface.
244 * @param visdisp_if interface to query for drawing ops
245 */
246void
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 */
256void
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 */
266void
267LaserDrawingArea::set_resolution(unsigned int resolution)
268{
269 resolution_ = resolution;
270}
271
272/** Set the drawing mode.
273 * @param mode the new drawing mode
274 */
275void
277{
278 draw_mode_ = mode;
279 queue_draw();
280}
281
282/** Zoom in.
283 * Increases zoom factor by 20, no upper limit.
284 */
285void
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 */
295void
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 */
309void
311{
312 rotation_ = rot_rad;
313}
314
315bool
316LaserDrawingArea::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 */
334bool
335LaserDrawingArea::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 */
341bool
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 */
502void
503LaserDrawingArea::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 */
520void
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 */
610void
611LaserDrawingArea::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 */
917void
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 */
997bool
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. */
1009void
1011{
1012 break_drawing_ = !break_drawing_;
1013}
1014
1015/** Button press event handler.
1016 * @param event event data
1017 * @return true
1018 */
1019bool
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 */
1043bool
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 */
1064std::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