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
00035
00036 #include <gui_utils/service_chooser_dialog.h>
00037 #include <gui_utils/interface_dispatcher.h>
00038 #include <gui_utils/connection_dispatcher.h>
00039 #include <gui_utils/robot/allemaniacs_athome.h>
00040
00041 #include <gtkmm/main.h>
00042 #include <libglademm/xml.h>
00043 #include <list>
00044 #include <utils/misc/string_conversions.h>
00045
00046
00047 #define MAX_OBJECTPOSITIONINTERFACES_PERSONS 10
00048 #define MAX_OBJECTPOSITIONINTERFACES_LEGS 15
00049 #define MAX_OBJECTPOSITIONINTERFACES_MISC 20
00050 #define MAX_TRACKINTERFACES 10
00051
00052 using namespace fawkes;
00053
00054
00055
00056
00057
00058 class LaserGuiGtkWindow : public Gtk::Window
00059 {
00060 public:
00061
00062
00063
00064
00065 LaserGuiGtkWindow(BaseObjectType* cobject,
00066 const Glib::RefPtr<Gnome::Glade::Xml> &refxml)
00067 : Gtk::Window(cobject), __athome_drawer(true)
00068 {
00069 refxml->get_widget_derived("da_laser", __area);
00070 refxml->get_widget("tb_connection", __tb_connection);
00071 refxml->get_widget("tb_lines", __tb_lines);
00072 refxml->get_widget("tb_points", __tb_points);
00073 refxml->get_widget("tb_hull", __tb_hull);
00074 refxml->get_widget("tb_highres", __tb_highres);
00075 refxml->get_widget("tb_trimvals", __tb_trimvals);
00076 refxml->get_widget("tb_rotation", __tb_rotation);
00077 refxml->get_widget("tb_legtracker", __tb_legtracker);
00078 refxml->get_widget("tb_stop", __tb_stop);
00079 refxml->get_widget("tb_zoom_in", __tb_zoom_in);
00080 refxml->get_widget("tb_zoom_out", __tb_zoom_out);
00081 refxml->get_widget("tb_exit", __tb_exit);
00082 refxml->get_widget("dlg_ltopen", __dlg_ltopen);
00083 refxml->get_widget("pgb_ltopen", __pgb_ltopen);
00084
00085 __area->set_robot_drawer(&__athome_drawer);
00086
00087 __tb_lines->set_sensitive(false);
00088 __tb_points->set_sensitive(false);
00089 __tb_hull->set_sensitive(false);
00090 __tb_highres->set_sensitive(false);
00091 __tb_trimvals->set_sensitive(false);
00092 __tb_rotation->set_sensitive(false);
00093 __tb_legtracker->set_sensitive(false);
00094 __tb_stop->set_sensitive(false);
00095 __tb_zoom_in->set_sensitive(false);
00096 __tb_zoom_out->set_sensitive(false);
00097
00098 __tb_connection->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_connection_clicked));
00099 __tb_lines->signal_toggled().connect(sigc::bind(sigc::mem_fun(*__area, &LaserDrawingArea::set_draw_mode), LaserDrawingArea::MODE_LINES));
00100 __tb_points->signal_toggled().connect(sigc::bind(sigc::mem_fun(*__area, &LaserDrawingArea::set_draw_mode), LaserDrawingArea::MODE_POINTS));
00101 __tb_hull->signal_toggled().connect(sigc::bind(sigc::mem_fun(*__area, &LaserDrawingArea::set_draw_mode), LaserDrawingArea::MODE_HULL));
00102 __tb_zoom_in->signal_clicked().connect(sigc::mem_fun(*__area, &LaserDrawingArea::zoom_in));
00103 __tb_zoom_out->signal_clicked().connect(sigc::mem_fun(*__area, &LaserDrawingArea::zoom_out));
00104
00105 __tb_highres->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_resolution_toggled));
00106 __tb_legtracker->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_legtracker_toggled));
00107 __tb_trimvals->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_trimvals_toggled));
00108 __tb_rotation->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_rotation_toggled));
00109 __tb_stop->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_stop_toggled));
00110 __tb_exit->signal_clicked().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_exit_clicked));
00111
00112 __connection_dispatcher.signal_connected().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_connect));
00113 __connection_dispatcher.signal_disconnected().connect(sigc::mem_fun(*this, &LaserGuiGtkWindow::on_disconnect));
00114 }
00115
00116
00117 protected:
00118
00119 virtual void on_connection_clicked()
00120 {
00121 if ( ! __connection_dispatcher.get_client()->connected() ) {
00122 ServiceChooserDialog ssd(*this, __connection_dispatcher.get_client());
00123 ssd.run_and_connect();
00124 } else {
00125 __connection_dispatcher.get_client()->disconnect();
00126 }
00127
00128 }
00129
00130
00131 virtual void on_connect()
00132 {
00133 try {
00134 __bb = new RemoteBlackBoard(__connection_dispatcher.get_client());
00135 __laser360_if = NULL;
00136 __laser720_if = NULL;
00137 __l_objpos_if_persons = NULL;
00138 __l_objpos_if_legs = NULL;
00139 __l_objpos_if_misc = NULL;
00140 __l_track_if = NULL;
00141 __laser_segmentation_if = NULL;
00142 __switch_if = NULL;
00143 __target_if = NULL;
00144 __line_if = NULL;
00145
00146
00147
00148
00149 if (__tb_highres->get_active()) {
00150 __laser720_if = __bb->open_for_reading<Laser720Interface>("Laser");
00151 __area->set_laser720_if(__laser720_if);
00152 __ifd = new InterfaceDispatcher("LaserInterfaceDispatcher", __laser720_if);
00153 } else {
00154 __laser360_if = __bb->open_for_reading<Laser360Interface>("Laser");
00155 __area->set_laser360_if(__laser360_if);
00156 __ifd = new InterfaceDispatcher("LaserInterfaceDispatcher", __laser360_if);
00157 }
00158
00159 __line_if = __bb->open_for_reading<ObjectPositionInterface>("LaserLine");
00160 __area->set_line_if(__line_if);
00161
00162 on_legtracker_toggled();
00163
00164 __ifd->signal_data_changed().connect(sigc::hide(sigc::mem_fun(*__area, &LaserDrawingArea::queue_draw)));
00165 __bb->register_listener(__ifd, BlackBoard::BBIL_FLAG_DATA);
00166
00167 __area->queue_draw();
00168
00169 __tb_connection->set_stock_id(Gtk::Stock::DISCONNECT);
00170 __tb_lines->set_sensitive(true);
00171 __tb_points->set_sensitive(true);
00172 __tb_hull->set_sensitive(true);
00173 __tb_highres->set_sensitive(true);
00174 __tb_trimvals->set_sensitive(true);
00175 __tb_rotation->set_sensitive(true);
00176 __tb_legtracker->set_sensitive(true);
00177 __tb_stop->set_sensitive(true);
00178 __tb_zoom_in->set_sensitive(true);
00179 __tb_zoom_out->set_sensitive(true);
00180 } catch (Exception &e) {
00181 if ( __bb ) {
00182 __bb->close(__laser360_if);
00183 __bb->close(__laser720_if);
00184 __bb->close(__line_if);
00185 delete __ifd;
00186 delete __bb;
00187 __laser360_if = NULL;
00188 __laser720_if = NULL;
00189 __bb = NULL;
00190 __ifd = NULL;
00191 __line_if = NULL;
00192 }
00193 }
00194 }
00195
00196
00197 virtual void on_disconnect()
00198 {
00199 __area->reset_laser_ifs();
00200 __area->set_line_if(NULL);
00201 __area->queue_draw();
00202 if(__laser360_if)
00203 __bb->close(__laser360_if);
00204 if(__laser720_if)
00205 __bb->close(__laser720_if);
00206 if(__laser_segmentation_if)
00207 __bb->close(__laser_segmentation_if);
00208 if(__switch_if)
00209 __bb->close(__switch_if);
00210 if(__target_if)
00211 __bb->close(__target_if);
00212 __bb->close(__line_if);
00213
00214 std::list<ObjectPositionInterface*>::iterator objpos_if_itt;
00215 std::list<Position2DTrackInterface*>::iterator track_if_itt;
00216 if(__l_objpos_if_persons){
00217 for( objpos_if_itt = __l_objpos_if_persons->begin(); objpos_if_itt != __l_objpos_if_persons->end(); objpos_if_itt++ ) {
00218 __bb->close(*objpos_if_itt);
00219 }
00220 __l_objpos_if_persons->clear();
00221 }
00222 if(__l_objpos_if_legs){
00223 for( objpos_if_itt = __l_objpos_if_legs->begin(); objpos_if_itt != __l_objpos_if_legs->end(); objpos_if_itt++ ) {
00224 __bb->close(*objpos_if_itt);
00225 }
00226 __l_objpos_if_legs->clear();
00227 }
00228 if(__l_objpos_if_misc){
00229 for( objpos_if_itt = __l_objpos_if_misc->begin(); objpos_if_itt != __l_objpos_if_misc->end(); objpos_if_itt++ ) {
00230 __bb->close(*objpos_if_itt);
00231 }
00232 __l_objpos_if_misc->clear();
00233 }
00234 if(__l_track_if){
00235 for( track_if_itt = __l_track_if->begin(); track_if_itt != __l_track_if->end(); track_if_itt++ ) {
00236 __bb->close(*track_if_itt);
00237 }
00238 __l_track_if->clear();
00239 }
00240
00241
00242
00243 delete __bb;
00244 delete __ifd;
00245 __bb = NULL;
00246 __ifd = NULL;
00247 __laser360_if = NULL;
00248 __laser720_if = NULL;
00249 __l_objpos_if_persons = NULL;
00250 __l_objpos_if_legs = NULL;
00251 __l_objpos_if_misc = NULL;
00252 __l_track_if = NULL;
00253 __laser_segmentation_if = NULL;
00254 __switch_if = NULL;
00255 __target_if = NULL;
00256 __line_if = NULL;
00257
00258 __tb_connection->set_stock_id(Gtk::Stock::CONNECT);
00259 __tb_lines->set_sensitive(false);
00260 __tb_points->set_sensitive(false);
00261 __tb_hull->set_sensitive(false);
00262 __tb_highres->set_sensitive(false);
00263 __tb_trimvals->set_sensitive(false);
00264 __tb_rotation->set_sensitive(false);
00265 __tb_legtracker->set_sensitive(false);
00266 __tb_stop->set_sensitive(false);
00267 __tb_zoom_in->set_sensitive(false);
00268 __tb_zoom_out->set_sensitive(false);
00269 }
00270
00271
00272
00273 void on_rotation_toggled()
00274 {
00275 if ( __tb_rotation->get_active() ) {
00276 __area->set_rotation(M_PI / 2);
00277 } else {
00278 __area->set_rotation(0);
00279 }
00280 }
00281
00282
00283
00284 void on_stop_toggled()
00285 {
00286 __area->toggle_break_drawing();
00287 }
00288
00289
00290 void on_resolution_toggled()
00291 {
00292 if (! __bb) return;
00293
00294 try {
00295 __bb->close(__laser360_if);
00296 __bb->close(__laser720_if);
00297 __bb->unregister_listener(__ifd);
00298 delete __ifd;
00299 __laser360_if = NULL;
00300 __laser720_if = NULL;
00301
00302 if ( __tb_highres->get_active() ) {
00303 __laser720_if = __bb->open_for_reading<Laser720Interface>("Laser");
00304 __ifd = new InterfaceDispatcher("LaserInterfaceDispatcher", __laser720_if);
00305 __ifd->signal_data_changed().connect(sigc::hide(sigc::mem_fun(*__area, &LaserDrawingArea::queue_draw)));
00306 __bb->register_listener(__ifd, BlackBoard::BBIL_FLAG_DATA);
00307 __area->set_laser720_if(__laser720_if);
00308 } else {
00309 __laser360_if = __bb->open_for_reading<Laser360Interface>("Laser");
00310 __ifd = new InterfaceDispatcher("LaserInterfaceDispatcher", __laser360_if);
00311 __ifd->signal_data_changed().connect(sigc::hide(sigc::mem_fun(*__area, &LaserDrawingArea::queue_draw)));
00312 __bb->register_listener(__ifd, BlackBoard::BBIL_FLAG_DATA);
00313 __area->set_laser360_if(__laser360_if);
00314 }
00315 __area->queue_draw();
00316 } catch (Exception &e) {
00317 e.print_trace();
00318 }
00319 }
00320
00321
00322 void on_legtracker_toggled()
00323 {
00324 if (! __bb) return;
00325
00326 if (!__tb_legtracker->get_active()) {
00327 __bb->close(__laser_segmentation_if);
00328 __bb->close(__switch_if);
00329 __bb->close(__target_if);
00330
00331 std::list<ObjectPositionInterface*>::iterator objpos_if_itt;
00332 std::list<Position2DTrackInterface*>::iterator track_if_itt;
00333 if (__l_objpos_if_persons) {
00334 for( objpos_if_itt = __l_objpos_if_persons->begin(); objpos_if_itt != __l_objpos_if_persons->end(); objpos_if_itt++ ) {
00335 __bb->close(*objpos_if_itt);
00336 }
00337 __l_objpos_if_persons->clear();
00338 }
00339 if (__l_objpos_if_legs) {
00340 for( objpos_if_itt = __l_objpos_if_legs->begin(); objpos_if_itt != __l_objpos_if_legs->end(); objpos_if_itt++ ) {
00341 __bb->close(*objpos_if_itt);
00342 }
00343 __l_objpos_if_legs->clear();
00344 }
00345 if (__l_objpos_if_misc) {
00346 for( objpos_if_itt = __l_objpos_if_misc->begin(); objpos_if_itt != __l_objpos_if_misc->end(); objpos_if_itt++ ) {
00347 __bb->close(*objpos_if_itt);
00348 }
00349 __l_objpos_if_misc->clear();
00350 }
00351
00352 if (__l_track_if) {
00353 for( track_if_itt = __l_track_if->begin(); track_if_itt != __l_track_if->end(); track_if_itt++ ) {
00354 __bb->close(*track_if_itt);
00355 }
00356 __l_track_if->clear();
00357 }
00358
00359 __laser_segmentation_if = NULL;
00360 __switch_if = NULL;
00361 __target_if = NULL;
00362 __l_objpos_if_persons = NULL;
00363 __l_objpos_if_legs = NULL;
00364 __l_objpos_if_misc = NULL;
00365 __l_track_if = NULL;
00366
00367 __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);
00368
00369 } else {
00370 unsigned int num_opens = 3
00371 + MAX_OBJECTPOSITIONINTERFACES_PERSONS
00372 + MAX_OBJECTPOSITIONINTERFACES_LEGS
00373 + MAX_OBJECTPOSITIONINTERFACES_MISC
00374 + MAX_TRACKINTERFACES;
00375
00376 float step_fraction = 1.0 / num_opens;
00377 unsigned int opened = 0;
00378 __pgb_ltopen->set_fraction(0);
00379 __dlg_ltopen->show();
00380 __area->queue_draw();
00381
00382 __laser_segmentation_if = __bb->open_for_reading<Laser720Interface>("SegmentsLaser");
00383 __pgb_ltopen->set_fraction(++opened * step_fraction);
00384 while (Gtk::Main::events_pending()) Gtk::Main::iteration();
00385
00386 __target_if = __bb->open_for_reading<ObjectPositionInterface>("legtracker Target");
00387
00388 ObjectPositionInterface* new_objpos_if;
00389 __l_objpos_if_persons = new std::list<ObjectPositionInterface*>();
00390 __l_objpos_if_legs = new std::list<ObjectPositionInterface*>();
00391 __l_objpos_if_misc = new std::list<ObjectPositionInterface*>();
00392 __l_track_if = new std::list<Position2DTrackInterface*>();
00393 for (int i=1; i <= MAX_OBJECTPOSITIONINTERFACES_PERSONS; ++i){
00394 new_objpos_if= __bb->open_for_reading<ObjectPositionInterface>((std::string("legtracker CurrentLegsTracked") + StringConversions::to_string(i)).c_str());
00395 __l_objpos_if_persons->push_back(new_objpos_if);
00396 __pgb_ltopen->set_fraction(++opened * step_fraction);
00397 while (Gtk::Main::events_pending()) Gtk::Main::iteration();
00398 }
00399 for (int i=1; i <= MAX_OBJECTPOSITIONINTERFACES_LEGS; ++i){
00400 new_objpos_if= __bb->open_for_reading<ObjectPositionInterface>((std::string("legtracker Leg") + StringConversions::to_string(i)).c_str());
00401 __l_objpos_if_legs->push_back(new_objpos_if);
00402 __pgb_ltopen->set_fraction(++opened * step_fraction);
00403 while (Gtk::Main::events_pending()) Gtk::Main::iteration();
00404 }
00405 for (int i=1; i <= MAX_OBJECTPOSITIONINTERFACES_MISC; ++i){
00406 new_objpos_if= __bb->open_for_reading<ObjectPositionInterface>((std::string("legtracker Misc") + StringConversions::to_string(i)).c_str());
00407 __l_objpos_if_misc->push_back(new_objpos_if);
00408 __pgb_ltopen->set_fraction(++opened * step_fraction);
00409 while (Gtk::Main::events_pending()) Gtk::Main::iteration();
00410 }
00411 Position2DTrackInterface* new_track_if;
00412 for (int i=1; i <= MAX_TRACKINTERFACES; ++i){
00413 new_track_if = __bb->open_for_reading<Position2DTrackInterface>((std::string("legtracker Track") + StringConversions::to_string(i)).c_str());
00414 __l_track_if->push_back( new_track_if );
00415 __pgb_ltopen->set_fraction(++opened * step_fraction);
00416 while (Gtk::Main::events_pending()) Gtk::Main::iteration();
00417 }
00418
00419 __switch_if = __bb->open_for_reading<SwitchInterface>("legtracker write!");
00420 __pgb_ltopen->set_fraction(++opened * step_fraction);
00421 while (Gtk::Main::events_pending()) Gtk::Main::iteration();
00422 __dlg_ltopen->hide();
00423 __area->set_objpos_if(__l_objpos_if_persons, __l_objpos_if_legs,
00424 __l_objpos_if_misc,__laser_segmentation_if,
00425 __l_track_if, __target_if,__switch_if);
00426 __area->queue_draw();
00427 }
00428 }
00429
00430
00431
00432 void on_trimvals_toggled()
00433 {
00434 if ( __tb_trimvals->get_active() ) {
00435 __area->set_resolution(3);
00436 } else {
00437 __area->set_resolution(1);
00438 }
00439 }
00440
00441
00442 void on_exit_clicked()
00443 {
00444 Gtk::Main::quit();
00445 }
00446
00447 private:
00448 BlackBoard *__bb;
00449 Laser360Interface *__laser360_if;
00450 Laser720Interface *__laser720_if;
00451 Laser720Interface *__laser_segmentation_if;
00452 SwitchInterface *__switch_if;
00453 ObjectPositionInterface *__target_if;
00454
00455 InterfaceDispatcher *__ifd;
00456 std::list<ObjectPositionInterface*>* __l_objpos_if_persons;
00457 std::list<ObjectPositionInterface*>* __l_objpos_if_legs;
00458 std::list<ObjectPositionInterface*>* __l_objpos_if_misc;
00459 std::list<Position2DTrackInterface*>* __l_track_if;
00460
00461 ObjectPositionInterface *__line_if;
00462
00463 LaserDrawingArea *__area;
00464 AllemaniACsAtHomeCairoRobotDrawer __athome_drawer;
00465 ConnectionDispatcher __connection_dispatcher;
00466
00467 Gtk::ToolButton *__tb_connection;
00468 Gtk::RadioToolButton *__tb_lines;
00469 Gtk::RadioToolButton *__tb_points;
00470 Gtk::RadioToolButton *__tb_hull;
00471 Gtk::ToggleToolButton *__tb_highres;
00472 Gtk::ToggleToolButton *__tb_trimvals;
00473 Gtk::ToggleToolButton *__tb_rotation;
00474 Gtk::ToggleToolButton *__tb_legtracker;
00475 Gtk::ToggleToolButton *__tb_stop;
00476 Gtk::ToolButton *__tb_zoom_in;
00477 Gtk::ToolButton *__tb_zoom_out;
00478 Gtk::ToolButton *__tb_exit;
00479
00480 Gtk::Dialog *__dlg_ltopen;
00481 Gtk::ProgressBar *__pgb_ltopen;
00482 };
00483
00484 int
00485 main(int argc, char** argv)
00486 {
00487 Gtk::Main kit(argc, argv);
00488
00489 Glib::RefPtr<Gnome::Glade::Xml> refxml;
00490 refxml = Gnome::Glade::Xml::create(RESDIR"/lasergui/lasergui.glade");
00491
00492 LaserGuiGtkWindow *window = NULL;
00493 refxml->get_widget_derived("wnd_lasergui", window);
00494
00495 Gtk::Main::run(*window);
00496
00497 return 0;
00498 }
