23 #include "laser_drawing_area.h"
24 #include "visdisplay.h"
25 #include <interfaces/Laser720Interface.h>
26 #include <interfaces/Laser360Interface.h>
27 #include <interfaces/ObjectPositionInterface.h>
28 #include <interfaces/VisualDisplay2DInterface.h>
29 #include <utils/math/angle.h>
30 #include <gui_utils/robot/drawer.h>
32 #include <utils/misc/string_conversions.h>
37 #define CFG_PRINT_NR_TRACKELEMENTS 5
53 const Glib::RefPtr<Gtk::Builder> &builder)
54 : Gtk::DrawingArea(cobject)
58 __l_objpos_if_persons = NULL;
59 __l_objpos_if_legs = NULL;
60 __l_objpos_if_misc = NULL;
61 __laser_segmentation_if = NULL;
67 __robot_drawer = NULL;
70 __break_drawing =
false;
76 add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK |
77 Gdk::BUTTON_PRESS_MASK );
79 #if GTK_VERSION_LT(3,0)
92 __l_objpos_if_persons = NULL;
93 __l_objpos_if_legs = NULL;
94 __l_objpos_if_misc = NULL;
95 __laser_segmentation_if = NULL;
101 __robot_drawer = NULL;
104 __break_drawing =
false;
108 add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK);
110 #if GTK_VERSION_LT(3,0)
135 std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_legs,
136 std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_misc,
138 std::list<fawkes::Position2DTrackInterface*>* l_track_if,
141 __l_objpos_if_persons = l_objpos_if_persons;
142 __l_objpos_if_legs = l_objpos_if_legs;
143 __l_objpos_if_misc = l_objpos_if_misc;
144 __laser_segmentation_if=laser_segmentation_if;
145 __l_track_if = l_track_if;
146 __target_if = target_if;
147 __switch_if = switch_if;
156 __connected = connected;
188 unsigned char color_counter = 0;
189 unsigned char intensity = 255;
190 for (std::list<fawkes::Interface*>::const_iterator it = ifs.begin();
191 it != ifs.end(); ++it) {
192 if ((color_counter & 0x1 & 0x2 & 0x4) != 0) {
196 c.r = ((color_counter & 0x1) != 0) ? intensity : 0;
197 c.g = ((color_counter & 0x2) != 0) ? intensity : 0;
198 c.b = ((color_counter & 0x4) != 0) ? intensity : 0;
199 const InterfaceColorPair p = std::make_pair(*it, c);
200 __laser_ifs.push_back(p);
212 __l_objpos_if_persons = NULL;
213 __l_objpos_if_legs = NULL;
214 __l_objpos_if_misc = NULL;
215 __laser_segmentation_if = NULL;
220 Gtk::Allocation allocation = get_allocation();
221 const int width = allocation.get_width();
222 const int height = allocation.get_height();
246 __visdisp_if = visdisp_if;
257 __robot_drawer = robot_drawer;
267 __resolution = resolution;
297 if ( __zoom_factor > 20 ) {
312 __rotation = rot_rad;
317 LaserDrawingArea::all_laser_ifs_have_writer()
const
319 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
320 it != __laser_ifs.end(); ++it) {
330 #if GTK_VERSION_GE(3,0)
336 LaserDrawingArea::on_draw(
const Cairo::RefPtr<Cairo::Context> &cr)
347 Glib::RefPtr<Gdk::Window> window = get_window();
349 Gtk::Allocation allocation = get_allocation();
353 __first_draw =
false;
354 const int width = allocation.get_width();
355 const int height = allocation.get_height();
361 #if GTK_VERSION_LT(3,0)
362 Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
364 cr->set_line_width(1.0);
366 cr->set_source_rgb(1, 1, 1);
367 #if GTK_VERSION_LT(3,0)
370 cr->rectangle(event->area.x, event->area.y,
371 event->area.width, event->area.height);
377 cr->set_source_rgb(0, 0, 0);
382 cr->translate(__xc, __yc);
386 Cairo::TextExtents te;
387 std::string t =
"Not connected to BlackBoard";
388 cr->set_source_rgb(1, 0, 0);
389 cr->set_font_size(20);
390 cr->get_text_extents(t, te);
391 cr->move_to(- te.width / 2, -te.height / 2);
393 }
else if ( __laser_ifs.empty() ) {
394 Cairo::TextExtents te;
395 std::string t =
"No interface opened";
396 cr->set_source_rgb(1, 0, 0);
397 cr->set_font_size(20);
398 cr->get_text_extents(t, te);
399 cr->move_to(- te.width / 2, -te.height / 2);
401 }
else if (! all_laser_ifs_have_writer() ) {
402 Cairo::TextExtents te;
403 std::string t =
"No writer for ";
404 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
405 it != __laser_ifs.end(); ++it) {
412 cr->set_source_rgb(1, 0, 0);
413 cr->set_font_size(20);
414 cr->get_text_extents(t, te);
415 cr->move_to(- te.width / 2, -te.height / 2);
418 if (! __break_drawing) {
419 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
420 it != __laser_ifs.end(); ++it) {
426 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
427 it != __laser_ifs.end(); ++it) {
429 const Color& color = it->second;
431 cr->set_source_rgb(color.r, color.g, color.b);
432 draw_beams(laser_if, window, cr);
435 if (__robot_drawer) __robot_drawer->draw_robot(window, cr);
436 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
437 it != __laser_ifs.end(); ++it) {
439 const Color& color = it->second;
441 cr->set_source_rgb(color.r, color.g, color.b);
442 draw_segments(laser_if, window, cr);
445 draw_persons_legs(window, cr);
447 if(__switch_if != NULL && __switch_if->has_writer()){
449 __switch_if->msgq_enqueue(esm);
455 cr->rotate(0.5 * M_PI + __rotation);
456 cr->scale(-__zoom_factor, __zoom_factor);
457 cr->set_line_width(1. / __zoom_factor);
459 __visdisp->process_messages();
463 const float radius = 0.01;
466 if (__line_if->has_writer() &&
467 __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, __line_if->world_y() - radius * 0.5, radius, radius);
476 cr->rectangle(__line_if->relative_x() - radius * 0.5, __line_if->relative_y() - radius * 0.5, radius, radius);
479 cr->move_to(__line_if->world_x(), __line_if->world_y());
480 cr->line_to(__line_if->relative_x(), __line_if->relative_y());
499 const Cairo::RefPtr<Cairo::Context> &cr)
502 cr->set_source_rgba(0, 0, 0.8, 0.2);
503 cr->arc(0, 0, 1.0, 0, 2 * M_PI);
518 Glib::RefPtr<Gdk::Window> &window,
519 const Cairo::RefPtr<Cairo::Context> &cr)
526 if ((itf360 = dynamic_cast<const fawkes::Laser360Interface*>(itf))) {
530 }
else if ((itf720 = dynamic_cast<const fawkes::Laser720Interface*>(itf))) {
535 throw fawkes::Exception(
"Interface is neither Laser360Interface nor Laser720Interface");
538 const float nd_factor = 360.0 / nd;
541 float *revdists = NULL;
544 revdists = (
float *)
new float[nd];
545 for (
size_t i = 0; i < nd; ++i) {
546 revdists[nd - i] = distances[i];
548 distances = revdists;
551 cr->scale(__zoom_factor, __zoom_factor);
552 cr->rotate(__rotation);
553 cr->set_line_width(1. / __zoom_factor);
558 for (
size_t i = 0; i < nd; i += __resolution) {
559 if ( distances[i] == 0 || ! std::isfinite(distances[i]) )
continue;
560 const float anglerad =
deg2rad(i * nd_factor);
562 cr->line_to(distances[i] * sin(anglerad),
563 distances[i] * -cos(anglerad));
567 const float radius = 4 / __zoom_factor;
568 for (
size_t i = 0; i < nd; i += __resolution) {
569 if ( distances[i] == 0 )
continue;
570 float anglerad =
deg2rad(i * nd_factor);
571 float x = distances[i] * sin(anglerad);
572 float y = distances[i] * -cos(anglerad);
576 cr->rectangle(x, y, radius, radius);
581 cr->move_to(0, - distances[0]);
582 for (
size_t i = __resolution; i <= nd + __resolution; i += __resolution) {
583 if ( distances[i] == 0 )
continue;
585 cr->line_to(distances[i % nd] * sin(anglerad),
586 distances[i % nd] * -cos(anglerad));
591 if (revdists)
delete[] revdists;
603 const Cairo::RefPtr<Cairo::Context> &cr)
605 std::list<ObjectPositionInterface*>::iterator objpos_if_itt;;
608 if (__l_objpos_if_persons) {
609 cr->set_source_rgb(0,0,1);
610 for( objpos_if_itt = __l_objpos_if_persons->begin();
611 objpos_if_itt != __l_objpos_if_persons->end() && (*objpos_if_itt)->has_writer();
614 (*objpos_if_itt)->read();
615 if ((*objpos_if_itt)->is_valid()){
621 cr->arc(x, y, 0.2, 0, 2*M_PI);
627 if (__l_objpos_if_legs) {
628 cr->set_source_rgb(0,1,0);
629 for( objpos_if_itt = __l_objpos_if_legs->begin();
630 objpos_if_itt != __l_objpos_if_legs->end() && (*objpos_if_itt)->has_writer() ;
633 (*objpos_if_itt)->read();
634 if ((*objpos_if_itt)->is_valid()){
639 cr->arc(x, y, 0.1, 0, 2*M_PI);
645 if (__l_objpos_if_misc) {
646 cr->set_source_rgb(0,1,1);
647 for( objpos_if_itt = __l_objpos_if_misc->begin();
648 objpos_if_itt != __l_objpos_if_misc->end() && (*objpos_if_itt)->has_writer() ;
651 (*objpos_if_itt)->read();
652 if ((*objpos_if_itt)->is_valid()){
656 if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_BALL){
661 float begin_x=pos.first;
662 float begin_y=pos.second;
664 float end_x= pos.first;
665 float end_y= pos.first;
666 float angle1=atan2(begin_y- y, begin_x - x);
667 float angle2=atan2(end_y- y, end_x - x);
668 float radius=(*objpos_if_itt)->relative_x_velocity();
669 float probability = (*objpos_if_itt)->relative_z_velocity();
670 cr->move_to(begin_x, begin_y);
671 cr->arc(x, y, radius, angle2, angle1);
674 std::string t = StringConversions::to_string(probability);
677 cr->set_font_size(0.08);
680 cr->move_to(begin_x, begin_y);
686 }
else if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_LINE){
688 float begin_x=pos.first;
689 float begin_y=pos.second;
691 float end_x= pos.first;
692 float end_y= pos.first;
693 cr->move_to(begin_x, begin_y);
694 cr->line_to(end_x, end_y);
703 cr->set_source_rgb(1,0,1);
713 std::list<Position2DTrackInterface*>::iterator track_if_itt;;
714 const float radius (0.1);
718 float* x_positions2 = NULL;
719 float* y_positions2 = NULL;
720 unsigned int track_length1 = 0;
721 unsigned int track_length2 = 0;
722 int* timestamps2 = NULL;
724 cr->set_font_size(0.03);
725 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
726 printf(
"\n\n################################\n");
728 for( track_if_itt = __l_track_if->begin();
729 track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer();) {
730 bool b_compound_track(
false);
732 (*track_if_itt)->read();
733 if ((*track_if_itt)->is_valid()){
734 x_positions1=(*track_if_itt)->track_x_positions();
735 y_positions1=(*track_if_itt)->track_y_positions();
736 timestamps1=(*track_if_itt)->track_timestamps();
737 track_length1 = (*track_if_itt)->length();
738 id = (*track_if_itt)->track_id();
740 if( track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer()){
742 (*track_if_itt)->read();
743 if( (*track_if_itt)->is_valid() && (*track_if_itt)->track_id()==id ){
744 b_compound_track =
true;
745 x_positions2=(*track_if_itt)->track_x_positions();
746 y_positions2=(*track_if_itt)->track_y_positions();
747 timestamps2=(*track_if_itt)->track_timestamps();
748 track_length2 = (*track_if_itt)->length();
752 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
753 printf(
"\n trackid %d\n",
id);
757 float x = x_positions1[i];
758 float y = y_positions1[i];
759 if(b_compound_track){
760 while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
763 if(timestamps2[j] == timestamps1[i]){
764 x += x_positions2[i];
766 y += y_positions2[i];
771 cr->move_to(pos.first,pos.second);
772 for (; i < track_length1; ++i){
775 if(b_compound_track){
776 while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
779 if(timestamps2[j] == timestamps1[i]){
780 x += x_positions2[i];
782 y += y_positions2[i];
789 cr->line_to(pos.first, pos.second);
793 std::string t = StringConversions::to_string(timestamps1[i]);
796 cr->move_to(pos.first, pos.second);
797 #ifdef LASERGUI_DEBUG_PRINT_TRACKS
798 printf(
"( %f,%f,[%d] )", pos.first, pos.second, timestamps1[i] );
803 if (div(color_it,3).rem == 0) r+= delta;
804 if (div(color_it,3).rem == 1) g+= delta;
805 if (div(color_it,3).rem == 2) b+= delta;
806 cr->set_source_rgb(r,g,b);
812 i = std::max(0, (
int) track_length1 - CFG_PRINT_NR_TRACKELEMENTS);
814 for (; i < track_length1; ++i){
817 if(b_compound_track){
818 while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
824 cr->move_to(pos.first - radius, pos.second);
825 cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
827 if(b_compound_track && timestamps2[j] == timestamps1[i]){
828 cr->move_to(pos.first, pos.second);
831 cr->line_to(pos.first, pos.second);
832 cr->move_to(pos.first - radius, pos.second);
833 cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
836 cr->set_source_rgb(0,0,1);
850 cr->set_source_rgb(1,0,0);
857 cr->arc(x, y, radius, 0, 2*M_PI);
858 cr->move_to(x - radius, y);
859 cr->line_to(x + radius, y);
860 cr->move_to(x, y - radius );
861 cr->line_to(x, y + radius);
898 Glib::RefPtr<Gdk::Window> &window,
899 const Cairo::RefPtr<Cairo::Context> &cr)
902 const float nd_factor = 360.0 / nd;
907 if ((itf360 = dynamic_cast<const fawkes::Laser360Interface*>(itf))) {
909 }
else if ((itf720 = dynamic_cast<const fawkes::Laser720Interface*>(itf))) {
912 throw fawkes::Exception(
"Interface is neither Laser360Interface nor Laser720Interface");
917 if( __laser_segmentation_if && __laser_segmentation_if->
has_writer()){
919 __laser_segmentation_if->
read();
920 float * segmentations = __laser_segmentation_if->
distances();
923 cr->set_source_rgb(1,1,0);
926 for (
size_t i = 0; i < nd; i += __resolution) {
927 if( segmentations[i]==0)
continue;
928 if ( distances[i] == 0 || ! std::isfinite(distances[i]))
continue;
929 float anglerad =
deg2rad(i * nd_factor);
931 cr->line_to(distances[i] * sin(anglerad),
932 distances[i] * -cos(anglerad));
936 float radius = 4 / __zoom_factor;
937 for (
size_t i = 0; i < nd; i += __resolution) {
938 if( segmentations[i]==0)
continue;
939 if ( distances[i] == 0 )
continue;
940 float anglerad =
deg2rad(i * nd_factor);
941 float x = distances[i] * sin(anglerad);
942 float y = distances[i] * -cos(anglerad);
946 cr->rectangle(x, y, radius, radius);
974 if (event->direction == GDK_SCROLL_UP) {
976 }
else if (event->direction == GDK_SCROLL_DOWN) {
986 __break_drawing = ! __break_drawing;
997 __last_mouse_x =
event->x;
998 __last_mouse_y =
event->y;
1000 double user_x =
event->x;
1001 double user_y =
event->y;
1002 Glib::RefPtr<Gdk::Window> window = get_window();
1003 Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
1005 cr->translate(__xc, __yc);
1006 cr->rotate(0.5 * M_PI + __rotation);
1007 cr->scale(-__zoom_factor, __zoom_factor);
1008 cr->device_to_user(user_x, user_y);
1009 printf(
"Clicked at (%.3lf, %.3lf)\n", user_x, user_y);
1024 __xc -= __last_mouse_x -
event->x;
1025 __yc -= __last_mouse_y -
event->y;
1027 __last_mouse_x =
event->x;
1028 __last_mouse_y =
event->y;
1042 std::pair<float,float>
1044 std::pair<float,float> pos;
Laser360Interface Fawkes BlackBoard Interface.
float relative_y() const
Get relative_y value.
void draw_scalebox(Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw scale box.
Only draw beam end points.
virtual bool on_expose_event(GdkEventExpose *event)
Expose event handler.
void reset_laser_ifs()
Reset laser interfaces to "no laser available".
float relative_x() const
Get relative_x value.
ObjectPositionInterface Fawkes BlackBoard Interface.
2D visualization processor for VisualDisplay2DInterface.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
void set_robot_drawer(fawkes::CairoRobotDrawer *robot_drawer)
Set robot drawer.
Fawkes library namespace.
void set_interface(fawkes::VisualDisplay2DInterface *interface)
Set interface.
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_visdisp_if(fawkes::VisualDisplay2DInterface *visdisp_if)
Set visual display interface.
void set_objpos_if(std::list< fawkes::ObjectPositionInterface * > *l_objpos_if_persons, std::list< fawkes::ObjectPositionInterface * > *l_objpos_if_legs, std::list< fawkes::ObjectPositionInterface * > *l_objpos_if_misc, fawkes::Laser720Interface *laser_segmentation_if, std::list< fawkes::Position2DTrackInterface * > *l_track_if, fawkes::ObjectPositionInterface *target_if, fawkes::SwitchInterface *switch_if)
Set ObjectPosition interfaces.
Base class for all Fawkes BlackBoard interfaces.
void set_line_if(fawkes::ObjectPositionInterface *line_if)
Set line interface.
const char * uid() const
Get unique identifier of interface.
virtual bool on_scroll_event(GdkEventScroll *event)
Scroll event handler.
void toggle_break_drawing()
Set a member for breaking the drawing.
bool has_writer() const
Check if there is a writer for the interface.
void set_draw_mode(draw_mode_t mode)
Set the drawing mode.
~LaserDrawingArea()
Destructor.
SwitchInterface Fawkes BlackBoard Interface.
bool is_clockwise_angle() const
Get clockwise_angle value.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
void set_resolution(unsigned int resolution)
Set resolution.
void set_laser_ifs(const std::list< fawkes::Interface * > &laser_if)
Set new laser interfaces.
void set_rotation(float rot_rad)
Set rotation.
bool is_clockwise_angle() const
Get clockwise_angle value.
void draw_beams(const fawkes::Interface *itf, Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw Beams of an interface.
float * distances() const
Get distances value.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
virtual bool on_button_press_event(GdkEventButton *event)
Button press event handler.
bool is_valid() const
Get valid value.
void draw_segments(const fawkes::Interface *itf, Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw laser segments as produced by leg tracker application.
void draw_persons_legs(Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw person legs.
size_t maxlenof_distances() const
Get maximum length of distances value.
float * distances() const
Get distances value.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
size_t maxlenof_distances() const
Get maximum length of distances value.
VisualDisplay2DInterface Fawkes BlackBoard Interface.
virtual bool on_motion_notify_event(GdkEventMotion *event)
Mouse motion notify event handler.
void set_connected(bool connected)
Set connection status.
Laser720Interface Fawkes BlackBoard Interface.
LaserDrawingArea()
Constructor.