24 #include "laser_drawing_area.h"
26 #include <blackboard/remote.h>
27 #include <gtkmm/main.h>
28 #include <gui_utils/connection_dispatcher.h>
29 #include <gui_utils/interface_dispatcher.h>
30 #include <gui_utils/multi_interface_chooser_dialog.h>
31 #include <gui_utils/robot/allemaniacs_athome.h>
32 #include <gui_utils/service_chooser_dialog.h>
33 #include <interfaces/Laser1080Interface.h>
34 #include <interfaces/Laser360Interface.h>
35 #include <interfaces/Laser720Interface.h>
36 #include <interfaces/ObjectPositionInterface.h>
37 #include <interfaces/Position2DTrackInterface.h>
38 #include <interfaces/SwitchInterface.h>
39 #include <interfaces/VisualDisplay2DInterface.h>
40 #include <netcomm/fawkes/client.h>
41 #include <utils/misc/string_conversions.h>
48 #define MAX_OBJECTPOSITIONINTERFACES_PERSONS 10
49 #define MAX_OBJECTPOSITIONINTERFACES_LEGS 15
50 #define MAX_OBJECTPOSITIONINTERFACES_MISC 20
51 #define MAX_TRACKINTERFACES 10
81 : Gtk::Window(cobject), athome_drawer_(true)
83 laser_if_names_.push_back(std::make_pair(
"Laser360Interface",
"Laser"));
85 builder->get_widget_derived(
"da_laser", area_);
86 builder->get_widget(
"tb_connection", tb_connection_);
87 builder->get_widget(
"tb_select", tb_select_);
88 builder->get_widget(
"tb_lines", tb_lines_);
89 builder->get_widget(
"tb_points", tb_points_);
90 builder->get_widget(
"tb_hull", tb_hull_);
91 builder->get_widget(
"tb_trimvals", tb_trimvals_);
92 builder->get_widget(
"tb_rotation", tb_rotation_);
93 builder->get_widget(
"tb_legtracker", tb_legtracker_);
94 builder->get_widget(
"tb_stop", tb_stop_);
95 builder->get_widget(
"tb_zoom_in", tb_zoom_in_);
96 builder->get_widget(
"tb_zoom_out", tb_zoom_out_);
97 builder->get_widget(
"tb_exit", tb_exit_);
98 builder->get_widget(
"dlg_ltopen", dlg_ltopen_);
99 builder->get_widget(
"pgb_ltopen", pgb_ltopen_);
101 area_->set_robot_drawer(&athome_drawer_);
103 tb_select_->set_sensitive(
false);
104 tb_lines_->set_sensitive(
false);
105 tb_points_->set_sensitive(
false);
106 tb_hull_->set_sensitive(
false);
107 tb_trimvals_->set_sensitive(
false);
108 tb_rotation_->set_sensitive(
false);
109 tb_legtracker_->set_sensitive(
false);
110 tb_stop_->set_sensitive(
false);
111 tb_zoom_in_->set_sensitive(
false);
112 tb_zoom_out_->set_sensitive(
false);
114 tb_connection_->signal_clicked().connect(
116 tb_select_->signal_clicked().connect(
118 tb_lines_->signal_toggled().connect(
121 tb_points_->signal_toggled().connect(
124 tb_hull_->signal_toggled().connect(
130 tb_legtracker_->signal_clicked().connect(
132 tb_trimvals_->signal_clicked().connect(
134 tb_rotation_->signal_clicked().connect(
139 connection_dispatcher_.signal_connected().connect(
141 connection_dispatcher_.signal_disconnected().connect(
150 if (!connection_dispatcher_.get_client()->connected()) {
154 connection_dispatcher_.get_client()->disconnect();
162 if (!connection_dispatcher_.get_client()->connected()) {
163 Gtk::MessageDialog md(*
this,
164 "Cannot get list of interfaces if not connected.",
169 md.set_title(
"Interface Selection Failed");
172 #if __cplusplus >= 201103L
173 std::unique_ptr<MultiInterfaceChooserDialog> ifcd(
175 std::auto_ptr<MultiInterfaceChooserDialog> ifcd(
179 const TypeIdPairList interfaces = ifcd->get_selected_interfaces();
180 open_interfaces(interfaces);
193 area_->reset_laser_ifs();
194 for (InterfaceDispatcherPairList::const_iterator it = laser_ifs_.begin();
195 it != laser_ifs_.end();
197 bb_->unregister_listener(it->second);
199 bb_->close(it->first);
202 laser_if_names_ = types_and_ids;
205 for (TypeIdPairList::const_iterator it = types_and_ids.begin(); it != types_and_ids.end();
207 const Glib::ustring &type = it->first;
208 const Glib::ustring &
id = it->second;
211 if (type ==
"Laser1080Interface") {
213 }
else if (type ==
"Laser720Interface") {
215 }
else if (type ==
"Laser360Interface") {
218 throw Exception(
"Invalid interface type %s", type.c_str());
221 std::string msg = std::string(
"Failed to open interface: ") + e.
what();
222 Gtk::MessageDialog md(*
this,
228 md.set_title(
"Opening Interface Failed");
234 sigc::hide(sigc::mem_fun(*area_, &LaserDrawingArea::queue_draw)));
238 std::string msg = std::string(
"Failed to register interface dispatcher: ") + e.
what();
239 Gtk::MessageDialog md(*
this,
245 md.set_title(
"Registrating Interface Dispatcher Failed");
252 laser_ifs_.push_back(p);
256 std::list<Interface *> keys;
257 for (InterfaceDispatcherPairList::const_iterator it = laser_ifs_.begin();
258 it != laser_ifs_.end();
260 keys.push_back(it->first);
262 area_->set_laser_ifs(keys);
272 l_objpos_if_persons_ = NULL;
273 l_objpos_if_legs_ = NULL;
274 l_objpos_if_misc_ = NULL;
276 laser_segmentation_if_ = NULL;
284 area_->set_connected(
true);
285 open_interfaces(laser_if_names_);
288 area_->set_line_if(line_if_);
291 area_->set_visdisp_if(visdis_if_);
298 on_legtracker_toggled();
302 tb_connection_->set_stock_id(Gtk::Stock::DISCONNECT);
303 tb_select_->set_sensitive(
true);
304 tb_lines_->set_sensitive(
true);
305 tb_points_->set_sensitive(
true);
306 tb_hull_->set_sensitive(
true);
307 tb_trimvals_->set_sensitive(
true);
308 tb_rotation_->set_sensitive(
true);
309 tb_legtracker_->set_sensitive(
true);
310 tb_stop_->set_sensitive(
true);
311 tb_zoom_in_->set_sensitive(
true);
312 tb_zoom_out_->set_sensitive(
true);
314 area_->reset_laser_ifs();
315 area_->set_line_if(NULL);
316 area_->set_visdisp_if(NULL);
318 area_->set_connected(
false);
320 area_->reset_laser_ifs();
321 for (InterfaceDispatcherPairList::const_iterator it = laser_ifs_.begin();
322 it != laser_ifs_.end();
324 bb_->unregister_listener(it->second);
326 bb_->close(it->first);
328 bb_->close(line_if_);
329 bb_->close(visdis_if_);
343 area_->set_connected(
false);
344 area_->reset_laser_ifs();
345 area_->set_line_if(NULL);
346 area_->set_visdisp_if(NULL);
348 for (InterfaceDispatcherPairList::const_iterator it = laser_ifs_.begin();
349 it != laser_ifs_.end();
351 bb_->unregister_listener(it->second);
353 bb_->close(it->first);
356 if (laser_segmentation_if_)
357 bb_->close(laser_segmentation_if_);
359 bb_->close(switch_if_);
361 bb_->close(target_if_);
362 bb_->close(line_if_);
363 bb_->close(visdis_if_);
365 std::list<ObjectPositionInterface *>::iterator objpos_if_itt;
366 std::list<Position2DTrackInterface *>::iterator track_if_itt;
367 if (l_objpos_if_persons_) {
368 for (objpos_if_itt = l_objpos_if_persons_->begin();
369 objpos_if_itt != l_objpos_if_persons_->end();
371 bb_->close(*objpos_if_itt);
373 l_objpos_if_persons_->clear();
375 if (l_objpos_if_legs_) {
376 for (objpos_if_itt = l_objpos_if_legs_->begin(); objpos_if_itt != l_objpos_if_legs_->end();
378 bb_->close(*objpos_if_itt);
380 l_objpos_if_legs_->clear();
382 if (l_objpos_if_misc_) {
383 for (objpos_if_itt = l_objpos_if_misc_->begin(); objpos_if_itt != l_objpos_if_misc_->end();
385 bb_->close(*objpos_if_itt);
387 l_objpos_if_misc_->clear();
390 for (track_if_itt = l_track_if_->begin(); track_if_itt != l_track_if_->end();
392 bb_->close(*track_if_itt);
394 l_track_if_->clear();
400 l_objpos_if_persons_ = NULL;
401 l_objpos_if_legs_ = NULL;
402 l_objpos_if_misc_ = NULL;
404 laser_segmentation_if_ = NULL;
410 tb_connection_->set_stock_id(Gtk::Stock::CONNECT);
411 tb_select_->set_sensitive(
false);
412 tb_lines_->set_sensitive(
false);
413 tb_points_->set_sensitive(
false);
414 tb_hull_->set_sensitive(
false);
415 tb_trimvals_->set_sensitive(
false);
416 tb_rotation_->set_sensitive(
false);
417 tb_legtracker_->set_sensitive(
false);
418 tb_stop_->set_sensitive(
false);
419 tb_zoom_in_->set_sensitive(
false);
420 tb_zoom_out_->set_sensitive(
false);
427 if (tb_rotation_->get_active()) {
428 area_->set_rotation(M_PI / 2);
430 area_->set_rotation(0);
438 area_->toggle_break_drawing();
448 if (!tb_legtracker_->get_active()) {
449 bb_->close(laser_segmentation_if_);
450 bb_->close(switch_if_);
451 bb_->close(target_if_);
453 std::list<ObjectPositionInterface *>::iterator objpos_if_itt;
454 std::list<Position2DTrackInterface *>::iterator track_if_itt;
455 if (l_objpos_if_persons_) {
456 for (objpos_if_itt = l_objpos_if_persons_->begin();
457 objpos_if_itt != l_objpos_if_persons_->end();
459 bb_->close(*objpos_if_itt);
461 l_objpos_if_persons_->clear();
463 if (l_objpos_if_legs_) {
464 for (objpos_if_itt = l_objpos_if_legs_->begin(); objpos_if_itt != l_objpos_if_legs_->end();
466 bb_->close(*objpos_if_itt);
468 l_objpos_if_legs_->clear();
470 if (l_objpos_if_misc_) {
471 for (objpos_if_itt = l_objpos_if_misc_->begin(); objpos_if_itt != l_objpos_if_misc_->end();
473 bb_->close(*objpos_if_itt);
475 l_objpos_if_misc_->clear();
479 for (track_if_itt = l_track_if_->begin(); track_if_itt != l_track_if_->end();
481 bb_->close(*track_if_itt);
483 l_track_if_->clear();
486 laser_segmentation_if_ = NULL;
489 l_objpos_if_persons_ = NULL;
490 l_objpos_if_legs_ = NULL;
491 l_objpos_if_misc_ = NULL;
494 area_->set_objpos_if(l_objpos_if_persons_,
497 laser_segmentation_if_,
503 unsigned int num_opens = 3 + MAX_OBJECTPOSITIONINTERFACES_PERSONS
504 + MAX_OBJECTPOSITIONINTERFACES_LEGS
505 + MAX_OBJECTPOSITIONINTERFACES_MISC + MAX_TRACKINTERFACES;
507 float step_fraction = 1.0 / num_opens;
508 unsigned int opened = 0;
509 pgb_ltopen_->set_fraction(0);
513 laser_segmentation_if_ = bb_->open_for_reading<
Laser720Interface>(
"SegmentsLaser");
514 pgb_ltopen_->set_fraction(++opened * step_fraction);
515 while (Gtk::Main::events_pending())
516 Gtk::Main::iteration();
521 l_objpos_if_persons_ =
new std::list<ObjectPositionInterface *>();
522 l_objpos_if_legs_ =
new std::list<ObjectPositionInterface *>();
523 l_objpos_if_misc_ =
new std::list<ObjectPositionInterface *>();
524 l_track_if_ =
new std::list<Position2DTrackInterface *>();
525 for (
int i = 1; i <= MAX_OBJECTPOSITIONINTERFACES_PERSONS; ++i) {
528 l_objpos_if_persons_->push_back(new_objpos_if);
529 pgb_ltopen_->set_fraction(++opened * step_fraction);
530 while (Gtk::Main::events_pending())
531 Gtk::Main::iteration();
533 for (
int i = 1; i <= MAX_OBJECTPOSITIONINTERFACES_LEGS; ++i) {
536 l_objpos_if_legs_->push_back(new_objpos_if);
537 pgb_ltopen_->set_fraction(++opened * step_fraction);
538 while (Gtk::Main::events_pending())
539 Gtk::Main::iteration();
541 for (
int i = 1; i <= MAX_OBJECTPOSITIONINTERFACES_MISC; ++i) {
544 l_objpos_if_misc_->push_back(new_objpos_if);
545 pgb_ltopen_->set_fraction(++opened * step_fraction);
546 while (Gtk::Main::events_pending())
547 Gtk::Main::iteration();
549 for (
int i = 1; i <= MAX_TRACKINTERFACES; ++i) {
552 l_track_if_->push_back(new_track_if);
553 pgb_ltopen_->set_fraction(++opened * step_fraction);
554 while (Gtk::Main::events_pending())
555 Gtk::Main::iteration();
558 switch_if_ = bb_->open_for_reading<
SwitchInterface>(
"legtracker write!");
559 pgb_ltopen_->set_fraction(++opened * step_fraction);
560 while (Gtk::Main::events_pending())
561 Gtk::Main::iteration();
563 area_->set_objpos_if(l_objpos_if_persons_,
566 laser_segmentation_if_,
578 if (tb_trimvals_->get_active()) {
579 area_->set_resolution(3);
581 area_->set_resolution(1);
594 InterfaceDispatcherPairList laser_ifs_;
599 std::list<ObjectPositionInterface *> * l_objpos_if_persons_;
600 std::list<ObjectPositionInterface *> * l_objpos_if_legs_;
601 std::list<ObjectPositionInterface *> * l_objpos_if_misc_;
602 std::list<Position2DTrackInterface *> *l_track_if_;
611 Gtk::ToolButton * tb_connection_;
612 Gtk::RadioToolButton * tb_lines_;
613 Gtk::RadioToolButton * tb_points_;
614 Gtk::RadioToolButton * tb_hull_;
615 Gtk::ToggleToolButton *tb_trimvals_;
616 Gtk::ToggleToolButton *tb_rotation_;
617 Gtk::ToggleToolButton *tb_legtracker_;
618 Gtk::ToggleToolButton *tb_stop_;
619 Gtk::ToolButton * tb_zoom_in_;
620 Gtk::ToolButton * tb_zoom_out_;
621 Gtk::ToolButton * tb_exit_;
622 Gtk::ToolButton * tb_select_;
624 Gtk::Dialog * dlg_ltopen_;
625 Gtk::ProgressBar *pgb_ltopen_;
627 TypeIdPairList laser_if_names_;
631 main(
int argc,
char **argv)
633 Gtk::Main kit(argc, argv);
638 Glib::RefPtr<Gtk::Builder> builder;
639 builder = Gtk::Builder::create_from_file(RESDIR
"/guis/lasergui/lasergui.ui");
642 builder->get_widget_derived(
"wnd_lasergui", window);
644 Gtk::Main::run(*window);