Fawkes API Fawkes Development Version

lasergui.cpp

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 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends