00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #ifndef _MANAGER_GUI_H_
00028 #define _MANAGER_GUI_H_
00029
00035 #include <gtkmm/window.h>
00036 #include <gtkmm/messagedialog.h>
00037 #include <gtkmm/image.h>
00038 #include <gtkmm/main.h>
00039 #include <gtkmm/label.h>
00040 #include <gtkmm/grid.h>
00041 #include <gtkmm/frame.h>
00042 #include <gtkmm/separator.h>
00043 #include <glibmm/main.h>
00044 #include <gtkmm/drawingarea.h>
00045 #include <gdkmm/pixbuf.h>
00046 #include <cairomm/context.h>
00047 #include <gdkmm/general.h>
00048 #include <glibmm/fileutils.h>
00049
00050
00051 #include <atlascar_base/ManagerCommand.h>
00052 #include <atlascar_base/ManagerStatus.h>
00053 #include "Gamepad.h"
00054
00055 #include <ros/package.h>
00056
00057 #include <boost/lexical_cast.hpp>
00058 #include <boost/math/constants/constants.hpp>
00059 #include <boost/format.hpp>
00060
00061 #include <Eigen/Dense>
00062
00063
00064 using Eigen::Vector2d;
00065
00066 const double pi = boost::math::constants::pi<double>();
00067
00074 class PressureSensors: public Gtk::Frame
00075 {
00076 public:
00077
00083 PressureSensors():
00084 title_throttle_("Throttle:"),
00085 title_brake_("Brake:"),
00086 title_clutch_("Clutch:"),
00087 throttle_("NA"),
00088 brake_("NA"),
00089 clutch_("NA")
00090 {
00091 set_label("Pressure Sensors");
00092 set_border_width(20);
00093
00094 add(grid_);
00095
00096 grid_.set_border_width(10);
00097
00098 grid_.attach(title_throttle_,1,1,1,1);
00099 grid_.attach(title_brake_,1,2,1,1);
00100 grid_.attach(title_clutch_,1,3,1,1);
00101
00102 grid_.attach(throttle_,2,1,1,1);
00103 grid_.attach(brake_,2,2,1,1);
00104 grid_.attach(clutch_,2,3,1,1);
00105
00106 grid_.set_row_spacing(10);
00107 grid_.set_column_spacing(10);
00108
00109 grid_.set_row_homogeneous(true);
00110 grid_.set_column_homogeneous(true);
00111
00112 title_throttle_.set_justify(Gtk::JUSTIFY_RIGHT);
00113 title_throttle_.set_halign(Gtk::ALIGN_END);
00114
00115 throttle_.set_hexpand();
00116
00117 title_brake_.set_justify(Gtk::JUSTIFY_RIGHT);
00118 title_brake_.set_halign(Gtk::ALIGN_END);
00119
00120 title_clutch_.set_justify(Gtk::JUSTIFY_RIGHT);
00121 title_clutch_.set_halign(Gtk::ALIGN_END);
00122 }
00123
00129 ~PressureSensors()
00130 {
00131 }
00132
00137 void setThrottle(double throttle)
00138 {
00139 throttle_pressure_=throttle;
00140
00141 boost::format fmt("%3.1f");
00142 fmt % throttle_pressure_;
00143
00144 throttle_.set_text(fmt.str());
00145 }
00146
00151 void setBrake(double brake)
00152 {
00153 brake_pressure_=brake;
00154
00155 boost::format fmt("%3.1f");
00156 fmt % brake_pressure_;
00157
00158 brake_.set_text(fmt.str());
00159 }
00160
00165 void setClutch(double clutch)
00166 {
00167 clutch_pressure_=clutch;
00168
00169 boost::format fmt("%3.1f");
00170 fmt % clutch_pressure_;
00171
00172 clutch_.set_text(fmt.str());
00173 }
00174
00175 protected:
00176
00178 Gtk::Grid grid_;
00180 Gtk::Label title_throttle_;
00182 Gtk::Label title_brake_;
00184 Gtk::Label title_clutch_;
00185
00187 Gtk::Label throttle_;
00189 Gtk::Label brake_;
00191 Gtk::Label clutch_;
00192
00194 double throttle_pressure_;
00196 double brake_pressure_;
00198 double clutch_pressure_;
00199 };
00200
00207 class ExtendedDrawingArea: public Gtk::DrawingArea
00208 {
00209 public:
00215 ExtendedDrawingArea()
00216 {
00217
00218 }
00219
00225 virtual ~ExtendedDrawingArea()
00226 {
00227
00228 }
00229
00240 void drawText(std::string text,const Cairo::RefPtr<Cairo::Context>& cr,const int rectangle_width,const int rectangle_height,double font_size=10)
00241 {
00242
00243 Pango::FontDescription font;
00244
00245 font.set_family("Monospace");
00246 font.set_weight(Pango::WEIGHT_BOLD);
00247 font.set_size(font_size* PANGO_SCALE);
00248
00249
00250 Glib::RefPtr<Pango::Layout> layout = create_pango_layout(text);
00251
00252 layout->set_font_description(font);
00253
00254 int text_width;
00255 int text_height;
00256
00257
00258 layout->get_pixel_size(text_width, text_height);
00259
00260
00261 cr->move_to(rectangle_width-(text_width/2.), rectangle_height-(text_height/2.));
00262
00263 layout->show_in_cairo_context(cr);
00264 }
00265
00276 void drawTextItalics(std::string text,const Cairo::RefPtr<Cairo::Context>& cr,const int rectangle_width,const int rectangle_height,double font_size=10)
00277 {
00278
00279 Pango::FontDescription font;
00280
00281 font.set_family("Monospace");
00282 font.set_weight(Pango::WEIGHT_BOLD);
00283 font.set_style(Pango::STYLE_ITALIC);
00284 font.set_size(font_size* PANGO_SCALE);
00285
00286
00287 Glib::RefPtr<Pango::Layout> layout = create_pango_layout(text);
00288
00289 layout->set_font_description(font);
00290
00291 int text_width;
00292 int text_height;
00293
00294
00295 layout->get_pixel_size(text_width, text_height);
00296
00297
00298 cr->move_to(rectangle_width-(text_width/2.), rectangle_height-(text_height/2.));
00299
00300 layout->show_in_cairo_context(cr);
00301 }
00302
00313 void drawTextItalicsNotBold(std::string text,const Cairo::RefPtr<Cairo::Context>& cr,const int rectangle_width,const int rectangle_height,double font_size=10)
00314 {
00315
00316 Pango::FontDescription font;
00317
00318 font.set_family("Monospace");
00319 font.set_style(Pango::STYLE_ITALIC);
00320 font.set_size(font_size* PANGO_SCALE);
00321
00322
00323 Glib::RefPtr<Pango::Layout> layout = create_pango_layout(text);
00324
00325 layout->set_font_description(font);
00326
00327 int text_width;
00328 int text_height;
00329
00330
00331 layout->get_pixel_size(text_width, text_height);
00332
00333
00334 cr->move_to(rectangle_width-(text_width/2.), rectangle_height-(text_height/2.));
00335
00336 layout->show_in_cairo_context(cr);
00337 }
00338 protected:
00339
00341 Glib::RefPtr<Gdk::Pixbuf> image_;
00342 };
00343
00350 class GamepadInfo: public Gtk::DrawingArea
00351 {
00352 public:
00359 GamepadInfo(std::string active_path =
00360 ros::package::getPath("atlascar_base")+"/images/xbox-360-controller_active_tumb.png"):
00361 active_path_(active_path),
00362 fade_(0.2)
00363 {
00364
00365 device_="NOT_SET";
00366
00367 active_=false;
00368
00369 command_.reset(new atlascar_base::ManagerCommand);
00370
00371 try
00372 {
00373
00374 active_image_ = Gdk::Pixbuf::create_from_file(active_path_);
00375
00376 active_image_ ->saturate_and_pixelate(active_image_ ,5.0,false);
00377
00378 not_active_image_ = Gdk::Pixbuf::create_from_file(active_path_);
00379
00380 not_active_image_->saturate_and_pixelate(not_active_image_,0.0,false);
00381
00382 not_active_image_->add_alpha(false,0,0,0);
00383
00384 not_active_image_->composite_color(not_active_image_,0,0,not_active_image_->get_width(),not_active_image_->get_height(),0,0,1,1,Gdk::INTERP_BILINEAR,100,0,0,1,0xffffffff,0xffffffff);
00385
00386 auxiliary_image_ = Gdk::Pixbuf::create_from_file(active_path_);
00387
00388 }catch(const Glib::FileError& ex)
00389 {
00390 std::cerr << "FileError: " << ex.what() << std::endl;
00391 }catch(const Gdk::PixbufError& ex)
00392 {
00393 std::cerr << "PixbufError: " << ex.what() << std::endl;
00394 }
00395
00396
00397 override_background_color(Gdk::RGBA("white"));
00398
00399
00400 if (active_image_)
00401 set_size_request(active_image_->get_width(), active_image_->get_height());
00402 }
00403
00409 ~GamepadInfo()
00410 {
00411 }
00412
00418 bool initializeGamepad()
00419 {
00420
00421 if(device_=="Not found" || active_==true)
00422 return false;
00423
00424 int ret=0;
00425
00426
00427 ret = gamepad_.startComm(device_.c_str());
00428 if(ret<0)
00429 return false;
00430
00431
00432 gamepad_.buttons(7)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadIgnitionONCallback);
00433 gamepad_.buttons(6)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadIgnitionOFFCallback);
00434 gamepad_.buttons(4)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadLeftLightCallback);
00435 gamepad_.buttons(5)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadRightLightCallback);
00436
00437 gamepad_.buttons(13)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadHighLightCallback);
00438 gamepad_.buttons(12)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadMediumLightCallback);
00439 gamepad_.buttons(11)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadWarningCallback);
00440 gamepad_.buttons(0)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadAutoDirectionCallback);
00441
00442 gamepad_.buttons(2)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadAutoThrottleCallback);
00443 gamepad_.buttons(1)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadAutoBrakeCallback);
00444 gamepad_.buttons(3)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadAutoClutchCallback);
00445
00446 gamepad_.axes(5)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadThrottleCallback);
00447 gamepad_.axes(2)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadBrakeCallback);
00448 gamepad_.axes(4)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadClutchCallback);
00449 gamepad_.axes(0)->callback = sigc::mem_fun<int>(*this,&GamepadInfo::gamepadSteeringWheelCallback);
00450
00451 active_=true;
00452 return true;
00453 }
00454
00461 bool stateManager()
00462 {
00463 gamepad_.dispatch();
00464
00465 double min_fade=0.7;
00466
00467 if(active_)
00468 {
00469
00470 if((ros::Time::now()-ta_)<fade_)
00471 {
00472 double percentage = 1 - (ros::Time::now()-ta_).toSec()/fade_.toSec();
00473
00474 percentage = percentage*(1-min_fade) + min_fade;
00475
00476 active_image_->composite_color(auxiliary_image_,0,0,not_active_image_->get_width(),not_active_image_->get_height(),0,0,1,1,Gdk::INTERP_BILINEAR,percentage*255,0,0,1,0xffffffff,0xffffffff);
00477 }else
00478 {
00479 active_image_->composite_color(auxiliary_image_,0,0,not_active_image_->get_width(),not_active_image_->get_height(),0,0,1,1,Gdk::INTERP_BILINEAR,min_fade*255,0,0,1,0xffffffff,0xffffffff);
00480 }
00481 }else
00482 auxiliary_image_ = not_active_image_;
00483
00484 queue_draw();
00485
00486 return true;
00487 }
00488
00494 bool touch()
00495 {
00496 ta_=ros::Time::now();
00497
00498 return true;
00499 }
00500
00502 std::string device_;
00504 sigc::slot<void,const atlascar_base::ManagerCommandPtr> event_;
00505
00506 protected:
00507
00519 double map(double value,double min_value,double max_value,double min_required,double max_required)
00520 {
00521 assert(min_value!=max_value);
00522 assert(min_required!=max_required);
00523
00524 double m = (max_required-min_required)/(max_value-min_value);
00525 double b = max_required - m*max_value;
00526
00527 return value*m+b;
00528 }
00529
00536 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
00537 {
00538 if (!active_image_)
00539 return false;
00540
00541 if (!not_active_image_)
00542 return false;
00543
00544 Gtk::Allocation allocation = get_allocation();
00545 const int width = allocation.get_width();
00546 const int height = allocation.get_height();
00547
00548 Gdk::Cairo::set_source_pixbuf(cr, auxiliary_image_,
00549 (width - auxiliary_image_->get_width())/2, (height - auxiliary_image_->get_height())/2);
00550
00551 cr->paint();
00552
00553 if(!active_)
00554 {
00555 cr->set_source_rgb(1,0,0);
00556 cr->set_line_width(3);
00557 cr->move_to(0,0);
00558 cr->line_to(width,height);
00559 cr->stroke();
00560 }
00561
00562 return true;
00563 }
00564
00565
00566
00573 void gamepadThrottleCallback(int value)
00574 {
00575 touch();
00576 double rescaled = map(value,-32767,+32767,0,1);
00577 command_->throttle=rescaled;
00578 if(!event_.empty())
00579 event_(command_);
00580 }
00581
00588 void gamepadBrakeCallback(int value)
00589 {
00590 touch();
00591 double rescaled = map(value,-32767,+32767,0,1);
00592 command_->brake=rescaled;
00593 if(!event_.empty())
00594 event_(command_);
00595 }
00596
00603 void gamepadClutchCallback(int value)
00604 {
00605 if(value>0)
00606 value=0;
00607
00608 double rescaled = map(value,0,-32767,0,1);
00609
00610 if(rescaled==command_->clutch)
00611 return;
00612
00613 touch();
00614 command_->clutch=rescaled;
00615 if(!event_.empty())
00616 event_(command_);
00617
00618 }
00619
00626 void gamepadSteeringWheelCallback(int value)
00627 {
00628 touch();
00629 double rescaled = map(value,-32767,+32767,0.5,-0.5);
00630 command_->steering_wheel=rescaled;
00631 if(!event_.empty())
00632 event_(command_);
00633 }
00634
00641 void gamepadIgnitionONCallback(int value)
00642 {
00643 if(value)
00644 {
00645 touch();
00646 command_->ignition=1;
00647 if(!event_.empty())
00648 event_(command_);
00649 }
00650 }
00651
00658 void gamepadIgnitionOFFCallback(int value)
00659 {
00660 if(value)
00661 {
00662 touch();
00663 command_->ignition=0;
00664 if(!event_.empty())
00665 event_(command_);
00666 }
00667 }
00668
00675 void gamepadLeftLightCallback(int value)
00676 {
00677 if(value)
00678 {
00679 touch();
00680 command_->lights_left=!command_->lights_left;
00681 if(!event_.empty())
00682 event_(command_);
00683 }
00684 }
00685
00692 void gamepadRightLightCallback(int value)
00693 {
00694 if(value)
00695 {
00696 touch();
00697 command_->lights_right=!command_->lights_right;
00698 if(!event_.empty())
00699 event_(command_);
00700 }
00701 }
00702
00709 void gamepadHighLightCallback(int value)
00710 {
00711 if(value)
00712 {
00713 touch();
00714 command_->lights_high=!command_->lights_high;
00715 if(!event_.empty())
00716 event_(command_);
00717 }
00718 }
00719
00726 void gamepadMediumLightCallback(int value)
00727 {
00728 if(value)
00729 {
00730 touch();
00731 command_->lights_medium=!command_->lights_medium;
00732 if(!event_.empty())
00733 event_(command_);
00734 }
00735 }
00736
00743 void gamepadWarningCallback(int value)
00744 {
00745 if(value)
00746 {
00747 touch();
00748 command_->lights_warning=!command_->lights_warning;
00749 if(!event_.empty())
00750 event_(command_);
00751 }
00752 }
00753
00760 void gamepadAutoDirectionCallback(int value)
00761 {
00762 if(value)
00763 {
00764 touch();
00765 command_->auto_direction=!command_->auto_direction;
00766 if(!event_.empty())
00767 event_(command_);
00768 }
00769 }
00770
00777 void gamepadAutoThrottleCallback(int value)
00778 {
00779 if(value)
00780 {
00781 touch();
00782 command_->auto_throttle=!command_->auto_throttle;
00783 if(!event_.empty())
00784 event_(command_);
00785 }
00786 }
00787
00794 void gamepadAutoBrakeCallback(int value)
00795 {
00796 if(value)
00797 {
00798 touch();
00799 command_->auto_brake=!command_->auto_brake;
00800 if(!event_.empty())
00801 event_(command_);
00802 }
00803 }
00804
00811 void gamepadAutoClutchCallback(int value)
00812 {
00813 if(value)
00814 {
00815 touch();
00816 command_->auto_clutch=!command_->auto_clutch;
00817 if(!event_.empty())
00818 event_(command_);
00819 }
00820 }
00821
00823 std::string active_path_;
00825 Glib::RefPtr<Gdk::Pixbuf> active_image_;
00827 Glib::RefPtr<Gdk::Pixbuf> not_active_image_;
00829 Glib::RefPtr<Gdk::Pixbuf> auxiliary_image_;
00830
00832 bool active_;
00834 ros::Time ta_;
00836 ros::Duration fade_;
00838 Gamepad gamepad_;
00840 atlascar_base::ManagerCommandPtr command_;
00841 };
00842
00851 class OperationMode: public ExtendedDrawingArea
00852 {
00853 public:
00855 enum EnumOperationMode {DIRECT, HIGH};
00856
00863 OperationMode(int side_length=100,EnumOperationMode operation_mode=DIRECT):
00864 side_length_(side_length),
00865 operation_mode_(operation_mode)
00866 {
00867 override_background_color(Gdk::RGBA("white"));
00868 set_size_request(side_length_*2,side_length_);
00869 add_events(Gdk::BUTTON_PRESS_MASK);
00870 }
00871
00877 virtual ~OperationMode()
00878 {}
00879
00884 void operator=(EnumOperationMode mode)
00885 {
00886 operation_mode_ = mode;
00887 queue_draw();
00888 }
00889
00895 bool operator==(EnumOperationMode mode)
00896 {
00897 if(operation_mode_==mode)
00898 return true;
00899 else
00900 return false;
00901 }
00902
00907 int getMode(void)
00908 {
00909 switch(operation_mode_)
00910 {
00911 case DIRECT:
00912 return 1;
00913 case HIGH:
00914 return 2;
00915 }
00916
00917 return 1;
00918 }
00919
00920 protected:
00921
00928 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
00929 {
00930 Gtk::Allocation allocation = get_allocation();
00931 const int width = allocation.get_width();
00932 const int height = allocation.get_height();
00933
00934 switch(operation_mode_)
00935 {
00936 case DIRECT:
00937 cr->set_source_rgb(0,0,0);
00938 cr->set_line_width(1);
00939 cr->save();
00940
00941 cr->move_to(0.12*width,0.20*height);
00942 cr->line_to(0.88*width,0.20*height);
00943 cr->line_to(0.88*width,0.60*height);
00944 cr->line_to(0.12*width,0.60*height);
00945 cr->close_path();
00946 cr->set_source_rgba(0,0,0,0.7);
00947 cr->fill_preserve();
00948 cr->restore();
00949 cr->stroke();
00950 cr->set_source_rgb(1,1,1);
00951
00952 drawText("Direct",cr,0.5*width,0.4*height,12);
00953 cr->set_source_rgba(0,0,0,0.5);
00954 drawText("High",cr,0.5*width,0.7*height,6);
00955 break;
00956
00957 case HIGH:
00958
00959 cr->set_source_rgb(0,0,0);
00960 cr->set_line_width(1);
00961 cr->save();
00962
00963 cr->move_to(0.12*width,0.40*height);
00964 cr->line_to(0.88*width,0.40*height);
00965 cr->line_to(0.88*width,0.80*height);
00966 cr->line_to(0.12*width,0.80*height);
00967 cr->close_path();
00968 cr->set_source_rgba(0,0,0,0.7);
00969 cr->fill_preserve();
00970 cr->restore();
00971 cr->stroke();
00972 cr->set_source_rgb(1,1,1);
00973
00974 drawText("High",cr,0.5*width,0.6*height,12);
00975 cr->set_source_rgba(0,0,0,0.5);
00976 drawText("Direct",cr,0.5*width,0.3*height,6);
00977
00978 break;
00979 }
00980
00981 return true;
00982 }
00983
00985 int side_length_;
00987 EnumOperationMode operation_mode_;
00988 };
00989
00996 class PedalDrawingArea: public ExtendedDrawingArea
00997 {
00998 public:
00999
01008 PedalDrawingArea(std::string title=std::string("NOT_SET"),double min_value=0,double max_value=100):
01009 title_(title),
01010 zoom_(1),
01011 min_value_(min_value),
01012 max_value_(max_value)
01013 {
01014 override_background_color(Gdk::RGBA("white"));
01015 set_size_request(zoom_*50,zoom_*150);
01016 value_=min_value_;
01017 }
01018
01024 virtual ~PedalDrawingArea()
01025 {}
01026
01032 void setValue(double value)
01033 {
01034 if(value>max_value_)
01035 value=max_value_;
01036
01037 if(value<min_value_)
01038 value=min_value_;
01039
01040 if(value != value_)
01041 queue_draw();
01042
01043 value_=value;
01044 }
01045
01046 protected:
01047
01048
01049 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
01050 {
01051 Gtk::Allocation allocation = get_allocation();
01052 const int width = allocation.get_width();
01053 const int height = allocation.get_height();
01054
01055 Cairo::Matrix transformation = Cairo::rotation_matrix(-pi/2);
01056
01057 cr->transform(transformation);
01058 drawText(title_,cr,-height/2.,0.05*height,8);
01059
01060 transformation.invert();
01061 cr->transform(transformation);
01062
01063 cr->move_to(.3*width-0.05*width,.05*height-0.05*width);
01064 cr->line_to(.3*width-0.05*width,.95*height+0.05*width);
01065 cr->line_to(.6*width+0.05*width,.95*height+0.05*width);
01066 cr->line_to(.9*width+0.05*width,.05*height-0.05*width);
01067 cr->close_path();
01068 cr->set_source_rgb(.4,.4,.4);
01069 cr->fill();
01070
01071 cr->set_source_rgb(0,0,0);
01072 cr->set_line_width(2);
01073 cr->move_to(.3*width,.05*height);
01074 cr->line_to(.3*width,.95*height);
01075 cr->line_to(.6*width,.95*height);
01076 cr->line_to(.9*width,.05*height);
01077 cr->close_path();
01078 cr->stroke();
01079
01080 cr->move_to(.3*width,.05*height);
01081 cr->line_to(.3*width,.95*height);
01082 cr->line_to(.6*width,.95*height);
01083 cr->line_to(.9*width,.05*height);
01084 cr->close_path();
01085 cr->set_source_rgb(.8,.8,.8);
01086 cr->fill();
01087
01088 Cairo::RefPtr< Cairo::LinearGradient > gradient_ptr = Cairo::LinearGradient::create(0.5*width,.05*height,0.5*width,0.95*height);
01089
01090
01091 gradient_ptr->add_color_stop_rgb(0,1,0,0);
01092 gradient_ptr->add_color_stop_rgb(1,1.0,0.6,0);
01093
01094
01095 double racio = ((value_-min_value_)/(max_value_-min_value_));
01096 double top_value = -racio*.9*height + 0.95*height;
01097 double x_value = racio*0.3*width+.6*width;
01098
01099 cr->set_source(gradient_ptr);
01100 cr->move_to(.3*width,.95*height);
01101 cr->line_to(.6*width,.95*height);
01102 cr->line_to(x_value,top_value);
01103 cr->line_to(0.3*width,top_value);
01104 cr->close_path();
01105 cr->fill();
01106
01107 boost::format fmt("%3.1f");
01108 fmt % value_;
01109
01110 cr->set_source_rgb(0,0,0);
01111 drawText(fmt.str(),cr,width/2.+0.08*width,0.1*height,8);
01112
01113 return true;
01114 }
01115
01116 std::string title_;
01117 double zoom_;
01118 double value_;
01119 double min_value_;
01120 double max_value_;
01121 };
01122
01123 class SwitchButtonDrawingArea: public ExtendedDrawingArea
01124 {
01125 public:
01126 SwitchButtonDrawingArea(int side_length=50,std::string title=std::string("NOT_SET")):
01127 active_(false),
01128 title_text_(title),
01129 active_text_(std::string("AUTO")),
01130 not_active_text_(std::string("MAN")),
01131 side_length_(side_length)
01132 {
01133 override_background_color(Gdk::RGBA("white"));
01134 set_size_request(0.75*side_length_,side_length_);
01135 }
01136
01137 virtual ~SwitchButtonDrawingArea()
01138 {}
01139
01140 void setStatus(bool active)
01141 {
01142 if(active_ != active)
01143 queue_draw();
01144 active_=active;
01145 }
01146
01147 protected:
01148
01149
01150 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
01151 {
01152 Gtk::Allocation allocation = get_allocation();
01153 const int width = allocation.get_width();
01154 const int height = allocation.get_height();
01155
01156 drawText(title_text_,cr,width/2.,0.1*height,7);
01157
01158 if(active_)
01159 cr->set_source_rgb(1,0,0);
01160 else
01161 cr->set_source_rgb(0,0,0);
01162
01163 drawText(active_text_,cr,width/2.,0.22*height,6);
01164
01165 if(!active_)
01166 cr->set_source_rgb(0,1,0);
01167 else
01168 cr->set_source_rgb(0,0,0);
01169
01170 drawText(not_active_text_,cr,width/2.,0.90*height,6);
01171
01172 double center = 0.55*height;
01173 double length = .25*height;
01174
01175
01176 Cairo::RefPtr< Cairo::LinearGradient > gradient_ptr = Cairo::LinearGradient::create(0,0,width,height);
01177 Cairo::RefPtr< Cairo::LinearGradient > gradient_t_ptr = Cairo::LinearGradient::create(0,center-0.06*height,width,center);
01178 Cairo::RefPtr< Cairo::LinearGradient > gradient_b_ptr = Cairo::LinearGradient::create(0,center+0.06*height,width,center);
01179
01180
01181 gradient_ptr->add_color_stop_rgb(0,0.7,0.7,0.7);
01182 gradient_ptr->add_color_stop_rgb(1,0.5,0.5,0.5);
01183
01184 gradient_t_ptr->add_color_stop_rgb(0.44,0.7,0.7,0.7);
01185 gradient_t_ptr->add_color_stop_rgb(0.49,0.1,0.1,0.1);
01186
01187 gradient_b_ptr->add_color_stop_rgb(0.44,0.7,0.7,0.7);
01188 gradient_b_ptr->add_color_stop_rgb(0.49,0.1,0.1,0.1);
01189
01190 cr->set_line_cap(Cairo::LINE_CAP_ROUND);
01191 cr->set_line_join(Cairo::LINE_JOIN_ROUND);
01192
01193 cr->save();
01194
01195 cr->move_to(.5*width,.5*height);
01196 cr->arc(.5*width,center,.20*side_length_,0,2*pi);
01197 cr->set_source_rgb(0.1,0.1,0.1);
01198 cr->fill();
01199
01200 cr->set_source(gradient_ptr);
01201 cr->arc(.5*width,center,.19*side_length_,0,2*pi);
01202 cr->fill();
01203
01204 cr->arc(.5*width,center,.08*side_length_,0,2*pi);
01205 cr->set_source_rgb(0.4,0.4,0.4);
01206 cr->fill();
01207
01208 cr->set_line_width(0.01*side_length_);
01209 cr->set_source_rgb(0,0,0);
01210 cr->arc(.5*width,center,.08*side_length_,0,2*pi);
01211 cr->stroke();
01212
01213 cr->restore();
01214
01215 if(active_)
01216 {
01217 cr->set_line_width(0.11*side_length_);
01218 cr->set_source_rgb(0,0,0);
01219 cr->move_to(.5*width,center);
01220 cr->line_to(0.48*width,center-length);
01221 cr->stroke();
01222 cr->move_to(.5*width,center);
01223 cr->line_to(0.52*width,center-length);
01224 cr->stroke();
01225
01226 cr->set_source(gradient_b_ptr);
01227 cr->set_line_width(0.1*side_length_);
01228 cr->move_to(.5*width,center);
01229 cr->line_to(0.48*width,center-length);
01230 cr->stroke();
01231 cr->move_to(.5*width,center);
01232 cr->line_to(0.52*width,center-length);
01233 cr->stroke();
01234 }else
01235 {
01236 cr->set_line_width(0.11*side_length_);
01237 cr->set_source_rgb(0,0,0);
01238 cr->move_to(.5*width,center);
01239 cr->line_to(0.48*width,center+length);
01240 cr->stroke();
01241 cr->move_to(.5*width,center);
01242 cr->line_to(0.52*width,center+length);
01243 cr->stroke();
01244
01245 cr->set_source(gradient_t_ptr);
01246 cr->set_line_width(0.1*side_length_);
01247 cr->move_to(.5*width,center);
01248 cr->line_to(0.48*width,center+length);
01249 cr->stroke();
01250 cr->move_to(.5*width,center);
01251 cr->line_to(0.52*width,center+length);
01252 cr->stroke();
01253 }
01254
01255 return true;
01256 }
01257
01258 bool active_;
01259 std::string title_text_;
01260 std::string active_text_;
01261 std::string not_active_text_;
01262 int side_length_;
01263 };
01264
01265 class WarningDrawingArea: public Gtk::DrawingArea
01266 {
01267 public:
01268 WarningDrawingArea(int side_length=50):
01269 active_(false),
01270 side_length_(side_length),
01271 blink_on_(0.5),
01272 blink_off_(0.2)
01273 {
01274 override_background_color(Gdk::RGBA("white"));
01275
01276 set_size_request(side_length_,side_length_);
01277 }
01278
01279 virtual ~WarningDrawingArea()
01280 {}
01281
01282 void setStatus(bool active)
01283 {
01284 if(!active_ && active)
01285 {
01286 ta_=ros::Time::now();
01287 blink_state_=true;
01288 queue_draw();
01289 }else if(active_ && !active)
01290 queue_draw();
01291
01292
01293
01294 active_=active;
01295 }
01296
01297 bool stateManager()
01298 {
01299 if(active_)
01300 {
01301 if(blink_state_)
01302 {
01303 if( ros::Time::now()-ta_ > blink_on_)
01304 {
01305 blink_state_=false;
01306 ta_=ros::Time::now();
01307 queue_draw();
01308 return true;
01309 }
01310 }else
01311 {
01312 if( ros::Time::now()-ta_ > blink_off_)
01313 {
01314 blink_state_=true;
01315 ta_=ros::Time::now();
01316 queue_draw();
01317 return true;
01318 }
01319 }
01320 }
01321
01322 return false;
01323 }
01324
01325 protected:
01326
01327
01328 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
01329 {
01330 Gtk::Allocation allocation = get_allocation();
01331 const int width = allocation.get_width();
01332 const int height = allocation.get_height();
01333
01334 cr->scale(width,height);
01335
01336 cr->set_line_cap(Cairo::LINE_CAP_ROUND);
01337 cr->set_line_join(Cairo::LINE_JOIN_ROUND);
01338
01339 if(active_)
01340 {
01341 if(blink_state_)
01342 {
01343 cr->set_source_rgba(1.0,0.0,0.0,0.5);
01344
01345 cr->set_line_width(0.15);
01346
01347 cr->move_to(.5,.1);
01348 cr->line_to(.9,.9);
01349 cr->line_to(.1,.9);
01350 cr->close_path();
01351 cr->stroke();
01352
01353 cr->set_source_rgb(1.0,0.0,0.0);
01354 }else
01355 cr->set_source_rgba(1.0,0.0,0.0,0.7);
01356 }else
01357 cr->set_source_rgba(0.6,0.6,0.6,1.0);
01358
01359 cr->set_line_width(0.1);
01360
01361 cr->move_to(.5,.1);
01362 cr->line_to(.9,.9);
01363 cr->line_to(.1,.9);
01364 cr->close_path();
01365 cr->stroke();
01366
01367 if(active_)
01368 cr->set_source_rgb(0,0,0);
01369 else
01370 cr->set_source_rgb(0.6,0.6,0.6);
01371
01372 cr->move_to(0.48,0.4);
01373 cr->line_to(0.5,0.60);
01374 cr->stroke();
01375
01376 cr->move_to(0.52,0.4);
01377 cr->line_to(0.5,0.60);
01378 cr->stroke();
01379
01380 cr->move_to(0.50,0.395);
01381 cr->line_to(0.5,0.60);
01382 cr->stroke();
01383
01384 cr->set_line_width(0.05);
01385
01386 cr->arc(0.5,0.75,0.07,0,2*pi);
01387 cr->fill();
01388
01389 return true;
01390 }
01391
01392 bool active_;
01393 bool blink_state_;
01394 int side_length_;
01395 ros::Duration blink_on_;
01396 ros::Duration blink_off_;
01397 ros::Time ta_;
01398
01399 };
01400
01401 class IgnitionDrawingArea: public Gtk::DrawingArea
01402 {
01403 public:
01404 IgnitionDrawingArea(int side_length=50):
01405 active_(false),
01406 side_length_(side_length)
01407 {
01408 override_background_color(Gdk::RGBA("white"));
01409
01410 set_size_request(side_length_*0.7,side_length_);
01411 }
01412
01413 virtual ~IgnitionDrawingArea()
01414 {}
01415
01416 void setStatus(bool active)
01417 {
01418 if(active_!=active)
01419 queue_draw();
01420
01421 active_=active;
01422 }
01423
01424 protected:
01425
01426
01427 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
01428 {
01429 Gtk::Allocation allocation = get_allocation();
01430 const int width = allocation.get_width();
01431 const int height = allocation.get_height();
01432
01433
01434 if(active_)
01435 cr->set_source_rgb(1, 0, 0.0);
01436 else
01437 cr->set_source_rgb(0.0, 0.0, 0.0);
01438
01439 cr->scale(width/0.7,height);
01440
01441 cr->set_line_width(0.01);
01442 cr->set_line_cap(Cairo::LINE_CAP_ROUND);
01443
01444 cr->move_to(0.6,0.1);
01445 cr->line_to(0.3,0.15);
01446 cr->line_to(0.2,0.38);
01447 cr->line_to(0.35,0.33);
01448 cr->line_to(0.2,0.63);
01449 cr->line_to(0.3,0.60);
01450 cr->line_to(0.18,0.9);
01451 cr->line_to(0.45,0.55);
01452 cr->line_to(0.32,0.60);
01453 cr->line_to(0.55,0.28);
01454 cr->line_to(0.38,0.33);
01455 cr->line_to(0.6,0.1);
01456 cr->fill();
01457
01458 if(active_)
01459 cr->set_source_rgb(1.0, 0.85, 0.0);
01460 else
01461 cr->set_source_rgb(0.2, 0.2, 0.2);
01462
01463 cr->scale(0.9,1);
01464
01465 cr->move_to(0.6,0.1);
01466 cr->line_to(0.3,0.15);
01467 cr->line_to(0.2,0.38);
01468 cr->line_to(0.35,0.33);
01469 cr->line_to(0.2,0.63);
01470 cr->line_to(0.3,0.60);
01471 cr->line_to(0.18,0.9);
01472 cr->line_to(0.45,0.55);
01473 cr->line_to(0.32,0.60);
01474 cr->line_to(0.55,0.28);
01475 cr->line_to(0.38,0.33);
01476 cr->line_to(0.6,0.1);
01477 cr->fill();
01478
01479 return true;
01480 }
01481
01482 bool active_;
01483 int side_length_;
01484
01485 };
01486
01487 class LightsDrawingArea: public ExtendedDrawingArea
01488 {
01489 public:
01490 enum light_enum {MAXIMUM, MEDIUM};
01491
01492 LightsDrawingArea(light_enum l=MAXIMUM,int side_length=50):
01493 light_(l),
01494 active_(false),
01495 side_length_(side_length)
01496 {
01497 override_background_color(Gdk::RGBA("white"));
01498
01499 set_size_request(side_length_,side_length_);
01500 }
01501
01502 virtual ~LightsDrawingArea()
01503 {}
01504
01505 void setStatus(bool active)
01506 {
01507 if(active_!=active)
01508 queue_draw();
01509
01510 active_=active;
01511 }
01512
01513 protected:
01514
01515
01516 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
01517 {
01518 Gtk::Allocation allocation = get_allocation();
01519 const int width = allocation.get_width();
01520 const int height = allocation.get_height();
01521
01522 switch(light_)
01523 {
01524 case MAXIMUM:
01525 if(active_)
01526 cr->set_source_rgb(0, 0.2, 1);
01527 else
01528 cr->set_source_rgb(.2, .2, .2);
01529
01530 break;
01531 case MEDIUM:
01532 if(active_)
01533 cr->set_source_rgb(.3, 1, 0);
01534 else
01535 cr->set_source_rgb(.2, .2, .2);
01536
01537 break;
01538 }
01539
01540
01541 double x0=0.5, y0=0.75,
01542 x1=0, y1=0.80,
01543 x2=0, y2=0.20,
01544 x3=0.5, y3=0.25;
01545
01546
01547 cr->scale(width, height);
01548
01549 cr->set_line_width(0.08);
01550 cr->set_line_cap(Cairo::LINE_CAP_ROUND);
01551
01552
01553 cr->move_to(x0, y0);
01554 cr->curve_to(x1, y1, x2, y2, x3, y3);
01555
01556 cr->move_to(x0, y0);
01557
01558 x1=.6;
01559 y1=.6;
01560 x2=.6;
01561 y2=.4;
01562
01563 cr->curve_to(x1, y1, x2, y2, x3, y3);
01564 cr->stroke();
01565
01566 switch(light_)
01567 {
01568 case MAXIMUM:
01569 cr->move_to(0.65,0.26);
01570 cr->line_to(0.95,0.26);
01571
01572 cr->move_to(0.7,0.42);
01573 cr->line_to(0.95,0.42);
01574
01575 cr->move_to(0.7,0.58);
01576 cr->line_to(0.95,0.58);
01577
01578 cr->move_to(0.65,0.74);
01579 cr->line_to(0.95,0.74);
01580 break;
01581 case MEDIUM:
01582 cr->move_to(0.65,0.26);
01583 cr->line_to(0.95,0.26+0.1);
01584
01585 cr->move_to(0.7,0.42+.01);
01586 cr->line_to(0.95,0.42+0.1);
01587
01588 cr->move_to(0.7,0.58+.01);
01589 cr->line_to(0.95,0.58+0.1);
01590
01591 cr->move_to(0.65,0.74);
01592 cr->line_to(0.95,0.74+0.1);
01593 break;
01594 }
01595
01596 cr->stroke();
01597
01598 return true;
01599 }
01600
01601 light_enum light_;
01602 bool active_;
01603 int side_length_;
01604
01605 };
01606
01607 class SteeringWheelDrawingArea: public ExtendedDrawingArea
01608 {
01609 public:
01610
01611 SteeringWheelDrawingArea(int side_length=250):
01612 steering_wheel_(0),
01613 side_length_(side_length)
01614 {
01615 override_background_color(Gdk::RGBA("white"));
01616 set_size_request(side_length_, side_length_);
01617 }
01618
01619 virtual ~SteeringWheelDrawingArea()
01620 {}
01621
01622 void setSteeringWheel(double steering_wheel)
01623 {
01624 if(steering_wheel!=steering_wheel_)
01625 queue_draw();
01626
01627 steering_wheel_=steering_wheel;
01628 }
01629
01630 protected:
01631
01632
01633 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
01634 {
01635 Gtk::Allocation allocation = get_allocation();
01636 const int width = allocation.get_width();
01637 const int height = allocation.get_height();
01638
01639 cr->scale(width, height);
01640
01641 Cairo::Matrix transformation = Cairo::identity_matrix();
01642 transformation.translate(0.5,0.5);
01643 transformation.rotate(-steering_wheel_);
01644
01645 cr->transform(transformation);
01646
01647 cr->set_line_width(0.05);
01648 cr->set_line_cap(Cairo::LINE_CAP_ROUND);
01649
01650 cr->set_source_rgb(0.4, 0.4, 0.4);
01651 cr->arc(0,0,0.41,0,2*pi);
01652 cr->stroke();
01653
01654 cr->set_source_rgb(0, 0, 0);
01655 cr->arc(0,0,0.4,0,2*pi);
01656 cr->stroke();
01657
01658 cr->set_line_width(0.1);
01659
01660 double x0=-0.36, y0=-0.10,
01661 x1=-.2, y1=0.00,
01662 x2=0.2, y2=0.00,
01663 x3=0.36, y3=-0.10;
01664
01665
01666 cr->move_to(x0, y0);
01667 cr->curve_to(x1, y1, x2, y2, x3, y3);
01668 cr->stroke();
01669
01670 x0=-.35;
01671 y0=-.05;
01672
01673 x1=-.2,
01674 y1=.0;
01675
01676 x2=-.02;
01677 y2=.1;
01678
01679 x3=-.1;
01680 y3=.36;
01681
01682 cr->move_to(x0, y0);
01683 cr->curve_to(x1, y1, x2, y2, x3, y3);
01684 cr->stroke();
01685
01686 x0=.35;
01687 y0=-.05;
01688
01689 x1=.2,
01690 y1=.0;
01691
01692 x2=.02;
01693 y2=.1;
01694
01695 x3=.1;
01696 y3=.36;
01697
01698 cr->move_to(x0, y0);
01699 cr->curve_to(x1, y1, x2, y2, x3, y3);
01700 cr->stroke();
01701
01702 cr->set_line_width(0.15);
01703 cr->move_to(0,0.35);
01704 cr->line_to(0,0);
01705 cr->stroke();
01706
01707 cr->set_line_width(0.2);
01708 cr->move_to(-0.1,-0.02);
01709 cr->line_to(0.1,-0.02);
01710 cr->stroke();
01711
01712 cr->scale(1./width,1./height);
01713
01714 transformation.invert();
01715 cr->transform(transformation);
01716
01717 cr->set_source_rgb(1, 1, 1);
01718
01719 boost::format fmt("%3.2f");
01720 fmt % steering_wheel_;
01721
01722 drawText(fmt.str(),cr,0,0,13);
01723 drawText(std::string("rad"),cr,0,0.07*height,8);
01724
01725 return true;
01726 }
01727
01728 double steering_wheel_;
01729 int side_length_;
01730 };
01731
01732 class GearDrawnigArea: public ExtendedDrawingArea
01733 {
01734 public:
01735 enum gear_enum {REVERSE, NEUTRAL, G1, G2, G3, G4, G5};
01736
01737 GearDrawnigArea(gear_enum g,int side_length=50):
01738 side_length_(side_length),
01739 gear_(g)
01740 {
01741 override_background_color(Gdk::RGBA("white"));
01742
01743 set_size_request(side_length_,side_length_);
01744 }
01745
01746 virtual ~GearDrawnigArea()
01747 {}
01748
01749 void setGear(gear_enum gear)
01750 {
01751 if(gear!=gear_)
01752 queue_draw();
01753
01754 gear_=gear;
01755 }
01756
01757 protected:
01758
01759
01760 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
01761 {
01762 cr->set_source_rgb(0, 0, 0.0);
01763
01764 int x_offset=-2;
01765 switch(gear_)
01766 {
01767 case REVERSE:
01768
01769 drawTextItalics(std::string("R"),cr,side_length_/2+x_offset,side_length_/2,30);
01770 break;
01771
01772 case NEUTRAL:
01773 drawTextItalics(std::string("N"),cr,side_length_/2+x_offset,side_length_/2,30);
01774 break;
01775
01776 case G1:
01777 drawTextItalics(std::string("1"),cr,side_length_/2+x_offset,side_length_/2,30);
01778 break;
01779
01780 case G2:
01781 drawTextItalics(std::string("2"),cr,side_length_/2+x_offset,side_length_/2,30);
01782 break;
01783
01784 case G3:
01785 drawTextItalics(std::string("3"),cr,side_length_/2+x_offset,side_length_/2,30);
01786 break;
01787
01788 case G4:
01789 drawTextItalics(std::string("4"),cr,side_length_/2+x_offset,side_length_/2,30);
01790 break;
01791
01792 case G5:
01793 drawTextItalics(std::string("5"),cr,side_length_/2+x_offset,side_length_/2,30);
01794 break;
01795 }
01796
01797
01798 int p01=0.1*side_length_;
01799 int p09=0.9*side_length_;
01800
01801 cr->set_line_width(2.0);
01802 cr->set_line_join(Cairo::LINE_JOIN_BEVEL);
01803
01804 cr->move_to(p01,p01);
01805 cr->line_to(p01,p09);
01806 cr->line_to(p09,p09);
01807 cr->line_to(p09,p01);
01808 cr->line_to(p01,p01);
01809 cr->line_to(p01,p09);
01810
01811 cr->stroke();
01812
01813 return true;
01814 }
01815
01816 int side_length_;
01817 gear_enum gear_;
01818 };
01819
01820 class TurnSignalDrawnigArea: public Gtk::DrawingArea
01821 {
01822 public:
01823 enum orientation_enum {LEFT=1, RIGHT=2};
01824
01825 TurnSignalDrawnigArea(orientation_enum o,int side_length=50):
01826 side_length_(side_length),
01827 orientation_(o)
01828 {
01829 active_=false;
01830
01831 override_background_color(Gdk::RGBA("white"));
01832
01833 set_size_request(side_length_,side_length_);
01834 }
01835
01836 virtual ~TurnSignalDrawnigArea()
01837 {}
01838
01839 void setStatus(bool active)
01840 {
01841 active_=active;
01842 queue_draw();
01843 }
01844
01845 protected:
01846
01847
01848 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
01849 {
01850 Gtk::Allocation allocation = get_allocation();
01851 const int width = allocation.get_width();
01852 const int height = allocation.get_height();
01853
01854 cr->scale(width, height);
01855
01856 cr->set_line_width(0.05);
01857 cr->set_line_cap(Cairo::LINE_CAP_ROUND);
01858 cr->set_line_join(Cairo::LINE_JOIN_ROUND);
01859
01860 if(active_)
01861 cr->set_source_rgb(0.0, 1.0, 0.0);
01862 else
01863 cr->set_source_rgb(0.2, 0.2, 0.2);
01864
01865 double p01=0.1;
01866 double p03=0.3;
01867 double p05=0.5;
01868 double p07=0.7;
01869 double p09=0.9;
01870
01871 if(orientation_==LEFT)
01872 {
01873 cr->move_to(p01,p05);
01874 cr->line_to(p05,p01);
01875 cr->line_to(p05,p03);
01876 cr->line_to(p09,p03);
01877 cr->line_to(p09,p07);
01878 cr->line_to(p05,p07);
01879 cr->line_to(p05,p09);
01880 cr->line_to(p01,p05);
01881
01882 }else if(orientation_==RIGHT)
01883 {
01884 cr->move_to(p09,p05);
01885 cr->line_to(p05,p01);
01886 cr->line_to(p05,p03);
01887 cr->line_to(p01,p03);
01888 cr->line_to(p01,p07);
01889 cr->line_to(p05,p07);
01890 cr->line_to(p05,p09);
01891 cr->line_to(p09,p05);
01892 }
01893
01894 cr->fill();
01895
01896 cr->set_line_width(0.13);
01897
01898 if(active_)
01899 cr->set_source_rgba(0.0, 1.0, 0.0,0.3);
01900 else
01901 cr->set_source_rgba(0.2, 0.2, 0.2,0.0);
01902
01903 if(orientation_==LEFT)
01904 {
01905 cr->move_to(p01,p05);
01906 cr->line_to(p05,p01);
01907 cr->line_to(p05,p03);
01908 cr->line_to(p09,p03);
01909 cr->line_to(p09,p07);
01910 cr->line_to(p05,p07);
01911 cr->line_to(p05,p09);
01912 cr->line_to(p01,p05);
01913
01914 }else if(orientation_==RIGHT)
01915 {
01916 cr->move_to(p09,p05);
01917 cr->line_to(p05,p01);
01918 cr->line_to(p05,p03);
01919 cr->line_to(p01,p03);
01920 cr->line_to(p01,p07);
01921 cr->line_to(p05,p07);
01922 cr->line_to(p05,p09);
01923 cr->line_to(p09,p05);
01924 }
01925
01926 cr->stroke();
01927
01928 return true;
01929 }
01930
01931 bool active_;
01932 int side_length_;
01933
01934 orientation_enum orientation_;
01935 };
01936
01937 class RpmsDrawingArea: public ExtendedDrawingArea
01938 {
01939 public:
01940 RpmsDrawingArea(int side_length=250,double max_rpm=7000, double large_step=1000,double min_angle=145*pi/180., double max_angle=320*pi/180.):
01941 side_length_(side_length),
01942 max_rpm_(max_rpm),
01943 large_step_(large_step),
01944 min_angle_(min_angle),
01945 max_angle_(max_angle),
01946 rpm_value_(0)
01947 {
01948 override_background_color(Gdk::RGBA("white"));
01949 set_size_request(side_length_,side_length_);
01950 }
01951
01952 virtual ~RpmsDrawingArea()
01953 {}
01954
01955 void setRpm(double rpm)
01956 {
01957 if(rpm!=rpm_value_)
01958 queue_draw();
01959
01960 if(rpm>max_rpm_)
01961 rpm=max_rpm_;
01962 if(rpm<0)
01963 rpm=0;
01964
01965 rpm_value_=rpm;
01966 }
01967
01968 protected:
01969
01970
01971 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
01972 {
01973 Gtk::Allocation allocation = get_allocation();
01974 const int width = allocation.get_width();
01975 const int height = allocation.get_height();
01976
01977 cr->scale(width,height);
01978
01979 Vector2d pc(0.5,0.5);
01980
01981 double pointer_length = 0.48;
01982
01983
01984 double in_length = 0.44;
01985 double out_length = 0.49;
01986
01987 cr->set_source_rgb(0,0,0);
01988 cr->set_line_width(0.008);
01989
01990 double alpha = 0.1;
01991 for(uint rpm=max_rpm_-3*large_step_;rpm<max_rpm_;rpm+=large_step_)
01992 {
01993 double thi = min_angle_+rpm*(max_angle_-min_angle_)/max_rpm_;
01994 thi+=2.0*pi/180.;
01995 double thf = min_angle_+(rpm+large_step_/2)*(max_angle_-min_angle_)/max_rpm_;
01996 thf-=1.0*pi/190.;
01997
01998 Vector2d diri(cos(thi),sin(thi));
01999 diri.normalize();
02000
02001 Vector2d dirf(cos(thf),sin(thf));
02002 dirf.normalize();
02003
02004 cr->set_source_rgba(1,0,0,alpha);
02005 cr->arc(pc(0),pc(1),out_length,thi,thf);
02006 cr->fill();
02007
02008 Vector2d p1 = pc+diri*out_length;
02009 Vector2d p2 = pc+dirf*out_length;
02010
02011 cr->move_to(pc(0),pc(1));
02012 cr->line_to(p1(0),p1(1));
02013 cr->line_to(p2(0),p2(1));
02014 cr->close_path();
02015 cr->fill();
02016
02017 cr->set_source_rgb(1,1,1);
02018 cr->arc(pc(0),pc(1),in_length,thi,thf);
02019 cr->fill();
02020
02021 Vector2d p1c = pc+diri*in_length;
02022 Vector2d p2c = pc+dirf*in_length;
02023
02024 cr->move_to(p1c(0),p1c(1));
02025 cr->line_to(pc(0),pc(1));
02026 cr->line_to(p2c(0),p2c(1));
02027 cr->fill_preserve();
02028 cr->stroke();
02029
02030 cr->set_source_rgba(1,0,0,alpha+.225);
02031
02032 thi = min_angle_+(rpm+large_step_/2)*(max_angle_-min_angle_)/max_rpm_;
02033 thi+=1.0*pi/180.;
02034 thf = min_angle_+(rpm+large_step_)*(max_angle_-min_angle_)/max_rpm_;
02035 thf-=2.0*pi/190.;
02036
02037 Vector2d diri2(cos(thi),sin(thi));
02038 diri2.normalize();
02039
02040 Vector2d dirf2(cos(thf),sin(thf));
02041 dirf2.normalize();
02042
02043 cr->arc(pc(0),pc(1),out_length,thi,thf);
02044 cr->fill();
02045
02046 Vector2d p12 = pc+diri2*out_length;
02047 Vector2d p22 = pc+dirf2*out_length;
02048
02049 cr->move_to(pc(0),pc(1));
02050 cr->line_to(p12(0),p12(1));
02051 cr->line_to(p22(0),p22(1));
02052 cr->close_path();
02053 cr->fill();
02054
02055 cr->set_source_rgb(1,1,1);
02056 cr->arc(pc(0),pc(1),in_length,thi,thf);
02057 cr->fill();
02058
02059 Vector2d p12c = pc+diri2*in_length;
02060 Vector2d p22c = pc+dirf2*in_length;
02061
02062 cr->move_to(p12c(0),p12c(1));
02063 cr->line_to(pc(0),pc(1));
02064 cr->line_to(p22c(0),p22c(1));
02065 cr->fill_preserve();
02066 cr->stroke();
02067
02068 alpha+=.45;
02069 }
02070
02071 cr->set_line_width(0.005);
02072 cr->arc(pc(0),pc(1),0.018,0,2*pi);
02073 cr->stroke_preserve();
02074 cr->set_source_rgb(0.4,0.4,0.4);
02075 cr->fill();
02076
02077
02078 for(uint rpm=0;rpm<=max_rpm_;rpm+=large_step_)
02079 {
02080 double th = min_angle_+rpm*(max_angle_-min_angle_)/max_rpm_;
02081 th+=-0.8*pi/180.;
02082
02083 Vector2d dir1(cos(th),sin(th));
02084 dir1.normalize();
02085
02086 Vector2d pin1 = pc+dir1*in_length;
02087 Vector2d pout1 = pc+dir1*out_length;
02088
02089 cr->move_to(pin1(0),pin1(1));
02090 cr->line_to(pout1(0),pout1(1));
02091 cr->stroke();
02092
02093 th+=2*0.8*pi/180.;
02094
02095 Vector2d dir2(cos(th),sin(th));
02096 dir2.normalize();
02097
02098 Vector2d pin2 = pc+dir2*in_length;
02099 Vector2d pout2 = pc+dir2*out_length;
02100
02101 cr->move_to(pin2(0),pin2(1));
02102 cr->line_to(pout2(0),pout2(1));
02103 cr->stroke();
02104 }
02105
02106 for(uint rpm=large_step_/2.;rpm<=max_rpm_;rpm+=large_step_)
02107 {
02108 double th = min_angle_+rpm*(max_angle_-min_angle_)/max_rpm_;
02109
02110 Vector2d dir1(cos(th),sin(th));
02111 dir1.normalize();
02112
02113 Vector2d pin1 = pc+dir1*in_length;
02114 Vector2d pout1 = pc+dir1*out_length;
02115
02116 cr->move_to(pin1(0),pin1(1));
02117 cr->line_to(pout1(0),pout1(1));
02118 cr->stroke();
02119 }
02120
02121
02122 cr->scale(1./width,1./height);
02123
02124 cr->set_source_rgb(0.0, 0.0, 0.0);
02125
02126 drawTextItalics("x1000r/min",cr,width/2.,(1./4.)*height,8);
02127
02128 int rpmi=rpm_value_;
02129 drawText(boost::lexical_cast<std::string>(rpmi),cr,width/2.,(3./4.)*height,12);
02130
02131 Vector2d pcu(0.5*width,0.5*height);
02132
02133 for(uint rpm=0;rpm<=max_rpm_;rpm+=large_step_)
02134 {
02135 double th = min_angle_+rpm*(max_angle_-min_angle_)/max_rpm_;
02136
02137 Vector2d dir(cos(th),sin(th));
02138 dir.normalize();
02139
02140 Vector2d pin = pcu+dir*0.39*side_length_;
02141
02142 int rpmi = rpm/1000.;
02143
02144 drawTextItalics(boost::lexical_cast<std::string>(rpmi),cr,pin(0),pin(1),11);
02145 }
02146
02147 cr->scale(width,height);
02148
02149 double theta = min_angle_+rpm_value_*(max_angle_-min_angle_)/max_rpm_;
02150
02151 Vector2d v1(cos(theta),sin(theta));
02152
02153 v1.normalize();
02154
02155 Vector2d pend(pc(0)+v1(0)*pointer_length,pc(1)+v1(1)*pointer_length);
02156
02157 cr->set_line_width(0.025);
02158 cr->set_line_cap(Cairo::LINE_CAP_ROUND);
02159
02160
02161 cr->set_source_rgb(0.5, 0.0, 0.0);
02162 cr->move_to(pc(0), pc(1));
02163 cr->line_to(pend(0), pend(1));
02164
02165 cr->stroke();
02166
02167 pend=pc+v1*pointer_length;
02168
02169 cr->set_line_width(0.015);
02170 cr->set_source_rgb(1.0, 0.0, 0.0);
02171 cr->move_to(pc(0), pc(1));
02172 cr->line_to(pend(0), pend(1));
02173
02174 cr->stroke();
02175
02176 return true;
02177 }
02178
02179
02180 int side_length_;
02181 double max_rpm_;
02182 double large_step_;
02183 double min_angle_;
02184 double max_angle_;
02185
02186 double rpm_value_;
02187 };
02188
02189 class VelocityDrawingArea: public ExtendedDrawingArea
02190 {
02191 public:
02192 VelocityDrawingArea(int side_length=350,double max_velocity=140, double large_step=10,double min_angle=145*pi/180., double max_angle=350*pi/180.):
02193 side_length_(side_length),
02194 max_velocity_(max_velocity),
02195 large_step_(large_step),
02196 min_angle_(min_angle),
02197 max_angle_(max_angle),
02198 velocity_value_(0)
02199 {
02200 override_background_color(Gdk::RGBA("white"));
02201 set_size_request(side_length,side_length);
02202 }
02203
02204 virtual ~VelocityDrawingArea()
02205 {}
02206
02207 void setVelocity(double velocity)
02208 {
02209 if(velocity!=velocity_value_)
02210 queue_draw();
02211
02212 if(velocity<0)
02213 {
02214 velocity=-velocity;
02215 negative_=true;
02216 }else
02217 negative_=false;
02218
02219 velocity_value_=velocity;
02220 }
02221
02222 protected:
02223
02224
02225 virtual bool on_draw(const Cairo::RefPtr<Cairo::Context>& cr)
02226 {
02227 Gtk::Allocation allocation = get_allocation();
02228 const int width = allocation.get_width();
02229 const int height = allocation.get_height();
02230
02231 cr->scale(width,height);
02232
02233 Vector2d pc(0.5,0.5);
02234
02235 cr->set_line_width(0.005);
02236 cr->arc(pc(0),pc(1),0.018,0,2*pi);
02237 cr->stroke_preserve();
02238 cr->set_source_rgb(0.4,0.4,0.4);
02239 cr->fill();
02240
02241 double pointer_length = 0.48;
02242
02243
02244 double in_length = 0.44;
02245 double out_length = 0.49;
02246
02247 cr->set_source_rgb(0,0,0);
02248 cr->set_line_width(0.008);
02249
02250 for(uint vel=0;vel<=max_velocity_;vel+=large_step_)
02251 {
02252 double th = min_angle_+vel*(max_angle_-min_angle_)/max_velocity_;
02253 th+=-0.8*pi/180.;
02254
02255 Vector2d dir1(cos(th),sin(th));
02256 dir1.normalize();
02257
02258 Vector2d pin1 = pc+dir1*in_length;
02259 Vector2d pout1 = pc+dir1*out_length;
02260
02261 cr->move_to(pin1(0),pin1(1));
02262 cr->line_to(pout1(0),pout1(1));
02263 cr->stroke();
02264
02265 th+=2*0.8*pi/180.;
02266
02267 Vector2d dir2(cos(th),sin(th));
02268 dir2.normalize();
02269
02270 Vector2d pin2 = pc+dir2*in_length;
02271 Vector2d pout2 = pc+dir2*out_length;
02272
02273 cr->move_to(pin2(0),pin2(1));
02274 cr->line_to(pout2(0),pout2(1));
02275 cr->stroke();
02276 }
02277
02278 double vel = max_velocity_ + large_step_/2.;
02279 for(uint i=0;i<3;i++)
02280 {
02281 double th = min_angle_+vel*(max_angle_-min_angle_)/max_velocity_;
02282
02283 Vector2d dir1(cos(th),sin(th));
02284 dir1.normalize();
02285
02286 Vector2d pin1 = pc+dir1*(in_length+out_length)/2.;
02287
02288 cr->arc(pin1(0),pin1(1),0.008,0,2*pi);
02289 cr->fill();
02290
02291 vel+=large_step_/2.;
02292 }
02293
02294 for(uint vel=large_step_/2.;vel<=max_velocity_;vel+=large_step_)
02295 {
02296 double th = min_angle_+vel*(max_angle_-min_angle_)/max_velocity_;
02297
02298 Vector2d dir1(cos(th),sin(th));
02299 dir1.normalize();
02300
02301 Vector2d pin1 = pc+dir1*in_length;
02302 Vector2d pout1 = pc+dir1*out_length;
02303
02304 cr->move_to(pin1(0),pin1(1));
02305 cr->line_to(pout1(0),pout1(1));
02306 cr->stroke();
02307 }
02308
02309 cr->scale(1./width,1./height);
02310
02311 cr->set_source_rgb(0.0, 0.0, 0.0);
02312
02313 drawTextItalics("km/h",cr,width/2.,(1./4.)*height,8);
02314
02315 int veli = velocity_value_*3.6;
02316
02317 if(negative_)
02318 drawText("-" + boost::lexical_cast<std::string>(veli),cr,width/2.,(3./4.)*height,12);
02319 else
02320 drawText(boost::lexical_cast<std::string>(veli),cr,width/2.,(3./4.)*height,12);
02321
02322 Vector2d pcu(0.5*width,0.5*height);
02323
02324 vel=0;
02325 for(;vel<=max_velocity_;vel+=large_step_)
02326 {
02327 double th = min_angle_+vel*(max_angle_-min_angle_)/max_velocity_;
02328
02329 Vector2d dir(cos(th),sin(th));
02330 dir.normalize();
02331
02332 Vector2d pin = pcu+dir*0.39*side_length_;
02333
02334 drawTextItalics(boost::lexical_cast<std::string>(vel),cr,pin(0),pin(1),11);
02335 }
02336
02337 vel+=large_step_*1.1;
02338
02339 double th = min_angle_+vel*(max_angle_-min_angle_)/max_velocity_;
02340 Vector2d dir(cos(th),sin(th));
02341 Vector2d pin = pcu+dir*0.45*side_length_;
02342
02343 drawTextItalicsNotBold(std::string("\u221E"),cr,pin(0),pin(1),28);
02344
02345
02346 cr->scale(width,height);
02347
02348 velocity_value_=velocity_value_*3.6;
02349
02350 double theta = min_angle_+velocity_value_*(max_angle_-min_angle_)/max_velocity_;
02351
02352 Vector2d v1(cos(theta),sin(theta));
02353
02354 v1.normalize();
02355
02356 Vector2d pend(pc(0)+v1(0)*pointer_length,pc(1)+v1(1)*pointer_length);
02357
02358 cr->set_line_width(0.025);
02359 cr->set_line_cap(Cairo::LINE_CAP_ROUND);
02360
02361
02362 cr->set_source_rgb(0.5, 0.0, 0.0);
02363 cr->move_to(pc(0), pc(1));
02364 cr->line_to(pend(0), pend(1));
02365
02366 cr->stroke();
02367
02368 pend=pc+v1*pointer_length;
02369
02370 cr->set_line_width(0.015);
02371 cr->set_source_rgb(1.0, 0.0, 0.0);
02372 cr->move_to(pc(0), pc(1));
02373 cr->line_to(pend(0), pend(1));
02374
02375 cr->stroke();
02376
02377 return true;
02378 }
02379
02380 int side_length_;
02381 double max_velocity_;
02382 double large_step_;
02383 double min_angle_;
02384 double max_angle_;
02385
02386 double velocity_value_;
02387 bool negative_;
02388 };
02389
02390 class ManagerGui: public Gtk::Window
02391 {
02392 public:
02393 ManagerGui(volatile sig_atomic_t* shutdown_request = NULL):
02394 top_box_(false,5),
02395 main_division_box_(false,5),
02396 rpms_container_box_(false,5),
02397 velocity_container_box_(false,5),
02398 right_container_box_(false,5),
02399 left_container_box_(false,5),
02400 turning_signals_box_(false,5),
02401 lights_box_(false,5),
02402 warnings_box_(false,5),
02403 auto_box_(false,5),
02404 pedal_box_(false,5),
02405 status_box_(false,5),
02406 gamepad_info_(),
02407 nh_("~"),
02408 left_turn_signal_(TurnSignalDrawnigArea::LEFT),
02409 right_turn_signal_(TurnSignalDrawnigArea::RIGHT),
02410 gear_drawing_area_(GearDrawnigArea::NEUTRAL),
02411 maximum_lights_drawing_area_(LightsDrawingArea::MAXIMUM),
02412 medium_lights_drawing_area_(LightsDrawingArea::MEDIUM),
02413 emergency_drawing_area_(150),
02414 auto_brake_(80,std::string("Brake")),
02415 auto_clutch_(80,std::string("Clutch")),
02416 auto_direction_(80,std::string("Direction")),
02417 auto_ignition_(80,std::string("Ignition")),
02418 auto_throttle_(80,std::string("Throttle")),
02419 brake_pedal_(std::string("Brake"),0,1),
02420 throttle_pedal_(std::string("Throttle"),0,1),
02421 clutch_pedal_(std::string("Clutch"),0,1),
02422 operation_mode_(50),
02423 shutdown_request_(shutdown_request)
02424 {
02425
02426 set_title("Atlascar Manager GUI");
02427 set_icon_from_file(ros::package::getPath("atlascar_base")+"/images/logo.png");
02428
02429
02430 status_sub_ = nh_.subscribe("status", 1, &ManagerGui::statusCallback, this);
02431
02432 command_pub_ = nh_.advertise<atlascar_base::ManagerCommand>("command", 1);
02433
02434
02435 nh_.param("gamepad_device",gamepad_info_.device_,std::string("Not found"));
02436 gamepad_info_.initializeGamepad();
02437
02438 nh_.param("gamepad_priority",gamepad_priority_,2);
02439 nh_.param("gamepad_lifetime",gamepad_lifetime_,std::numeric_limits<double>::infinity());
02440
02441
02442 add(top_box_);
02443
02444 top_box_.pack_start(main_division_box_,false,false,0);
02445 top_box_.pack_start(separator_,false,false,0);
02446 top_box_.pack_start(status_box_,false,false,5);
02447 top_box_.set_halign(Gtk::ALIGN_CENTER);
02448
02449 main_division_box_.pack_start(left_container_box_,false,false,5);
02450 main_division_box_.pack_start(right_container_box_,false,false,5);
02451 main_division_box_.set_halign(Gtk::ALIGN_CENTER);
02452
02453 right_container_box_.pack_start(rpms_container_box_,false,false,5);
02454 right_container_box_.pack_start(velocity_container_box_,false,false,5);
02455 right_container_box_.pack_start(pressure_sensors_,false,false,5);
02456
02457 right_container_box_.set_halign(Gtk::ALIGN_CENTER);
02458
02459 rpms_container_box_.pack_start(rpms_drawing_area_,false,false,5);
02460 rpms_container_box_.set_halign(Gtk::ALIGN_CENTER);
02461 velocity_container_box_.pack_start(velocity_drawing_area_,false,false,5);
02462 velocity_container_box_.set_halign(Gtk::ALIGN_CENTER);
02463
02464 left_container_box_.pack_start(turning_signals_box_,false,false,5);
02465 left_container_box_.pack_start(lights_box_,false,false,5);
02466 left_container_box_.pack_start(warnings_box_,false,false,5);
02467 left_container_box_.pack_start(auto_box_,false,false,5);
02468 left_container_box_.pack_start(pedal_box_,false,false,5);
02469
02470 lights_box_.pack_start(steering_wheel_drawing_area_,false,false,5);
02471 lights_box_.set_halign(Gtk::ALIGN_CENTER);
02472
02473 warnings_box_.pack_start(emergency_drawing_area_,false,false,5);
02474 warnings_box_.set_halign(Gtk::ALIGN_CENTER);
02475
02476 turning_signals_box_.pack_start(left_turn_signal_,false,false,0);
02477 turning_signals_box_.pack_start(right_turn_signal_,false,false,0);
02478
02479 turning_signals_box_.pack_start(maximum_lights_drawing_area_,false,false,0);
02480 turning_signals_box_.pack_start(medium_lights_drawing_area_,false,false,0);
02481 turning_signals_box_.pack_start(gear_drawing_area_,false,false,0);
02482 turning_signals_box_.pack_start(ignition_drawing_area_,false,false,0);
02483 turning_signals_box_.set_halign(Gtk::ALIGN_CENTER);
02484
02485 auto_box_.pack_start(auto_brake_,false,false,0);
02486 auto_box_.pack_start(auto_clutch_,false,false,0);
02487 auto_box_.pack_start(auto_direction_,false,false,0);
02488 auto_box_.pack_start(auto_ignition_,false,false,0);
02489 auto_box_.pack_start(auto_throttle_,false,false,0);
02490 auto_box_.set_halign(Gtk::ALIGN_CENTER);
02491
02492 pedal_box_.pack_start(brake_pedal_,false,false,0);
02493 pedal_box_.pack_start(throttle_pedal_,false,false,0);
02494 pedal_box_.pack_start(clutch_pedal_,false,false,0);
02495 pedal_box_.set_halign(Gtk::ALIGN_CENTER);
02496
02497 status_box_.pack_start(gamepad_info_,false,false,5);
02498 status_box_.pack_start(operation_mode_,false,false,5);
02499 status_box_.set_halign(Gtk::ALIGN_CENTER);
02500
02501 operation_mode_.signal_button_press_event().connect(sigc::mem_fun(*this,&ManagerGui::operationModeClickEvent));
02502 gamepad_info_.event_ = sigc::mem_fun<const atlascar_base::ManagerCommandPtr>(*this,&ManagerGui::gamepadEvent);
02503
02504
02505 sigc::slot<bool> rapid_slot = sigc::bind(sigc::mem_fun(*this,&ManagerGui::rapidTimeout),0);
02506 sigc::slot<bool> ros_slot = sigc::bind(sigc::mem_fun(*this,&ManagerGui::rosTimeout),0);
02507
02508
02509
02510 rapidTimeoutConnection = Glib::signal_timeout().connect(rapid_slot,10);
02511 rosTimeoutConnection = Glib::signal_timeout().connect(ros_slot,20);
02512
02513 command_.reset(new atlascar_base::ManagerCommand);
02514
02515 override_background_color(Gdk::RGBA("white"));
02516 top_box_.override_background_color(Gdk::RGBA("white"));
02517
02518 show_all_children();
02519 }
02520
02521 void selectControlMethodDialog()
02522 {
02524 Gtk::MessageDialog dialog(*this, "Please select the control operation mode.",false, Gtk::MESSAGE_QUESTION,Gtk::BUTTONS_NONE);
02525 dialog.set_secondary_text("DIRECT mode sends raw command values to the controller while HIGH sends processed values.");
02526
02527 dialog.add_button("DIRECT",OperationMode::DIRECT);
02528 dialog.add_button("HIGH",OperationMode::HIGH);
02529
02530
02531 int result = dialog.run();
02532
02533
02534 switch(result)
02535 {
02536 case OperationMode::DIRECT:
02537 operation_mode_=OperationMode::DIRECT;
02538 break;
02539 case OperationMode::HIGH:
02540 operation_mode_=OperationMode::HIGH;
02541 break;
02542 }
02543 }
02544
02545 virtual ~ManagerGui()
02546 {
02547 }
02548
02549 protected:
02550
02551 bool operationModeClickEvent(GdkEventButton *event)
02552 {
02553 if(operation_mode_==OperationMode::DIRECT)
02554 operation_mode_=OperationMode::HIGH;
02555 else
02556 operation_mode_=OperationMode::DIRECT;
02557
02558
02559 command_->direct_control = operation_mode_.getMode();
02560
02561 command_->lifetime=gamepad_lifetime_;
02562 command_->priority=gamepad_priority_;
02563
02564 command_pub_.publish(command_);
02565
02566 return true;
02567 }
02568
02569 void gamepadEvent(const atlascar_base::ManagerCommandPtr command)
02570 {
02571
02572 *command_=*command;
02573
02574
02575 command_->direct_control = operation_mode_.getMode();
02576
02577 command_->lifetime=gamepad_lifetime_;
02578 command_->priority=gamepad_priority_;
02579
02580 command_pub_.publish(command_);
02581 }
02582
02583 bool rosTimeout(int timer_number)
02584 {
02585
02586 if(shutdown_request_!=NULL)
02587 if(*shutdown_request_)
02588 Gtk::Main::quit();
02589
02590 ros::spinOnce();
02591 return true;
02592 }
02593
02594 bool rapidTimeout(int timer_number)
02595 {
02596 emergency_drawing_area_.stateManager();
02597 gamepad_info_.stateManager();
02598
02599 return true;
02600 }
02601
02602 bool slowTimeout(int timer_number)
02603 {
02604 static double theta=0;
02605 static double incrt=1;
02606
02607 if(theta<-3*pi)
02608 incrt=1;
02609
02610 if(theta>3*pi)
02611 incrt=-1;
02612
02613 theta+=incrt*0.1;
02614
02615 steering_wheel_drawing_area_.setSteeringWheel(theta);
02616
02617 static double rpms=0;
02618 static double incr=1;
02619
02620 if(rpms>7000)
02621 incr=-1;
02622
02623 if(rpms<0)
02624 incr=1;
02625
02626 rpms+=incr*100;
02627
02628 rpms_drawing_area_.setRpm(rpms);
02629
02630 static double vel=0;
02631 static double incrv=1;
02632
02633 if(vel>140)
02634 incrv=-1;;
02635
02636 if(vel<-20)
02637 incrv=1;
02638
02639 vel+=incrv*1;
02640
02641 velocity_drawing_area_.setVelocity(vel);
02642
02643 if(vel>10 && vel<20)
02644 left_turn_signal_.setStatus(1);
02645 else
02646 left_turn_signal_.setStatus(0);
02647
02648 left_turn_signal_.queue_draw();
02649
02650 if(vel>30 && vel<40)
02651 right_turn_signal_.setStatus(1);
02652 else
02653 right_turn_signal_.setStatus(0);
02654
02655 if(vel<10)
02656 gear_drawing_area_.setGear(GearDrawnigArea::REVERSE);
02657
02658 if(vel>10 && vel<30)
02659 gear_drawing_area_.setGear(GearDrawnigArea::NEUTRAL);
02660
02661 if(vel>30 && vel<60)
02662 gear_drawing_area_.setGear(GearDrawnigArea::G1);
02663
02664 if(vel>60 && vel<90)
02665 gear_drawing_area_.setGear(GearDrawnigArea::G2);
02666
02667 if(vel>90 && vel<120)
02668 gear_drawing_area_.setGear(GearDrawnigArea::G3);
02669
02670 if(vel>120 && vel<130)
02671 gear_drawing_area_.setGear(GearDrawnigArea::G4);
02672
02673 if(vel>130 && vel<140)
02674 gear_drawing_area_.setGear(GearDrawnigArea::G5);
02675
02676 if(vel>10 && vel<80)
02677 maximum_lights_drawing_area_.setStatus(true);
02678 else
02679 maximum_lights_drawing_area_.setStatus(false);
02680
02681 if(vel>30 && vel<120)
02682 medium_lights_drawing_area_.setStatus(true);
02683 else
02684 medium_lights_drawing_area_.setStatus(false);
02685
02686 if(vel>50)
02687 ignition_drawing_area_.setStatus(true);
02688 else
02689 ignition_drawing_area_.setStatus(false);
02690
02691 if(vel>30 && vel<100)
02692 emergency_drawing_area_.setStatus(true);
02693 else
02694 emergency_drawing_area_.setStatus(false);
02695
02696 if(vel>0 && vel<20)
02697 auto_brake_.setStatus(true);
02698 else
02699 auto_brake_.setStatus(false);
02700
02701 if(vel>20 && vel<40)
02702 auto_clutch_.setStatus(true);
02703 else
02704 auto_clutch_.setStatus(false);
02705
02706 if(vel>40 && vel<60)
02707 auto_direction_.setStatus(true);
02708 else
02709 auto_direction_.setStatus(false);
02710
02711 if(vel>60 && vel<80)
02712 auto_ignition_.setStatus(true);
02713 else
02714 auto_ignition_.setStatus(false);
02715
02716 if(vel>80 && vel<100)
02717 auto_throttle_.setStatus(true);
02718 else
02719 auto_throttle_.setStatus(false);
02720
02721 pressure_sensors_.setThrottle(vel*100.);
02722 pressure_sensors_.setBrake(vel*12.);
02723 pressure_sensors_.setClutch(vel*153.3);
02724
02725 brake_pedal_.setValue(vel/14.);
02726 throttle_pedal_.setValue(rpms/800.);
02727 clutch_pedal_.setValue(rpms/600.);
02728
02729 return true;
02730 }
02731
02732 void statusCallback(const atlascar_base::ManagerStatusPtr& status)
02733 {
02734 steering_wheel_drawing_area_.setSteeringWheel(status->steering_wheel);
02735
02736 rpms_drawing_area_.setRpm(status->rpm);
02737 velocity_drawing_area_.setVelocity(status->velocity);
02738
02739 left_turn_signal_.setStatus(status->lights_left);
02740 right_turn_signal_.setStatus(status->lights_right);
02741
02742 switch(status->gear)
02743 {
02744 case -1:
02745 gear_drawing_area_.setGear(GearDrawnigArea::REVERSE);
02746 break;
02747
02748 case 0:
02749 gear_drawing_area_.setGear(GearDrawnigArea::NEUTRAL);
02750 break;
02751
02752 case 1:
02753 gear_drawing_area_.setGear(GearDrawnigArea::G1);
02754 break;
02755
02756 case 2:
02757 gear_drawing_area_.setGear(GearDrawnigArea::G2);
02758 break;
02759
02760 case 3:
02761 gear_drawing_area_.setGear(GearDrawnigArea::G3);
02762 break;
02763
02764 case 4:
02765 gear_drawing_area_.setGear(GearDrawnigArea::G4);
02766 break;
02767
02768 case 5:
02769 gear_drawing_area_.setGear(GearDrawnigArea::G5);
02770 break;
02771 }
02772
02773 maximum_lights_drawing_area_.setStatus(status->lights_high);
02774 medium_lights_drawing_area_.setStatus(status->lights_medium);
02775 ignition_drawing_area_.setStatus(status->ignition);
02776
02777 emergency_drawing_area_.setStatus(status->emergency);
02778
02779 auto_brake_.setStatus(status->auto_brake);
02780 auto_clutch_.setStatus(status->auto_clutch);
02781 auto_direction_.setStatus(status->auto_direction);
02782 auto_ignition_.setStatus(status->auto_ignition);
02783 auto_throttle_.setStatus(status->auto_throttle);
02784
02785 brake_pedal_.setValue(status->brake);
02786 throttle_pedal_.setValue(status->throttle_pedal);
02787 clutch_pedal_.setValue(status->clutch);
02788
02789 pressure_sensors_.setThrottle(status->throttle_pressure);
02790 pressure_sensors_.setBrake(status->brake_pressure);
02791 pressure_sensors_.setClutch(status->clutch_pressure);
02792 }
02793
02794
02795 Gtk::VBox top_box_;
02796 Gtk::HBox main_division_box_;
02797 Gtk::HBox rpms_container_box_;
02798 Gtk::HBox velocity_container_box_;
02799 Gtk::VBox right_container_box_;
02800 Gtk::VBox left_container_box_;
02801 Gtk::HBox turning_signals_box_;
02802 Gtk::HBox lights_box_;
02803 Gtk::HBox warnings_box_;
02804 Gtk::HBox auto_box_;
02805 Gtk::HBox pedal_box_;
02806 Gtk::HBox status_box_;
02807 GamepadInfo gamepad_info_;
02808 Gtk::Separator separator_;
02809 ros::NodeHandle nh_;
02810 ros::Subscriber status_sub_;
02811 ros::Publisher command_pub_;
02812
02813 sigc::connection slowTimeoutConnection;
02814 sigc::connection rapidTimeoutConnection;
02815 sigc::connection rosTimeoutConnection;
02816
02817 RpmsDrawingArea rpms_drawing_area_;
02818 VelocityDrawingArea velocity_drawing_area_;
02819 TurnSignalDrawnigArea left_turn_signal_;
02820 TurnSignalDrawnigArea right_turn_signal_;
02821 GearDrawnigArea gear_drawing_area_;
02822 SteeringWheelDrawingArea steering_wheel_drawing_area_;
02823 LightsDrawingArea maximum_lights_drawing_area_;
02824 LightsDrawingArea medium_lights_drawing_area_;
02825 IgnitionDrawingArea ignition_drawing_area_;
02826 WarningDrawingArea emergency_drawing_area_;
02827
02828 SwitchButtonDrawingArea auto_brake_;
02829 SwitchButtonDrawingArea auto_clutch_;
02830 SwitchButtonDrawingArea auto_direction_;
02831 SwitchButtonDrawingArea auto_ignition_;
02832 SwitchButtonDrawingArea auto_throttle_;
02833
02834 PedalDrawingArea brake_pedal_;
02835 PedalDrawingArea throttle_pedal_;
02836 PedalDrawingArea clutch_pedal_;
02837
02838 PressureSensors pressure_sensors_;
02839
02840 OperationMode operation_mode_;
02841
02842 atlascar_base::ManagerCommandPtr command_;
02843 int gamepad_priority_;
02844 double gamepad_lifetime_;
02845
02846 sig_atomic_t volatile * shutdown_request_;
02847 };
02848
02849 #endif
