00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00056
00057
00058
00059 class LaserGuiGtkWindow : public Gtk::Window
00060 {
00061 public:
00062
00063
00064
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
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
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
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
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
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
00293 void on_stop_toggled()
00294 {
00295 __area->toggle_break_drawing();
00296 }
00297
00298
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
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
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
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 }