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) {
316LaserDrawingArea::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)
335LaserDrawingArea::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)
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;
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;
1064std::pair<float, float>
1067 std::pair<float, float> pos;
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 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 set_line_if(fawkes::ObjectPositionInterface *line_if)
Set line interface.
2D visualization processor for VisualDisplay2DInterface.
void set_interface(fawkes::VisualDisplay2DInterface *interface)
Set interface.
Base class for exceptions in Fawkes.
Base class for all Fawkes BlackBoard interfaces.
const char * uid() const
Get unique identifier of interface.
bool is_valid() const
Check validity of interface.
void read()
Read from BlackBoard into local copy.
bool has_writer() const
Check if there is a writer for the interface.
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.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).