23 #include "laser_drawing_area.h" 24 #include "visdisplay.h" 25 #include <interfaces/Laser720Interface.h> 26 #include <interfaces/Laser360Interface.h> 27 #include <interfaces/Laser1080Interface.h> 28 #include <interfaces/ObjectPositionInterface.h> 29 #include <interfaces/VisualDisplay2DInterface.h> 30 #include <utils/math/angle.h> 31 #include <gui_utils/robot/drawer.h> 33 #include <utils/misc/string_conversions.h> 38 #define CFG_PRINT_NR_TRACKELEMENTS 5 54 const Glib::RefPtr<Gtk::Builder> &builder)
55 : Gtk::DrawingArea(cobject)
59 __l_objpos_if_persons = NULL;
60 __l_objpos_if_legs = NULL;
61 __l_objpos_if_misc = NULL;
62 __laser_segmentation_if = NULL;
68 __robot_drawer = NULL;
71 __break_drawing =
false;
77 add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK |
78 Gdk::BUTTON_PRESS_MASK );
80 #if GTK_VERSION_LT(3,0) 93 __l_objpos_if_persons = NULL;
94 __l_objpos_if_legs = NULL;
95 __l_objpos_if_misc = NULL;
96 __laser_segmentation_if = NULL;
102 __robot_drawer = NULL;
105 __break_drawing =
false;
109 add_events(Gdk::SCROLL_MASK | Gdk::BUTTON_MOTION_MASK);
111 #if GTK_VERSION_LT(3,0) 136 std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_legs,
137 std::list<fawkes::ObjectPositionInterface*>* l_objpos_if_misc,
139 std::list<fawkes::Position2DTrackInterface*>* l_track_if,
142 __l_objpos_if_persons = l_objpos_if_persons;
143 __l_objpos_if_legs = l_objpos_if_legs;
144 __l_objpos_if_misc = l_objpos_if_misc;
145 __laser_segmentation_if=laser_segmentation_if;
146 __l_track_if = l_track_if;
147 __target_if = target_if;
148 __switch_if = switch_if;
157 __connected = connected;
189 unsigned char color_counter = 0;
190 unsigned char intensity = 255;
191 for (std::list<fawkes::Interface*>::const_iterator it = ifs.begin();
192 it != ifs.end(); ++it) {
193 if ((color_counter & 0x1 & 0x2 & 0x4) != 0) {
197 c.r = ((color_counter & 0x1) != 0) ? intensity : 0;
198 c.g = ((color_counter & 0x2) != 0) ? intensity : 0;
199 c.b = ((color_counter & 0x4) != 0) ? intensity : 0;
200 const InterfaceColorPair p = std::make_pair(*it, c);
201 __laser_ifs.push_back(p);
213 __l_objpos_if_persons = NULL;
214 __l_objpos_if_legs = NULL;
215 __l_objpos_if_misc = NULL;
216 __laser_segmentation_if = NULL;
221 Gtk::Allocation allocation = get_allocation();
222 const int width = allocation.get_width();
223 const int height = allocation.get_height();
247 __visdisp_if = visdisp_if;
258 __robot_drawer = robot_drawer;
268 __resolution = resolution;
298 if ( __zoom_factor > 20 ) {
313 __rotation = rot_rad;
318 LaserDrawingArea::all_laser_ifs_have_writer()
const 320 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
321 it != __laser_ifs.end(); ++it) {
331 #if GTK_VERSION_GE(3,0) 337 LaserDrawingArea::on_draw(
const Cairo::RefPtr<Cairo::Context> &cr)
348 Glib::RefPtr<Gdk::Window> window = get_window();
350 Gtk::Allocation allocation = get_allocation();
354 __first_draw =
false;
355 const int width = allocation.get_width();
356 const int height = allocation.get_height();
362 #if GTK_VERSION_LT(3,0) 363 Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
365 cr->set_line_width(1.0);
367 cr->set_source_rgb(1, 1, 1);
368 #if GTK_VERSION_LT(3,0) 371 cr->rectangle(event->area.x, event->area.y,
372 event->area.width, event->area.height);
378 cr->set_source_rgb(0, 0, 0);
383 cr->translate(__xc, __yc);
387 Cairo::TextExtents te;
388 std::string t =
"Not connected to BlackBoard";
389 cr->set_source_rgb(1, 0, 0);
390 cr->set_font_size(20);
391 cr->get_text_extents(t, te);
392 cr->move_to(- te.width / 2, -te.height / 2);
394 }
else if ( __laser_ifs.empty() ) {
395 Cairo::TextExtents te;
396 std::string t =
"No interface opened";
397 cr->set_source_rgb(1, 0, 0);
398 cr->set_font_size(20);
399 cr->get_text_extents(t, te);
400 cr->move_to(- te.width / 2, -te.height / 2);
402 }
else if (! all_laser_ifs_have_writer() ) {
403 Cairo::TextExtents te;
404 std::string t =
"No writer for ";
405 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
406 it != __laser_ifs.end(); ++it) {
413 cr->set_source_rgb(1, 0, 0);
414 cr->set_font_size(20);
415 cr->get_text_extents(t, te);
416 cr->move_to(- te.width / 2, -te.height / 2);
419 if (! __break_drawing) {
420 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
421 it != __laser_ifs.end(); ++it) {
427 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
428 it != __laser_ifs.end(); ++it) {
430 const Color& color = it->second;
432 cr->set_source_rgb(color.r, color.g, color.b);
436 if (__robot_drawer) __robot_drawer->
draw_robot(window, cr);
437 for (std::list<InterfaceColorPair>::const_iterator it = __laser_ifs.begin();
438 it != __laser_ifs.end(); ++it) {
440 const Color& color = it->second;
442 cr->set_source_rgb(color.r, color.g, color.b);
448 if(__switch_if != NULL && __switch_if->
has_writer()){
456 cr->rotate(0.5 * M_PI + __rotation);
457 cr->scale(-__zoom_factor, __zoom_factor);
458 cr->set_line_width(1. / __zoom_factor);
464 const float radius = 0.01;
470 cr->set_source_rgb(1, 0, 0);
476 cr->rectangle(__line_if->
world_x() - radius * 0.5, __line_if->
world_y() - radius * 0.5, radius, radius);
477 cr->rectangle(__line_if->
relative_x() - radius * 0.5, __line_if->
relative_y() - radius * 0.5, radius, radius);
500 const Cairo::RefPtr<Cairo::Context> &cr)
503 cr->set_source_rgba(0, 0, 0.8, 0.2);
504 cr->arc(0, 0, 1.0, 0, 2 * M_PI);
519 Glib::RefPtr<Gdk::Window> &window,
520 const Cairo::RefPtr<Cairo::Context> &cr)
528 if ((itf360 = dynamic_cast<const fawkes::Laser360Interface*>(itf))) {
532 }
else if ((itf720 = dynamic_cast<const fawkes::Laser720Interface*>(itf))) {
536 }
else if ((itf1080 = dynamic_cast<const fawkes::Laser1080Interface*>(itf))) {
541 throw fawkes::Exception(
"Interface is neither Laser360Interface nor Laser720Interface");
544 const float nd_factor = 360.0 / nd;
547 float *revdists = NULL;
550 revdists = (
float *)
new float[nd];
551 for (
size_t i = 0; i < nd; ++i) {
552 revdists[nd - i - 1] = distances[i];
554 distances = revdists;
557 cr->scale(__zoom_factor, __zoom_factor);
558 cr->rotate(__rotation);
559 cr->set_line_width(1. / __zoom_factor);
564 for (
size_t i = 0; i < nd; i += __resolution) {
565 if ( distances[i] == 0 || ! std::isfinite(distances[i]) )
continue;
566 const float anglerad =
deg2rad(i * nd_factor);
568 cr->line_to(distances[i] * sin(anglerad),
569 distances[i] * -cos(anglerad));
573 const float radius = 4 / __zoom_factor;
574 for (
size_t i = 0; i < nd; i += __resolution) {
575 if ( distances[i] == 0 )
continue;
576 float anglerad =
deg2rad(i * nd_factor);
577 float x = distances[i] * sin(anglerad);
578 float y = distances[i] * -cos(anglerad);
582 cr->rectangle(x, y, radius, radius);
587 cr->move_to(0, - distances[0]);
588 for (
size_t i = __resolution; i <= nd + __resolution; i += __resolution) {
589 if ( distances[i] == 0 )
continue;
591 cr->line_to(distances[i % nd] * sin(anglerad),
592 distances[i % nd] * -cos(anglerad));
597 if (revdists)
delete[] revdists;
609 const Cairo::RefPtr<Cairo::Context> &cr)
611 std::list<ObjectPositionInterface*>::iterator objpos_if_itt;;
614 if (__l_objpos_if_persons) {
615 cr->set_source_rgb(0,0,1);
616 for( objpos_if_itt = __l_objpos_if_persons->begin();
617 objpos_if_itt != __l_objpos_if_persons->end() && (*objpos_if_itt)->has_writer();
620 (*objpos_if_itt)->read();
621 if ((*objpos_if_itt)->is_valid()){
627 cr->arc(x, y, 0.2, 0, 2*M_PI);
633 if (__l_objpos_if_legs) {
634 cr->set_source_rgb(0,1,0);
635 for( objpos_if_itt = __l_objpos_if_legs->begin();
636 objpos_if_itt != __l_objpos_if_legs->end() && (*objpos_if_itt)->has_writer() ;
639 (*objpos_if_itt)->read();
640 if ((*objpos_if_itt)->is_valid()){
645 cr->arc(x, y, 0.1, 0, 2*M_PI);
651 if (__l_objpos_if_misc) {
652 cr->set_source_rgb(0,1,1);
653 for( objpos_if_itt = __l_objpos_if_misc->begin();
654 objpos_if_itt != __l_objpos_if_misc->end() && (*objpos_if_itt)->has_writer() ;
657 (*objpos_if_itt)->read();
658 if ((*objpos_if_itt)->is_valid()){
662 if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_BALL){
667 float begin_x=pos.first;
668 float begin_y=pos.second;
670 float end_x= pos.first;
671 float end_y= pos.first;
672 float angle1=atan2(begin_y- y, begin_x - x);
673 float angle2=atan2(end_y- y, end_x - x);
674 float radius=(*objpos_if_itt)->relative_x_velocity();
675 float probability = (*objpos_if_itt)->relative_z_velocity();
676 cr->move_to(begin_x, begin_y);
677 cr->arc(x, y, radius, angle2, angle1);
680 std::string t = StringConversions::to_string(probability);
683 cr->set_font_size(0.08);
686 cr->move_to(begin_x, begin_y);
692 }
else if((*objpos_if_itt)->object_type()==ObjectPositionInterface::TYPE_LINE){
694 float begin_x=pos.first;
695 float begin_y=pos.second;
697 float end_x= pos.first;
698 float end_y= pos.first;
699 cr->move_to(begin_x, begin_y);
700 cr->line_to(end_x, end_y);
709 cr->set_source_rgb(1,0,1);
719 std::list<Position2DTrackInterface*>::iterator track_if_itt;;
720 const float radius (0.1);
724 float* x_positions2 = NULL;
725 float* y_positions2 = NULL;
726 unsigned int track_length1 = 0;
727 unsigned int track_length2 = 0;
728 int* timestamps2 = NULL;
730 cr->set_font_size(0.03);
731 #ifdef LASERGUI_DEBUG_PRINT_TRACKS 732 printf(
"\n\n################################\n");
734 for( track_if_itt = __l_track_if->begin();
735 track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer();) {
736 bool b_compound_track(
false);
738 (*track_if_itt)->read();
739 if ((*track_if_itt)->is_valid()){
740 x_positions1=(*track_if_itt)->track_x_positions();
741 y_positions1=(*track_if_itt)->track_y_positions();
742 timestamps1=(*track_if_itt)->track_timestamps();
743 track_length1 = (*track_if_itt)->length();
744 id = (*track_if_itt)->track_id();
746 if( track_if_itt != __l_track_if->end() && (*track_if_itt)->has_writer()){
748 (*track_if_itt)->read();
749 if( (*track_if_itt)->is_valid() && (*track_if_itt)->track_id()==id ){
750 b_compound_track =
true;
751 x_positions2=(*track_if_itt)->track_x_positions();
752 y_positions2=(*track_if_itt)->track_y_positions();
753 timestamps2=(*track_if_itt)->track_timestamps();
754 track_length2 = (*track_if_itt)->length();
758 #ifdef LASERGUI_DEBUG_PRINT_TRACKS 759 printf(
"\n trackid %d\n",
id);
763 float x = x_positions1[i];
764 float y = y_positions1[i];
765 if(b_compound_track){
766 while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
769 if(timestamps2[j] == timestamps1[i]){
770 x += x_positions2[i];
772 y += y_positions2[i];
777 cr->move_to(pos.first,pos.second);
778 for (; i < track_length1; ++i){
781 if(b_compound_track){
782 while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
785 if(timestamps2[j] == timestamps1[i]){
786 x += x_positions2[i];
788 y += y_positions2[i];
795 cr->line_to(pos.first, pos.second);
799 std::string t = StringConversions::to_string(timestamps1[i]);
802 cr->move_to(pos.first, pos.second);
803 #ifdef LASERGUI_DEBUG_PRINT_TRACKS 804 printf(
"( %f,%f,[%d] )", pos.first, pos.second, timestamps1[i] );
809 if (div(color_it,3).rem == 0) r+= delta;
810 if (div(color_it,3).rem == 1) g+= delta;
811 if (div(color_it,3).rem == 2) b+= delta;
812 cr->set_source_rgb(r,g,b);
818 i = std::max(0, (
int) track_length1 - CFG_PRINT_NR_TRACKELEMENTS);
820 for (; i < track_length1; ++i){
823 if(b_compound_track){
824 while(j+1 < track_length2 && timestamps2[j] < timestamps1[i]){
830 cr->move_to(pos.first - radius, pos.second);
831 cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
833 if(b_compound_track && timestamps2[j] == timestamps1[i]){
834 cr->move_to(pos.first, pos.second);
837 cr->line_to(pos.first, pos.second);
838 cr->move_to(pos.first - radius, pos.second);
839 cr->arc(pos.first, pos.second, radius, 0, 2*M_PI);
842 cr->set_source_rgb(0,0,1);
856 cr->set_source_rgb(1,0,0);
863 cr->arc(x, y, radius, 0, 2*M_PI);
864 cr->move_to(x - radius, y);
865 cr->line_to(x + radius, y);
866 cr->move_to(x, y - radius );
867 cr->line_to(x, y + radius);
904 Glib::RefPtr<Gdk::Window> &window,
905 const Cairo::RefPtr<Cairo::Context> &cr)
908 const float nd_factor = 360.0 / nd;
914 if ((itf360 = dynamic_cast<const fawkes::Laser360Interface*>(itf))) {
916 }
else if ((itf720 = dynamic_cast<const fawkes::Laser720Interface*>(itf))) {
918 }
else if ((itf1080 = dynamic_cast<const fawkes::Laser1080Interface*>(itf))) {
921 throw fawkes::Exception(
"Interface is neither Laser360Interface nor Laser720Interface");
926 if( __laser_segmentation_if && __laser_segmentation_if->
has_writer()){
928 __laser_segmentation_if->
read();
929 float * segmentations = __laser_segmentation_if->
distances();
932 cr->set_source_rgb(1,1,0);
935 for (
size_t i = 0; i < nd; i += __resolution) {
936 if( segmentations[i]==0)
continue;
937 if ( distances[i] == 0 || ! std::isfinite(distances[i]))
continue;
938 float anglerad =
deg2rad(i * nd_factor);
940 cr->line_to(distances[i] * sin(anglerad),
941 distances[i] * -cos(anglerad));
945 float radius = 4 / __zoom_factor;
946 for (
size_t i = 0; i < nd; i += __resolution) {
947 if( segmentations[i]==0)
continue;
948 if ( distances[i] == 0 )
continue;
949 float anglerad =
deg2rad(i * nd_factor);
950 float x = distances[i] * sin(anglerad);
951 float y = distances[i] * -cos(anglerad);
955 cr->rectangle(x, y, radius, radius);
983 if (event->direction == GDK_SCROLL_UP) {
985 }
else if (event->direction == GDK_SCROLL_DOWN) {
995 __break_drawing = ! __break_drawing;
1006 __last_mouse_x =
event->x;
1007 __last_mouse_y =
event->y;
1009 double user_x =
event->x;
1010 double user_y =
event->y;
1011 Glib::RefPtr<Gdk::Window> window = get_window();
1012 Cairo::RefPtr<Cairo::Context> cr = window->create_cairo_context();
1014 cr->translate(__xc, __yc);
1015 cr->rotate(0.5 * M_PI + __rotation);
1016 cr->scale(-__zoom_factor, __zoom_factor);
1017 cr->device_to_user(user_x, user_y);
1018 printf(
"Clicked at (%.3lf, %.3lf)\n", user_x, user_y);
1033 __xc -= __last_mouse_x -
event->x;
1034 __yc -= __last_mouse_y -
event->y;
1036 __last_mouse_x =
event->x;
1037 __last_mouse_y =
event->y;
1051 std::pair<float,float>
1053 std::pair<float,float> pos;
Laser360Interface Fawkes BlackBoard Interface.
size_t maxlenof_distances() const
Get maximum length of distances value.
bool is_clockwise_angle() const
Get clockwise_angle 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.
bool is_valid() const
Get valid value.
bool is_clockwise_angle() const
Get clockwise_angle value.
Laser1080Interface Fawkes BlackBoard Interface.
void reset_laser_ifs()
Reset laser interfaces to "no laser available".
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_laser_ifs(const std::list< fawkes::Interface *> &laser_if)
Set new laser interfaces.
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.
Base class for all Fawkes BlackBoard interfaces.
void set_line_if(fawkes::ObjectPositionInterface *line_if)
Set line interface.
void process_messages()
Process messages.
float world_y() const
Get world_y value.
virtual bool on_scroll_event(GdkEventScroll *event)
Scroll event handler.
void toggle_break_drawing()
Set a member for breaking the drawing.
void set_draw_mode(draw_mode_t mode)
Set the drawing mode.
~LaserDrawingArea()
Destructor.
SwitchInterface Fawkes BlackBoard Interface.
bool is_visible() const
Get visible value.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
void set_resolution(unsigned int resolution)
Set resolution.
virtual void draw_robot(Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)=0
Draw robot.
bool has_writer() const
Check if there is a writer for the interface.
const char * uid() const
Get unique identifier of interface.
void set_rotation(float rot_rad)
Set rotation.
float * distances() const
Get distances value.
void draw_beams(const fawkes::Interface *itf, Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw Beams of an interface.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
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.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
virtual bool on_button_press_event(GdkEventButton *event)
Button press event handler.
float * distances() const
Get distances value.
void draw(Cairo::RefPtr< Cairo::Context > cr)
Draw objects.
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.
float world_x() const
Get world_x value.
void draw_persons_legs(Glib::RefPtr< Gdk::Window > &window, const Cairo::RefPtr< Cairo::Context > &cr)
Draw person legs.
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.
size_t maxlenof_distances() const
Get maximum length of distances value.
bool is_clockwise_angle() const
Get clockwise_angle value.
float relative_y() const
Get relative_y value.
LaserDrawingArea()
Constructor.
float relative_x() const
Get relative_x value.