Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * lasergui.cpp - minimalistic laser visualization 00004 * 00005 * Created: Thu Oct 09 12:51:52 2008 00006 * Copyright 2008 Tim Niemueller [www.niemueller.de] 00007 * 2009 Masrur Doostdar <doostdar@kbsg.rwth-aachen.de> 00008 * 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL file in the doc directory. 00022 */ 00023 00024 #include "laser_drawing_area.h" 00025 00026 #include <netcomm/fawkes/client.h> 00027 #include <blackboard/remote.h> 00028 #include <interfaces/Laser360Interface.h> 00029 #include <interfaces/Laser720Interface.h> 00030 00031 #include <interfaces/ObjectPositionInterface.h> 00032 #include <interfaces/Position2DTrackInterface.h> 00033 #include <interfaces/SwitchInterface.h> 00034 #include <interfaces/VisualDisplay2DInterface.h> 00035 00036 00037 #include <gui_utils/service_chooser_dialog.h> 00038 #include <gui_utils/interface_dispatcher.h> 00039 #include <gui_utils/connection_dispatcher.h> 00040 #include <gui_utils/robot/allemaniacs_athome.h> 00041 00042 #include <gtkmm/main.h> 00043 #include <libglademm/xml.h> 00044 #include <list> 00045 #include <utils/misc/string_conversions.h> 00046 00047 00048 #define MAX_OBJECTPOSITIONINTERFACES_PERSONS 10 00049 #define MAX_OBJECTPOSITIONINTERFACES_LEGS 15 00050 #define MAX_OBJECTPOSITIONINTERFACES_MISC 20 00051 #define MAX_TRACKINTERFACES 10 00052 00053 using namespace fawkes; 00054 00055 /** @class LaserGuiGtkWindow "lasergui.cpp" 00056 * Laser GUI window for Gtkmm. 00057 * @author Tim Niemueller 00058 */ 00059 class LaserGuiGtkWindow : public Gtk::Window 00060 { 00061 public: 00062 /** Constructor for Glademm. 00063 * @param cobject C base object 00064 * @param refxml reference to Glade's Xml parser 00065 */ 00066 LaserGuiGtkWindow(BaseObjectType* cobject, 00067 const Glib::RefPtr<Gnome::Glade::Xml> &refxml) 00068 : Gtk::Window(cobject), __athome_drawer(true) 00069 { 00070 refxml->get_widget_derived("da_laser", __area); 00071 refxml->get_widget("tb_connection", __tb_connection); 00072 refxml->get_widget("tb_lines", __tb_lines); 00073 refxml->get_widget("tb_points", __tb_points); 00074 refxml->get_widget("tb_hull", __tb_hull); 00075 refxml->get_widget("tb_highres", __tb_highres); 00076 refxml->get_widget("tb_trimvals", __tb_trimvals); 00077 refxml->get_widget("tb_rotation", __tb_rotation); 00078 refxml->get_widget("tb_legtracker", __tb_legtracker); 00079 refxml->get_widget("tb_stop", __tb_stop); 00080 refxml->get_widget("tb_zoom_in", __tb_zoom_in); 00081 refxml->get_widget("tb_zoom_out", __tb_zoom_out); 00082 refxml->get_widget("tb_exit", __tb_exit); 00083 refxml->get_widget("dlg_ltopen", __dlg_ltopen); 00084 refxml->get_widget("pgb_ltopen", __pgb_ltopen); 00085 00086 __area->set_robot_drawer(&__athome_drawer); 00087 00088 __tb_lines->set_sensitive(false); 00089 __tb_points->set_sensitive(false); 00090 __tb_hull->set_sensitive(false); 00091 __tb_highres->set_sensitive(false); 00092 __tb_trimvals->set_sensitive(false); 00093 __tb_rotation->set_sensitive(false); 00094 __tb_legtracker->set_sensitive(false); 00095 __tb_stop->set_sensitive(false); 00096 __tb_zoom_in->set_sensitive(false); 00097 __tb_zoom_out->set_sensitive(false); 00098 00099 __tb_connection->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_connection_clicked)); 00100 __tb_lines->signal_toggled().connect(sigc::bind(sigc::mem_fun(*__area, &LaserDrawingArea::set_draw_mode), LaserDrawingArea::MODE_LINES)); 00101 __tb_points->signal_toggled().connect(sigc::bind(sigc::mem_fun(*__area, &LaserDrawingArea::set_draw_mode), LaserDrawingArea::MODE_POINTS)); 00102 __tb_hull->signal_toggled().connect(sigc::bind(sigc::mem_fun(*__area, &LaserDrawingArea::set_draw_mode), LaserDrawingArea::MODE_HULL)); 00103 __tb_zoom_in->signal_clicked().connect(sigc::mem_fun(*__area, &LaserDrawingArea::zoom_in)); 00104 __tb_zoom_out->signal_clicked().connect(sigc::mem_fun(*__area, &LaserDrawingArea::zoom_out)); 00105 00106 __tb_highres->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_resolution_toggled)); 00107 __tb_legtracker->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_legtracker_toggled)); 00108 __tb_trimvals->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_trimvals_toggled)); 00109 __tb_rotation->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_rotation_toggled)); 00110 __tb_stop->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_stop_toggled)); 00111 __tb_exit->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_exit_clicked)); 00112 00113 __connection_dispatcher.signal_connected().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_connect)); 00114 __connection_dispatcher.signal_disconnected().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_disconnect)); 00115 } 00116 00117 00118 protected: 00119 /** Event handler for connection button. */ 00120 virtual void on_connection_clicked() 00121 { 00122 if ( ! __connection_dispatcher.get_client()->connected() ) { 00123 ServiceChooserDialog ssd(*this, __connection_dispatcher.get_client()); 00124 ssd.run_and_connect(); 00125 } else { 00126 __connection_dispatcher.get_client()->disconnect(); 00127 } 00128 00129 } 00130 00131 /** Event handler for connected event. */ 00132 virtual void on_connect() 00133 { 00134 try { 00135 __bb = new RemoteBlackBoard(__connection_dispatcher.get_client()); 00136 __laser360_if = NULL; 00137 __laser720_if = NULL; 00138 __l_objpos_if_persons = NULL; 00139 __l_objpos_if_legs = NULL; 00140 __l_objpos_if_misc = NULL; 00141 __l_track_if = NULL; 00142 __laser_segmentation_if = NULL; 00143 __switch_if = NULL; 00144 __target_if = NULL; 00145 __line_if = NULL; 00146 __visdis_if = NULL; 00147 00148 //__laser_if = __bb->open_for_reading<Laser360Interface>("LegtrackerAveragedLaser"); 00149 00150 00151 if (__tb_highres->get_active()) { 00152 __laser720_if = __bb->open_for_reading<Laser720Interface>("Laser"); 00153 __area->set_laser720_if(__laser720_if); 00154 __ifd = new InterfaceDispatcher("LaserInterfaceDispatcher", __laser720_if); 00155 } else { 00156 __laser360_if = __bb->open_for_reading<Laser360Interface>("Laser"); 00157 __area->set_laser360_if(__laser360_if); 00158 __ifd = new InterfaceDispatcher("LaserInterfaceDispatcher", __laser360_if); 00159 } 00160 00161 __line_if = __bb->open_for_reading<ObjectPositionInterface>("LaserLine"); 00162 __area->set_line_if(__line_if); 00163 __visdis_if = __bb->open_for_writing<VisualDisplay2DInterface>("LaserGUI"); 00164 __area->set_visdisp_if(__visdis_if); 00165 00166 on_legtracker_toggled(); 00167 00168 __ifd->signal_data_changed().connect(sigc::hide(sigc::mem_fun(*__area, &LaserDrawingArea::queue_draw))); 00169 __bb->register_listener(__ifd, BlackBoard::BBIL_FLAG_DATA); 00170 00171 __area->queue_draw(); 00172 00173 __tb_connection->set_stock_id(Gtk::Stock::DISCONNECT); 00174 __tb_lines->set_sensitive(true); 00175 __tb_points->set_sensitive(true); 00176 __tb_hull->set_sensitive(true); 00177 __tb_highres->set_sensitive(true); 00178 __tb_trimvals->set_sensitive(true); 00179 __tb_rotation->set_sensitive(true); 00180 __tb_legtracker->set_sensitive(true); 00181 __tb_stop->set_sensitive(true); 00182 __tb_zoom_in->set_sensitive(true); 00183 __tb_zoom_out->set_sensitive(true); 00184 } catch (Exception &e) { 00185 if ( __bb ) { 00186 __bb->close(__laser360_if); 00187 __bb->close(__laser720_if); 00188 __bb->close(__line_if); 00189 __bb->close(__visdis_if); 00190 delete __ifd; 00191 delete __bb; 00192 __laser360_if = NULL; 00193 __laser720_if = NULL; 00194 __bb = NULL; 00195 __ifd = NULL; 00196 __line_if = NULL; 00197 __visdis_if = NULL; 00198 } 00199 } 00200 } 00201 00202 /** Event handler for disconnected event. */ 00203 virtual void on_disconnect() 00204 { 00205 __area->reset_laser_ifs(); 00206 __area->set_line_if(NULL); 00207 __area->set_visdisp_if(NULL); 00208 __area->queue_draw(); 00209 if(__laser360_if) 00210 __bb->close(__laser360_if); 00211 if(__laser720_if) 00212 __bb->close(__laser720_if); 00213 if(__laser_segmentation_if) 00214 __bb->close(__laser_segmentation_if); 00215 if(__switch_if) 00216 __bb->close(__switch_if); 00217 if(__target_if) 00218 __bb->close(__target_if); 00219 __bb->close(__line_if); 00220 __bb->close(__visdis_if); 00221 00222 std::list<ObjectPositionInterface*>::iterator objpos_if_itt; 00223 std::list<Position2DTrackInterface*>::iterator track_if_itt; 00224 if(__l_objpos_if_persons){ 00225 for( objpos_if_itt = __l_objpos_if_persons->begin(); objpos_if_itt != __l_objpos_if_persons->end(); objpos_if_itt++ ) { 00226 __bb->close(*objpos_if_itt); 00227 } 00228 __l_objpos_if_persons->clear(); 00229 } 00230 if(__l_objpos_if_legs){ 00231 for( objpos_if_itt = __l_objpos_if_legs->begin(); objpos_if_itt != __l_objpos_if_legs->end(); objpos_if_itt++ ) { 00232 __bb->close(*objpos_if_itt); 00233 } 00234 __l_objpos_if_legs->clear(); 00235 } 00236 if(__l_objpos_if_misc){ 00237 for( objpos_if_itt = __l_objpos_if_misc->begin(); objpos_if_itt != __l_objpos_if_misc->end(); objpos_if_itt++ ) { 00238 __bb->close(*objpos_if_itt); 00239 } 00240 __l_objpos_if_misc->clear(); 00241 } 00242 if(__l_track_if){ 00243 for( track_if_itt = __l_track_if->begin(); track_if_itt != __l_track_if->end(); track_if_itt++ ) { 00244 __bb->close(*track_if_itt); 00245 } 00246 __l_track_if->clear(); 00247 } 00248 00249 00250 00251 delete __bb; 00252 delete __ifd; 00253 __bb = NULL; 00254 __ifd = NULL; 00255 __laser360_if = NULL; 00256 __laser720_if = NULL; 00257 __l_objpos_if_persons = NULL; 00258 __l_objpos_if_legs = NULL; 00259 __l_objpos_if_misc = NULL; 00260 __l_track_if = NULL; 00261 __laser_segmentation_if = NULL; 00262 __switch_if = NULL; 00263 __target_if = NULL; 00264 __visdis_if = NULL; 00265 __line_if = NULL; 00266 00267 __tb_connection->set_stock_id(Gtk::Stock::CONNECT); 00268 __tb_lines->set_sensitive(false); 00269 __tb_points->set_sensitive(false); 00270 __tb_hull->set_sensitive(false); 00271 __tb_highres->set_sensitive(false); 00272 __tb_trimvals->set_sensitive(false); 00273 __tb_rotation->set_sensitive(false); 00274 __tb_legtracker->set_sensitive(false); 00275 __tb_stop->set_sensitive(false); 00276 __tb_zoom_in->set_sensitive(false); 00277 __tb_zoom_out->set_sensitive(false); 00278 } 00279 00280 00281 /** Event handler for rotation button. */ 00282 void on_rotation_toggled() 00283 { 00284 if ( __tb_rotation->get_active() ) { 00285 __area->set_rotation(M_PI / 2); 00286 } else { 00287 __area->set_rotation(0); 00288 } 00289 } 00290 00291 00292 /** Event handler for stop button */ 00293 void on_stop_toggled() 00294 { 00295 __area->toggle_break_drawing(); 00296 } 00297 00298 /** Event handler for resolution button. */ 00299 void on_resolution_toggled() 00300 { 00301 if (! __bb) return; 00302 00303 try { 00304 __bb->close(__laser360_if); 00305 __bb->close(__laser720_if); 00306 __bb->unregister_listener(__ifd); 00307 delete __ifd; 00308 __laser360_if = NULL; 00309 __laser720_if = NULL; 00310 00311 if ( __tb_highres->get_active() ) { 00312 __laser720_if = __bb->open_for_reading<Laser720Interface>("Laser"); 00313 __ifd = new InterfaceDispatcher("LaserInterfaceDispatcher", __laser720_if); 00314 __ifd->signal_data_changed().connect(sigc::hide(sigc::mem_fun(*__area, &LaserDrawingArea::queue_draw))); 00315 __bb->register_listener(__ifd, BlackBoard::BBIL_FLAG_DATA); 00316 __area->set_laser720_if(__laser720_if); 00317 } else { 00318 __laser360_if = __bb->open_for_reading<Laser360Interface>("Laser"); 00319 __ifd = new InterfaceDispatcher("LaserInterfaceDispatcher", __laser360_if); 00320 __ifd->signal_data_changed().connect(sigc::hide(sigc::mem_fun(*__area, &LaserDrawingArea::queue_draw))); 00321 __bb->register_listener(__ifd, BlackBoard::BBIL_FLAG_DATA); 00322 __area->set_laser360_if(__laser360_if); 00323 } 00324 __area->queue_draw(); 00325 } catch (Exception &e) { 00326 e.print_trace(); 00327 } 00328 } 00329 00330 /** Event handler for legtracker button */ 00331 void on_legtracker_toggled() 00332 { 00333 if (! __bb) return; 00334 00335 if (!__tb_legtracker->get_active()) { 00336 __bb->close(__laser_segmentation_if); 00337 __bb->close(__switch_if); 00338 __bb->close(__target_if); 00339 00340 std::list<ObjectPositionInterface*>::iterator objpos_if_itt; 00341 std::list<Position2DTrackInterface*>::iterator track_if_itt; 00342 if (__l_objpos_if_persons) { 00343 for( objpos_if_itt = __l_objpos_if_persons->begin(); objpos_if_itt != __l_objpos_if_persons->end(); objpos_if_itt++ ) { 00344 __bb->close(*objpos_if_itt); 00345 } 00346 __l_objpos_if_persons->clear(); 00347 } 00348 if (__l_objpos_if_legs) { 00349 for( objpos_if_itt = __l_objpos_if_legs->begin(); objpos_if_itt != __l_objpos_if_legs->end(); objpos_if_itt++ ) { 00350 __bb->close(*objpos_if_itt); 00351 } 00352 __l_objpos_if_legs->clear(); 00353 } 00354 if (__l_objpos_if_misc) { 00355 for( objpos_if_itt = __l_objpos_if_misc->begin(); objpos_if_itt != __l_objpos_if_misc->end(); objpos_if_itt++ ) { 00356 __bb->close(*objpos_if_itt); 00357 } 00358 __l_objpos_if_misc->clear(); 00359 } 00360 00361 if (__l_track_if) { 00362 for( track_if_itt = __l_track_if->begin(); track_if_itt != __l_track_if->end(); track_if_itt++ ) { 00363 __bb->close(*track_if_itt); 00364 } 00365 __l_track_if->clear(); 00366 } 00367 00368 __laser_segmentation_if = NULL; 00369 __switch_if = NULL; 00370 __target_if = NULL; 00371 __l_objpos_if_persons = NULL; 00372 __l_objpos_if_legs = NULL; 00373 __l_objpos_if_misc = NULL; 00374 __l_track_if = NULL; 00375 00376 __area->set_objpos_if(__l_objpos_if_persons,__l_objpos_if_legs,__l_objpos_if_misc,__laser_segmentation_if, __l_track_if, __target_if,__switch_if); 00377 00378 } else { 00379 unsigned int num_opens = 3 00380 + MAX_OBJECTPOSITIONINTERFACES_PERSONS 00381 + MAX_OBJECTPOSITIONINTERFACES_LEGS 00382 + MAX_OBJECTPOSITIONINTERFACES_MISC 00383 + MAX_TRACKINTERFACES; 00384 00385 float step_fraction = 1.0 / num_opens; 00386 unsigned int opened = 0; 00387 __pgb_ltopen->set_fraction(0); 00388 __dlg_ltopen->show(); 00389 __area->queue_draw(); 00390 00391 __laser_segmentation_if = __bb->open_for_reading<Laser720Interface>("SegmentsLaser"); 00392 __pgb_ltopen->set_fraction(++opened * step_fraction); 00393 while (Gtk::Main::events_pending()) Gtk::Main::iteration(); 00394 00395 __target_if = __bb->open_for_reading<ObjectPositionInterface>("legtracker Target"); 00396 00397 ObjectPositionInterface* new_objpos_if; 00398 __l_objpos_if_persons = new std::list<ObjectPositionInterface*>(); 00399 __l_objpos_if_legs = new std::list<ObjectPositionInterface*>(); 00400 __l_objpos_if_misc = new std::list<ObjectPositionInterface*>(); 00401 __l_track_if = new std::list<Position2DTrackInterface*>(); 00402 for (int i=1; i <= MAX_OBJECTPOSITIONINTERFACES_PERSONS; ++i){ 00403 new_objpos_if= __bb->open_for_reading<ObjectPositionInterface>((std::string("legtracker CurrentLegsTracked") + StringConversions::to_string(i)).c_str()); 00404 __l_objpos_if_persons->push_back(new_objpos_if); 00405 __pgb_ltopen->set_fraction(++opened * step_fraction); 00406 while (Gtk::Main::events_pending()) Gtk::Main::iteration(); 00407 } 00408 for (int i=1; i <= MAX_OBJECTPOSITIONINTERFACES_LEGS; ++i){ 00409 new_objpos_if= __bb->open_for_reading<ObjectPositionInterface>((std::string("legtracker Leg") + StringConversions::to_string(i)).c_str()); 00410 __l_objpos_if_legs->push_back(new_objpos_if); 00411 __pgb_ltopen->set_fraction(++opened * step_fraction); 00412 while (Gtk::Main::events_pending()) Gtk::Main::iteration(); 00413 } 00414 for (int i=1; i <= MAX_OBJECTPOSITIONINTERFACES_MISC; ++i){ 00415 new_objpos_if= __bb->open_for_reading<ObjectPositionInterface>((std::string("legtracker Misc") + StringConversions::to_string(i)).c_str()); 00416 __l_objpos_if_misc->push_back(new_objpos_if); 00417 __pgb_ltopen->set_fraction(++opened * step_fraction); 00418 while (Gtk::Main::events_pending()) Gtk::Main::iteration(); 00419 } 00420 Position2DTrackInterface* new_track_if; 00421 for (int i=1; i <= MAX_TRACKINTERFACES; ++i){ 00422 new_track_if = __bb->open_for_reading<Position2DTrackInterface>((std::string("legtracker Track") + StringConversions::to_string(i)).c_str()); 00423 __l_track_if->push_back( new_track_if ); 00424 __pgb_ltopen->set_fraction(++opened * step_fraction); 00425 while (Gtk::Main::events_pending()) Gtk::Main::iteration(); 00426 } 00427 00428 __switch_if = __bb->open_for_reading<SwitchInterface>("legtracker write!"); 00429 __pgb_ltopen->set_fraction(++opened * step_fraction); 00430 while (Gtk::Main::events_pending()) Gtk::Main::iteration(); 00431 __dlg_ltopen->hide(); 00432 __area->set_objpos_if(__l_objpos_if_persons, __l_objpos_if_legs, 00433 __l_objpos_if_misc,__laser_segmentation_if, 00434 __l_track_if, __target_if,__switch_if); 00435 __area->queue_draw(); 00436 } 00437 } 00438 00439 00440 /** Event handler for trim button. */ 00441 void on_trimvals_toggled() 00442 { 00443 if ( __tb_trimvals->get_active() ) { 00444 __area->set_resolution(3); 00445 } else { 00446 __area->set_resolution(1); 00447 } 00448 } 00449 00450 /** Event handler for exit button. */ 00451 void on_exit_clicked() 00452 { 00453 Gtk::Main::quit(); 00454 } 00455 00456 private: 00457 BlackBoard *__bb; 00458 Laser360Interface *__laser360_if; 00459 Laser720Interface *__laser720_if; 00460 Laser720Interface *__laser_segmentation_if; 00461 SwitchInterface *__switch_if; 00462 ObjectPositionInterface *__target_if; 00463 00464 InterfaceDispatcher *__ifd; 00465 std::list<ObjectPositionInterface*>* __l_objpos_if_persons; 00466 std::list<ObjectPositionInterface*>* __l_objpos_if_legs; 00467 std::list<ObjectPositionInterface*>* __l_objpos_if_misc; 00468 std::list<Position2DTrackInterface*>* __l_track_if; 00469 00470 ObjectPositionInterface *__line_if; 00471 VisualDisplay2DInterface *__visdis_if; 00472 00473 LaserDrawingArea *__area; 00474 AllemaniACsAtHomeCairoRobotDrawer __athome_drawer; 00475 ConnectionDispatcher __connection_dispatcher; 00476 00477 Gtk::ToolButton *__tb_connection; 00478 Gtk::RadioToolButton *__tb_lines; 00479 Gtk::RadioToolButton *__tb_points; 00480 Gtk::RadioToolButton *__tb_hull; 00481 Gtk::ToggleToolButton *__tb_highres; 00482 Gtk::ToggleToolButton *__tb_trimvals; 00483 Gtk::ToggleToolButton *__tb_rotation; 00484 Gtk::ToggleToolButton *__tb_legtracker; 00485 Gtk::ToggleToolButton *__tb_stop; 00486 Gtk::ToolButton *__tb_zoom_in; 00487 Gtk::ToolButton *__tb_zoom_out; 00488 Gtk::ToolButton *__tb_exit; 00489 00490 Gtk::Dialog *__dlg_ltopen; 00491 Gtk::ProgressBar *__pgb_ltopen; 00492 }; 00493 00494 int 00495 main(int argc, char** argv) 00496 { 00497 Gtk::Main kit(argc, argv); 00498 00499 Glib::RefPtr<Gnome::Glade::Xml> refxml; 00500 refxml = Gnome::Glade::Xml::create(RESDIR"/guis/lasergui/lasergui.glade"); 00501 00502 LaserGuiGtkWindow *window = NULL; 00503 refxml->get_widget_derived("wnd_lasergui", window); 00504 00505 Gtk::Main::run(*window); 00506 00507 return 0; 00508 }