23 #include "laser_drawing_area.h"
25 #include "visdisplay.h"
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>
41 #define CFG_PRINT_NR_TRACKELEMENTS 5
57 const Glib::RefPtr<Gtk::Builder> &builder)
58 : Gtk::DrawingArea(cobject)
62 l_objpos_if_persons_ = NULL;
63 l_objpos_if_legs_ = NULL;
64 l_objpos_if_misc_ = NULL;
65 laser_segmentation_if_ = NULL;
74 break_drawing_ =
false;
80 add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK | Gdk::BUTTON_PRESS_MASK);
82 #if GTK_VERSION_LT(3, 0)
84 signal_button_press_event().connect(
86 signal_motion_notify_event().connect(
97 l_objpos_if_persons_ = NULL;
98 l_objpos_if_legs_ = NULL;
99 l_objpos_if_misc_ = NULL;
100 laser_segmentation_if_ = NULL;
106 robot_drawer_ = NULL;
109 break_drawing_ =
false;
113 add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK);
115 #if GTK_VERSION_LT(3, 0)
117 signal_button_press_event().connect(
119 signal_motion_notify_event().connect(
141 std::list<fawkes::ObjectPositionInterface *> *l_objpos_if_legs,
142 std::list<fawkes::ObjectPositionInterface *> *l_objpos_if_misc,
144 std::list<fawkes::Position2DTrackInterface *> *l_track_if,
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;
163 connected_ = connected;
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) {
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);
216 l_objpos_if_persons_ = NULL;
217 l_objpos_if_legs_ = NULL;
218 l_objpos_if_misc_ = NULL;
219 laser_segmentation_if_ = NULL;
224 Gtk::Allocation allocation = get_allocation();
225 const int width = allocation.get_width();
226 const int height = allocation.get_height();
249 visdisp_if_ = visdisp_if;
259 robot_drawer_ = robot_drawer;
269 resolution_ = resolution;
298 if (zoom_factor_ > 20) {
316 LaserDrawingArea::all_laser_ifs_have_writer()
const
318 for (std::list<InterfaceColorPair>::const_iterator it = laser_ifs_.begin();
319 it != laser_ifs_.end();
329 #if GTK_VERSION_GE(3, 0)
335 LaserDrawingArea::on_draw(
const Cairo::RefPtr<Cairo::Context> &cr)
346 Glib::RefPtr<Gdk::Window> window = get_window();
348 Gtk::Allocation allocation = get_allocation();
352 const int width = allocation.get_width();
353 const int height = allocation.get_height();
359 #if GTK_VERSION_LT(3, 0)
360 Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
362 cr->set_line_width(1.0);
364 cr->set_source_rgb(1, 1, 1);
365 #if GTK_VERSION_LT(3, 0)
368 cr->rectangle(event->area.x, event->area.y, event->area.width, event->area.height);
374 cr->set_source_rgb(0, 0, 0);
379 cr->translate(xc_, yc_);
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);
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);
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();
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);
416 if (!break_drawing_) {
417 for (std::list<InterfaceColorPair>::const_iterator it = laser_ifs_.begin();
418 it != laser_ifs_.end();
425 for (std::list<InterfaceColorPair>::const_iterator it = laser_ifs_.begin();
426 it != laser_ifs_.end();
429 const Color & color = it->second;
431 cr->set_source_rgb(color.r, color.g, color.b);
432 draw_beams(laser_if, window, cr);
436 robot_drawer_->draw_robot(window, cr);
437 for (std::list<InterfaceColorPair>::const_iterator it = laser_ifs_.begin();
438 it != laser_ifs_.end();
441 const Color & color = it->second;
443 cr->set_source_rgb(color.r, color.g, color.b);
444 draw_segments(laser_if, window, cr);
447 draw_persons_legs(window, cr);
449 if (switch_if_ != NULL && switch_if_->has_writer()) {
451 switch_if_->msgq_enqueue(esm);
457 cr->rotate(0.5 * M_PI + rotation_);
458 cr->scale(-zoom_factor_, zoom_factor_);
459 cr->set_line_width(1. / zoom_factor_);
461 visdisp_->process_messages();
465 const float radius = 0.01;
468 if (line_if_->has_writer() && line_if_->is_valid() && line_if_->is_visible()) {
469 cr->set_source_rgb(1, 0, 0);
475 cr->rectangle(line_if_->world_x() - radius * 0.5,
476 line_if_->world_y() - radius * 0.5,
479 cr->rectangle(line_if_->relative_x() - radius * 0.5,
480 line_if_->relative_y() - radius * 0.5,
485 cr->move_to(line_if_->world_x(), line_if_->world_y());
486 cr->line_to(line_if_->relative_x(), line_if_->relative_y());
504 const Cairo::RefPtr<Cairo::Context> &cr)
507 cr->set_source_rgba(0, 0, 0.8, 0.2);
508 cr->arc(0, 0, 1.0, 0, 2 * M_PI);
522 Glib::RefPtr<Gdk::Window> & window,
523 const Cairo::RefPtr<Cairo::Context> &cr)
531 if ((itf360 = dynamic_cast<const fawkes::Laser360Interface *>(itf))) {
535 }
else if ((itf720 = dynamic_cast<const fawkes::Laser720Interface *>(itf))) {
539 }
else if ((itf1080 = dynamic_cast<const fawkes::Laser1080Interface *>(itf))) {
544 throw fawkes::Exception(
"Interface is neither Laser360Interface nor Laser720Interface");
547 const float nd_factor = 360.0 / nd;
549 float *revdists = NULL;
552 revdists = (
float *)
new float[nd];
553 for (
size_t i = 0; i < nd; ++i) {
554 revdists[nd - i - 1] = distances[i];
556 distances = revdists;
559 cr->scale(zoom_factor_, zoom_factor_);
560 cr->rotate(rotation_);
561 cr->set_line_width(1. / zoom_factor_);
566 for (
size_t i = 0; i < nd; i += resolution_) {
567 if (distances[i] == 0 || !std::isfinite(distances[i]))
569 const float anglerad =
deg2rad(i * nd_factor);
571 cr->line_to(distances[i] * sin(anglerad), distances[i] * -cos(anglerad));
575 const float radius = 4 / zoom_factor_;
576 for (
size_t i = 0; i < nd; i += resolution_) {
577 if (distances[i] == 0)
579 float anglerad =
deg2rad(i * nd_factor);
580 float x = distances[i] * sin(anglerad);
581 float y = distances[i] * -cos(anglerad);
585 cr->rectangle(x, y, radius, radius);
590 cr->move_to(0, -distances[0]);
591 for (
size_t i = resolution_; i <= nd + resolution_; i += resolution_) {
592 if (distances[i] == 0)
595 cr->line_to(distances[i % nd] * sin(anglerad), distances[i % nd] * -cos(anglerad));
612 const Cairo::RefPtr<Cairo::Context> &cr)
614 std::list<ObjectPositionInterface *>::iterator objpos_if_itt;
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();
624 (*objpos_if_itt)->read();
625 if ((*objpos_if_itt)->is_valid()) {
627 (*objpos_if_itt)->relative_y());
629 float y = pos.second;
632 cr->arc(x, y, 0.2, 0, 2 * M_PI);
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();
643 if (!break_drawing_) {
644 (*objpos_if_itt)->read();
646 if ((*objpos_if_itt)->is_valid()) {
648 (*objpos_if_itt)->relative_y());
650 float y = pos.second;
652 cr->arc(x, y, 0.1, 0, 2 * M_PI);
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();
664 (*objpos_if_itt)->read();
665 if ((*objpos_if_itt)->is_valid()) {
669 if ((*objpos_if_itt)->object_type() == ObjectPositionInterface::TYPE_BALL) {
670 std::pair<float, float> pos =
672 (*objpos_if_itt)->relative_y());
674 float y = pos.second;
677 float begin_x = pos.first;
678 float begin_y = pos.second;
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);
691 std::string t = StringConversions::to_string(probability);
694 cr->set_font_size(0.08);
697 cr->move_to(begin_x, begin_y);
703 }
else if ((*objpos_if_itt)->object_type() == ObjectPositionInterface::TYPE_LINE) {
704 std::pair<float, float> pos =
706 float begin_x = pos.first;
707 float begin_y = pos.second;
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);
722 cr->set_source_rgb(1, 0, 1);
728 std::list<Position2DTrackInterface *>::iterator track_if_itt;
730 const float radius(0.1);
731 float * x_positions1;
732 float * y_positions1;
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;
742 cr->set_font_size(0.03);
743 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
744 printf(
"\n\n################################\n");
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();
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();
759 if (track_if_itt != l_track_if_->end() && (*track_if_itt)->has_writer()) {
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();
771 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
772 printf(
"\n trackid %u\n",
id);
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]) {
782 if (timestamps2[j] == timestamps1[i]) {
783 x += x_positions2[i];
785 y += y_positions2[i];
790 cr->move_to(pos.first, pos.second);
791 for (; i < track_length1; ++i) {
794 if (b_compound_track) {
795 while (j + 1 < track_length2 && timestamps2[j] < timestamps1[i]) {
798 if (timestamps2[j] == timestamps1[i]) {
799 x += x_positions2[i];
801 y += y_positions2[i];
808 cr->line_to(pos.first, pos.second);
812 std::string t = StringConversions::to_string(timestamps1[i]);
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]);
822 if (div(color_it, 3).rem == 0)
824 if (div(color_it, 3).rem == 1)
826 if (div(color_it, 3).rem == 2)
828 cr->set_source_rgb(r, g, b);
833 i = std::max(0, (
int)track_length1 - CFG_PRINT_NR_TRACKELEMENTS);
835 for (; i < track_length1; ++i) {
838 if (b_compound_track) {
839 while (j + 1 < track_length2 && timestamps2[j] < timestamps1[i]) {
844 std::pair<float, float> pos =
846 cr->move_to(pos.first - radius, pos.second);
847 cr->arc(pos.first, pos.second, radius, 0, 2 * M_PI);
849 if (b_compound_track && timestamps2[j] == timestamps1[i]) {
850 cr->move_to(pos.first, pos.second);
852 std::pair<float, float> pos =
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);
859 cr->set_source_rgb(0, 0, 1);
872 cr->set_source_rgb(1, 0, 0);
873 std::pair<float, float> pos =
876 float y = pos.second;
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);
919 Glib::RefPtr<Gdk::Window> & window,
920 const Cairo::RefPtr<Cairo::Context> &cr)
923 const float nd_factor = 360.0 / nd;
929 if ((itf360 = dynamic_cast<const fawkes::Laser360Interface *>(itf))) {
931 }
else if ((itf720 = dynamic_cast<const fawkes::Laser720Interface *>(itf))) {
933 }
else if ((itf1080 = dynamic_cast<const fawkes::Laser1080Interface *>(itf))) {
936 throw fawkes::Exception(
"Interface is neither Laser360Interface nor Laser720Interface");
941 if (laser_segmentation_if_ && laser_segmentation_if_->
has_writer()) {
943 laser_segmentation_if_->
read();
944 float *segmentations = laser_segmentation_if_->
distances();
947 cr->set_source_rgb(1, 1, 0);
950 for (
size_t i = 0; i < nd; i += resolution_) {
951 if (segmentations[i] == 0)
953 if (distances[i] == 0 || !std::isfinite(distances[i]))
955 float anglerad =
deg2rad(i * nd_factor);
957 cr->line_to(distances[i] * sin(anglerad), distances[i] * -cos(anglerad));
961 float radius = 4 / zoom_factor_;
962 for (
size_t i = 0; i < nd; i += resolution_) {
963 if (segmentations[i] == 0)
965 if (distances[i] == 0)
967 float anglerad =
deg2rad(i * nd_factor);
968 float x = distances[i] * sin(anglerad);
969 float y = distances[i] * -cos(anglerad);
973 cr->rectangle(x, y, radius, radius);
1000 if (event->direction == GDK_SCROLL_UP) {
1002 }
else if (event->direction == GDK_SCROLL_DOWN) {
1012 break_drawing_ = !break_drawing_;
1022 last_mouse_x_ =
event->x;
1023 last_mouse_y_ =
event->y;
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();
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);
1048 xc_ -= last_mouse_x_ -
event->x;
1049 yc_ -= last_mouse_y_ -
event->y;
1051 last_mouse_x_ =
event->x;
1052 last_mouse_y_ =
event->y;
1064 std::pair<float, float>
1067 std::pair<float, float> pos;