Fawkes API  Fawkes Development Version
naogui.cpp
00001 
00002 /***************************************************************************
00003  *  naogui.cpp - Nao GUI
00004  *
00005  *  Created: Mon Apr 14 12:54:19 2008
00006  *  Copyright  2008-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "naogui.h"
00024 
00025 #include <utils/system/argparser.h>
00026 #include <blackboard/remote.h>
00027 #include <interfaces/NaoJointPositionInterface.h>
00028 #include <interfaces/NaoJointStiffnessInterface.h>
00029 #include <interfaces/NaoSensorInterface.h>
00030 #include <interfaces/HumanoidMotionInterface.h>
00031 #include <interfaces/NavigatorInterface.h>
00032 #include <interfaces/SpeechSynthInterface.h>
00033 #include <interfaces/LedInterface.h>
00034 #include <interfaces/SwitchInterface.h>
00035 #include <netcomm/fawkes/client.h>
00036 
00037 #include <gui_utils/service_chooser_dialog.h>
00038 #include <gui_utils/interface_dispatcher.h>
00039 
00040 #include <cstring>
00041 #include <string>
00042 #include <sstream>
00043 #include <iomanip>
00044 #include <sstream>
00045 #include <gtkmm/clipboard.h>
00046 
00047 using namespace fawkes;
00048 
00049 
00050 /** @class NaoGuiGtkWindow "naogui.h"
00051  * Nao GUI main window.
00052  * The Nao GUI provides access to the most basic servo, sensor and motion
00053  * data and commands.
00054  * @author Tim Niemueller
00055  */
00056 
00057 /** Constructor.
00058  * @param cobject C base object
00059  * @param builder Gtk builder to get widgets from
00060  */
00061 NaoGuiGtkWindow::NaoGuiGtkWindow(BaseObjectType* cobject,
00062                                  const Glib::RefPtr<Gtk::Builder> &builder)
00063   : Gtk::Window(cobject)
00064 {
00065   bb = NULL;
00066   jointpos_if = NULL;
00067   jointstiff_if = NULL;
00068   sensor_if = NULL;
00069   nao_navi_if = NULL;
00070   hummot_naoqi_if = NULL;
00071   hummot_fawkes_if = NULL;
00072   speechsynth_if = NULL;
00073   servo_enabled = false;
00074 
00075   update_cycle = 0;
00076 
00077   builder->get_widget("frm_servos", frm_servos);
00078   builder->get_widget("frm_sensors", frm_sensors);
00079   builder->get_widget("frm_ultrasonic", frm_ultrasonic);
00080 
00081   builder->get_widget("lab_HeadYaw", lab_HeadYaw);
00082   builder->get_widget("lab_HeadPitch", lab_HeadPitch);
00083   builder->get_widget("lab_RShoulderPitch", lab_RShoulderPitch);
00084   builder->get_widget("lab_RShoulderRoll", lab_RShoulderRoll);
00085   builder->get_widget("lab_LShoulderPitch", lab_LShoulderPitch);
00086   builder->get_widget("lab_LShoulderRoll", lab_LShoulderRoll);
00087   builder->get_widget("lab_LElbowYaw", lab_LElbowYaw);
00088   builder->get_widget("lab_LElbowRoll", lab_LElbowRoll);
00089   builder->get_widget("lab_LWristYaw", lab_LWristYaw);
00090   builder->get_widget("lab_LHand", lab_LHand);
00091   builder->get_widget("lab_RElbowYaw", lab_RElbowYaw);
00092   builder->get_widget("lab_RElbowRoll", lab_RElbowRoll);
00093   builder->get_widget("lab_RWristYaw", lab_RWristYaw);
00094   builder->get_widget("lab_RHand", lab_RHand);
00095   builder->get_widget("lab_RHipYawPitch", lab_RHipYawPitch);
00096   builder->get_widget("lab_RHipPitch", lab_RHipPitch);
00097   builder->get_widget("lab_RHipRoll", lab_RHipRoll);
00098   builder->get_widget("lab_RKneePitch", lab_RKneePitch);
00099   builder->get_widget("lab_RAnklePitch", lab_RAnklePitch);
00100   builder->get_widget("lab_RAnkleRoll", lab_RAnkleRoll);
00101   builder->get_widget("lab_LHipYawPitch", lab_LHipYawPitch);
00102   builder->get_widget("lab_LHipPitch", lab_LHipPitch);
00103   builder->get_widget("lab_LHipRoll", lab_LHipRoll);
00104   builder->get_widget("lab_LKneePitch", lab_LKneePitch);
00105   builder->get_widget("lab_LAnklePitch", lab_LAnklePitch);
00106   builder->get_widget("lab_LAnkleRoll", lab_LAnkleRoll);
00107   builder->get_widget("lab_stiff_HeadYaw", lab_stiff_HeadYaw);
00108   builder->get_widget("lab_stiff_HeadPitch", lab_stiff_HeadPitch);
00109   builder->get_widget("lab_stiff_RShoulderPitch", lab_stiff_RShoulderPitch);
00110   builder->get_widget("lab_stiff_RShoulderRoll", lab_stiff_RShoulderRoll);
00111   builder->get_widget("lab_stiff_LShoulderPitch", lab_stiff_LShoulderPitch);
00112   builder->get_widget("lab_stiff_LShoulderRoll", lab_stiff_LShoulderRoll);
00113   builder->get_widget("lab_stiff_LElbowYaw", lab_stiff_LElbowYaw);
00114   builder->get_widget("lab_stiff_LElbowRoll", lab_stiff_LElbowRoll);
00115   builder->get_widget("lab_stiff_LWristYaw", lab_stiff_LWristYaw);
00116   builder->get_widget("lab_stiff_LHand", lab_stiff_LHand);
00117   builder->get_widget("lab_stiff_RElbowYaw", lab_stiff_RElbowYaw);
00118   builder->get_widget("lab_stiff_RElbowRoll", lab_stiff_RElbowRoll);
00119   builder->get_widget("lab_stiff_RWristYaw", lab_stiff_RWristYaw);
00120   builder->get_widget("lab_stiff_RHand", lab_stiff_RHand);
00121   builder->get_widget("lab_stiff_RHipYawPitch", lab_stiff_RHipYawPitch);
00122   builder->get_widget("lab_stiff_RHipPitch", lab_stiff_RHipPitch);
00123   builder->get_widget("lab_stiff_RHipRoll", lab_stiff_RHipRoll);
00124   builder->get_widget("lab_stiff_RKneePitch", lab_stiff_RKneePitch);
00125   builder->get_widget("lab_stiff_RAnklePitch", lab_stiff_RAnklePitch);
00126   builder->get_widget("lab_stiff_RAnkleRoll", lab_stiff_RAnkleRoll);
00127   builder->get_widget("lab_stiff_LHipYawPitch", lab_stiff_LHipYawPitch);
00128   builder->get_widget("lab_stiff_LHipPitch", lab_stiff_LHipPitch);
00129   builder->get_widget("lab_stiff_LHipRoll", lab_stiff_LHipRoll);
00130   builder->get_widget("lab_stiff_LKneePitch", lab_stiff_LKneePitch);
00131   builder->get_widget("lab_stiff_LAnklePitch", lab_stiff_LAnklePitch);
00132   builder->get_widget("lab_stiff_LAnkleRoll", lab_stiff_LAnkleRoll);
00133   builder->get_widget("hsc_HeadYaw", hsc_HeadYaw);
00134   builder->get_widget("hsc_HeadPitch", hsc_HeadPitch);
00135   builder->get_widget("hsc_RShoulderPitch", hsc_RShoulderPitch);
00136   builder->get_widget("hsc_RShoulderRoll", hsc_RShoulderRoll);
00137   builder->get_widget("hsc_RElbowYaw", hsc_RElbowYaw);
00138   builder->get_widget("hsc_RElbowRoll", hsc_RElbowRoll);
00139   builder->get_widget("hsc_RWristYaw", hsc_RWristYaw);
00140   builder->get_widget("hsc_RHand", hsc_RHand);
00141   builder->get_widget("hsc_LShoulderPitch", hsc_LShoulderPitch);
00142   builder->get_widget("hsc_LShoulderRoll", hsc_LShoulderRoll);
00143   builder->get_widget("hsc_LElbowYaw", hsc_LElbowYaw);
00144   builder->get_widget("hsc_LElbowRoll", hsc_LElbowRoll);
00145   builder->get_widget("hsc_LWristYaw", hsc_LWristYaw);
00146   builder->get_widget("hsc_LHand", hsc_LHand);
00147   builder->get_widget("hsc_RHipYawPitch", hsc_RHipYawPitch);
00148   builder->get_widget("hsc_RHipPitch", hsc_RHipPitch);
00149   builder->get_widget("hsc_RHipRoll", hsc_RHipRoll);
00150   builder->get_widget("hsc_RKneePitch", hsc_RKneePitch);
00151   builder->get_widget("hsc_RAnklePitch", hsc_RAnklePitch);
00152   builder->get_widget("hsc_RAnkleRoll", hsc_RAnkleRoll);
00153   builder->get_widget("hsc_LHipYawPitch", hsc_LHipYawPitch);
00154   builder->get_widget("hsc_LHipPitch", hsc_LHipPitch);
00155   builder->get_widget("hsc_LHipRoll", hsc_LHipRoll);
00156   builder->get_widget("hsc_LKneePitch", hsc_LKneePitch);
00157   builder->get_widget("hsc_LAnklePitch", hsc_LAnklePitch);
00158   builder->get_widget("hsc_LAnkleRoll", hsc_LAnkleRoll);
00159   builder->get_widget("hsc_speed", hsc_speed);
00160   builder->get_widget("lab_speed", lab_speed);
00161   builder->get_widget("tb_connection", tb_connection);
00162   builder->get_widget("tb_stiffness", tb_stiffness);
00163   builder->get_widget("tb_control", tb_control);
00164   builder->get_widget("tb_getup", tb_getup);
00165   builder->get_widget("tb_parkpos", tb_parkpos);
00166   builder->get_widget("tb_zeroall", tb_zeroall);
00167   builder->get_widget("tb_exit", tb_exit);
00168   builder->get_widget("lab_l_fsr_fl", lab_l_fsr_fl);
00169   builder->get_widget("lab_l_fsr_fr", lab_l_fsr_fr);
00170   builder->get_widget("lab_l_fsr_rl", lab_l_fsr_rl);
00171   builder->get_widget("lab_l_fsr_rr", lab_l_fsr_rr);
00172   builder->get_widget("lab_r_fsr_fl", lab_r_fsr_fl);
00173   builder->get_widget("lab_r_fsr_fr", lab_r_fsr_fr);
00174   builder->get_widget("lab_r_fsr_rl", lab_r_fsr_rl);
00175   builder->get_widget("lab_r_fsr_rr", lab_r_fsr_rr);
00176   builder->get_widget("lab_l_cop", lab_l_cop);
00177   builder->get_widget("lab_r_cop", lab_r_cop);
00178   builder->get_widget("lab_l_total_weight", lab_l_total_weight);
00179   builder->get_widget("lab_r_total_weight", lab_r_total_weight);
00180   builder->get_widget("lab_chest_button", lab_chest_button);
00181   builder->get_widget("lab_touch_front", lab_touch_front);
00182   builder->get_widget("lab_touch_middle", lab_touch_middle);
00183   builder->get_widget("lab_touch_rear", lab_touch_rear);
00184   builder->get_widget("lab_l_bumper_l", lab_l_bumper_l);
00185   builder->get_widget("lab_l_bumper_r", lab_l_bumper_r);
00186   builder->get_widget("lab_r_bumper_l", lab_r_bumper_l);
00187   builder->get_widget("lab_r_bumper_r", lab_r_bumper_r);
00188   builder->get_widget("lab_accel_x", lab_accel_x);
00189   builder->get_widget("lab_accel_y", lab_accel_y);
00190   builder->get_widget("lab_accel_z", lab_accel_z);
00191   builder->get_widget("lab_gyro_x", lab_gyro_x);
00192   builder->get_widget("lab_gyro_y", lab_gyro_y);
00193   builder->get_widget("lab_gyro_ref", lab_gyro_ref);
00194   builder->get_widget("lab_angles_xy", lab_angles_xy);
00195   builder->get_widget("lab_ultrasonic_direction", lab_ultrasonic_direction);
00196   builder->get_widget("lab_ultrasonic_left0", lab_ultrasonic_left0);
00197   builder->get_widget("lab_ultrasonic_left1", lab_ultrasonic_left1);
00198   builder->get_widget("lab_ultrasonic_left2", lab_ultrasonic_left2);
00199   builder->get_widget("lab_ultrasonic_left3", lab_ultrasonic_left3);
00200   builder->get_widget("lab_ultrasonic_right0", lab_ultrasonic_right0);
00201   builder->get_widget("lab_ultrasonic_right1", lab_ultrasonic_right1);
00202   builder->get_widget("lab_ultrasonic_right2", lab_ultrasonic_right2);
00203   builder->get_widget("lab_ultrasonic_right3", lab_ultrasonic_right3);
00204   builder->get_widget("lab_battery_charge", lab_battery_charge);
00205   builder->get_widget("but_sv_copy", but_sv_copy);
00206   builder->get_widget("cmb_us_direction", cmb_us_direction);
00207   builder->get_widget("but_us_emit", but_us_emit);
00208   builder->get_widget("but_us_auto", but_us_auto);
00209   builder->get_widget("but_stop", but_stop);
00210   builder->get_widget("but_ws_exec", but_ws_exec);
00211   builder->get_widget("ent_ws_distance", ent_ws_distance);
00212   builder->get_widget("but_wsw_exec", but_wsw_exec);
00213   builder->get_widget("ent_wsw_distance", ent_wsw_distance);
00214   builder->get_widget("but_wa_exec", but_wa_exec);
00215   builder->get_widget("ent_wa_angle", ent_wa_angle);
00216   builder->get_widget("ent_wa_radius", ent_wa_radius);
00217   builder->get_widget("but_turn_exec", but_turn_exec);
00218   builder->get_widget("cmb_kick_leg", cmb_kick_leg);
00219   builder->get_widget("ent_kick_strength", ent_kick_strength);
00220   builder->get_widget("but_kick_exec", but_kick_exec);
00221   builder->get_widget("cmb_standup_from", cmb_standup_from);
00222   builder->get_widget("but_standup_exec", but_standup_exec);
00223   builder->get_widget("ent_turn_angle", ent_turn_angle);
00224   builder->get_widget("rad_motion_fawkes", rad_motion_fawkes);
00225   builder->get_widget("rad_motion_naoqi", rad_motion_naoqi);
00226   builder->get_widget("ent_walkvel_x", ent_walkvel_x);
00227   builder->get_widget("ent_walkvel_y", ent_walkvel_y);
00228   builder->get_widget("ent_walkvel_theta", ent_walkvel_theta);
00229   builder->get_widget("ent_walkvel_speed", ent_walkvel_speed);
00230   builder->get_widget("but_walkvel_exec", but_walkvel_exec);
00231 
00232   builder->get_widget("but_stiffness_read", but_stiffness_read);
00233   builder->get_widget("but_stiffness_write", but_stiffness_write);
00234   builder->get_widget("chb_stiffness_global", chb_stiffness_global);
00235   builder->get_widget("spb_stiffness_global", spb_stiffness_global);
00236   builder->get_widget("spb_HeadYaw", spb_HeadYaw);
00237   builder->get_widget("spb_HeadPitch", spb_HeadPitch);
00238   builder->get_widget("spb_RShoulderPitch", spb_RShoulderPitch);
00239   builder->get_widget("spb_RShoulderRoll", spb_RShoulderRoll);
00240   builder->get_widget("spb_RElbowYaw", spb_RElbowYaw);
00241   builder->get_widget("spb_RElbowRoll", spb_RElbowRoll);
00242   builder->get_widget("spb_RWristYaw", spb_RWristYaw);
00243   builder->get_widget("spb_RHand", spb_RHand);
00244   builder->get_widget("spb_LShoulderPitch", spb_LShoulderPitch);
00245   builder->get_widget("spb_LShoulderRoll", spb_LShoulderRoll);
00246   builder->get_widget("spb_LElbowYaw", spb_LElbowYaw);
00247   builder->get_widget("spb_LElbowRoll", spb_LElbowRoll);
00248   builder->get_widget("spb_LWristYaw", spb_LWristYaw);
00249   builder->get_widget("spb_LHand", spb_LHand);
00250   builder->get_widget("spb_RHipYawPitch", spb_RHipYawPitch);
00251   builder->get_widget("spb_RHipPitch", spb_RHipPitch);
00252   builder->get_widget("spb_RHipRoll", spb_RHipRoll);
00253   builder->get_widget("spb_RKneePitch", spb_RKneePitch);
00254   builder->get_widget("spb_RAnklePitch", spb_RAnklePitch);
00255   builder->get_widget("spb_RAnkleRoll", spb_RAnkleRoll);
00256   builder->get_widget("spb_LHipYawPitch", spb_LHipYawPitch);
00257   builder->get_widget("spb_LHipPitch", spb_LHipPitch);
00258   builder->get_widget("spb_LHipRoll", spb_LHipRoll);
00259   builder->get_widget("spb_LKneePitch", spb_LKneePitch);
00260   builder->get_widget("spb_LAnklePitch", spb_LAnklePitch);
00261   builder->get_widget("spb_LAnkleRoll", spb_LAnkleRoll);
00262 
00263   builder->get_widget("ent_nav_x", ent_nav_x);
00264   builder->get_widget("ent_nav_y", ent_nav_y);
00265   builder->get_widget("ent_nav_ori", ent_nav_ori);
00266   builder->get_widget("but_nav_exec", but_nav_exec);
00267 
00268   builder->get_widget("ent_tts", ent_tts);
00269   builder->get_widget("but_tts_exec", but_tts_exec);
00270   builder->get_widget("lab_tts_active", lab_tts_active);
00271 
00272   builder->get_widget("scl_chest_r",      scl_chest_r);
00273   builder->get_widget("scl_chest_g",      scl_chest_g);
00274   builder->get_widget("scl_chest_b",      scl_chest_b);
00275   builder->get_widget("scl_left_eye_r",   scl_left_eye_r);
00276   builder->get_widget("scl_left_eye_g",   scl_left_eye_g);
00277   builder->get_widget("scl_left_eye_b",   scl_left_eye_b);
00278   builder->get_widget("scl_right_eye_r",  scl_right_eye_r);
00279   builder->get_widget("scl_right_eye_g",  scl_right_eye_g);
00280   builder->get_widget("scl_right_eye_b",  scl_right_eye_b);
00281   builder->get_widget("scl_left_foot_r",  scl_left_foot_r);
00282   builder->get_widget("scl_left_foot_g",  scl_left_foot_g);
00283   builder->get_widget("scl_left_foot_b",  scl_left_foot_b);
00284   builder->get_widget("scl_right_foot_r", scl_right_foot_r);
00285   builder->get_widget("scl_right_foot_g", scl_right_foot_g);
00286   builder->get_widget("scl_right_foot_b", scl_right_foot_b);
00287 
00288   builder->get_widget("tb_left_ear_0",    tb_left_ear_0);
00289   builder->get_widget("tb_left_ear_36",   tb_left_ear_36);
00290   builder->get_widget("tb_left_ear_72",   tb_left_ear_72);
00291   builder->get_widget("tb_left_ear_108",  tb_left_ear_108);
00292   builder->get_widget("tb_left_ear_144",  tb_left_ear_144);
00293   builder->get_widget("tb_left_ear_180",  tb_left_ear_180);
00294   builder->get_widget("tb_left_ear_216",  tb_left_ear_216);
00295   builder->get_widget("tb_left_ear_252",  tb_left_ear_252);
00296   builder->get_widget("tb_left_ear_288",  tb_left_ear_288);
00297   builder->get_widget("tb_left_ear_324",  tb_left_ear_324);
00298 
00299   builder->get_widget("tb_right_ear_0",   tb_right_ear_0);
00300   builder->get_widget("tb_right_ear_36",  tb_right_ear_36);
00301   builder->get_widget("tb_right_ear_72",  tb_right_ear_72);
00302   builder->get_widget("tb_right_ear_108", tb_right_ear_108);
00303   builder->get_widget("tb_right_ear_144", tb_right_ear_144);
00304   builder->get_widget("tb_right_ear_180", tb_right_ear_180);
00305   builder->get_widget("tb_right_ear_216", tb_right_ear_216);
00306   builder->get_widget("tb_right_ear_252", tb_right_ear_252);
00307   builder->get_widget("tb_right_ear_288", tb_right_ear_288);
00308   builder->get_widget("tb_right_ear_324", tb_right_ear_324);
00309 
00310   builder->get_widget("tb_control_leds", tb_control_leds);
00311 
00312   // required for LedInterface data callback
00313   led_scales["ChestBoard/Red"]   = scl_chest_r;
00314   led_scales["ChestBoard/Green"] = scl_chest_g;
00315   led_scales["ChestBoard/Blue"]  = scl_chest_b;
00316   led_scales["Face/Red/Left"]    = scl_left_eye_r;
00317   led_scales["Face/Green/Left"]  = scl_left_eye_g;
00318   led_scales["Face/Blue/Left"]   = scl_left_eye_b;
00319   led_scales["Face/Red/Right"]   = scl_right_eye_r;
00320   led_scales["Face/Green/Right"] = scl_right_eye_g;
00321   led_scales["Face/Blue/Right"]  = scl_right_eye_b;
00322   led_scales["LFoot/Red"]        = scl_left_foot_r;
00323   led_scales["LFoot/Green"]      = scl_left_foot_g;
00324   led_scales["LFoot/Blue"]       = scl_left_foot_b;
00325   led_scales["RFoot/Red"]        = scl_right_foot_r;
00326   led_scales["RFoot/Green"]      = scl_right_foot_g;
00327   led_scales["RFoot/Blue"]       = scl_right_foot_b;
00328 
00329   led_buttons["Ears/Left/0Deg"]   = tb_left_ear_0;
00330   led_buttons["Ears/Left/36Deg"]  = tb_left_ear_36;
00331   led_buttons["Ears/Left/72Deg"]  = tb_left_ear_72;
00332   led_buttons["Ears/Left/108Deg"] = tb_left_ear_108;
00333   led_buttons["Ears/Left/144Deg"] = tb_left_ear_144;
00334   led_buttons["Ears/Left/180Deg"] = tb_left_ear_180;
00335   led_buttons["Ears/Left/216Deg"] = tb_left_ear_216;
00336   led_buttons["Ears/Left/252Deg"] = tb_left_ear_252;
00337   led_buttons["Ears/Left/288Deg"] = tb_left_ear_288;
00338   led_buttons["Ears/Left/324Deg"] = tb_left_ear_324;
00339 
00340   led_buttons["Ears/Right/0Deg"]   = tb_right_ear_0;
00341   led_buttons["Ears/Right/36Deg"]  = tb_right_ear_36;
00342   led_buttons["Ears/Right/72Deg"]  = tb_right_ear_72;
00343   led_buttons["Ears/Right/108Deg"] = tb_right_ear_108;
00344   led_buttons["Ears/Right/144Deg"] = tb_right_ear_144;
00345   led_buttons["Ears/Right/180Deg"] = tb_right_ear_180;
00346   led_buttons["Ears/Right/216Deg"] = tb_right_ear_216;
00347   led_buttons["Ears/Right/252Deg"] = tb_right_ear_252;
00348   led_buttons["Ears/Right/288Deg"] = tb_right_ear_288;
00349   led_buttons["Ears/Right/324Deg"] = tb_right_ear_324;
00350 
00351 
00352   builder->get_widget("but_chestbut",     but_chestbut);
00353   builder->get_widget("but_head_front",   but_head_front);
00354   builder->get_widget("but_head_middle",  but_head_middle);
00355   builder->get_widget("but_head_rear",    but_head_rear);
00356   builder->get_widget("but_lfoot_bumper", but_lfoot_bumper);
00357   builder->get_widget("but_rfoot_bumper", but_rfoot_bumper);
00358 
00359   std::map<std::string, std::string> widget_if_map;
00360   widget_if_map["Chest"]       = "chestbut";
00361   widget_if_map["Foot Left"]   = "lfoot_bumper";
00362   widget_if_map["Foot Right"]  = "rfoot_bumper";
00363   widget_if_map["Head Front"]  = "head_front";
00364   widget_if_map["Head Middle"] = "head_middle";
00365   widget_if_map["Head Rear"]   = "head_rear";
00366 
00367   std::map<std::string, std::string>::iterator m;
00368   for (m = widget_if_map.begin(); m != widget_if_map.end(); ++m) {
00369     ButtonLabelSet labels;
00370 
00371     builder->get_widget("lab_enabled_" + m->second, labels.lab_enabled);
00372     builder->get_widget("lab_history_" + m->second, labels.lab_history);
00373     builder->get_widget("lab_value_" + m->second, labels.lab_value);
00374     builder->get_widget("lab_short_" + m->second, labels.lab_short);
00375     builder->get_widget("lab_long_" + m->second, labels.lab_long);
00376     builder->get_widget("lab_total_" + m->second, labels.lab_total);
00377 
00378     button_labels[m->first] = labels;
00379   }
00380 
00381   cmb_kick_leg->set_active(0);
00382   cmb_standup_from->set_active(0);
00383   cmb_us_direction->set_active(0);
00384   frm_servos->set_sensitive(false);
00385   frm_sensors->set_sensitive(false);
00386   frm_ultrasonic->set_sensitive(false);
00387 
00388   hsc_HeadYaw->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_HeadYaw, lab_HeadYaw, NaoJointPositionInterface::SERVO_head_yaw));
00389   hsc_HeadPitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_HeadPitch, lab_HeadPitch, NaoJointPositionInterface::SERVO_head_pitch));
00390   hsc_RShoulderPitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RShoulderPitch, lab_RShoulderPitch, NaoJointPositionInterface::SERVO_r_shoulder_pitch));
00391   hsc_RShoulderRoll->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RShoulderRoll, lab_RShoulderRoll, NaoJointPositionInterface::SERVO_r_shoulder_roll));
00392   hsc_RElbowYaw->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RElbowYaw, lab_RElbowYaw, NaoJointPositionInterface::SERVO_r_elbow_yaw));
00393   hsc_RElbowRoll->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RElbowRoll, lab_RElbowRoll, NaoJointPositionInterface::SERVO_r_elbow_roll));
00394   hsc_RWristYaw->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RWristYaw, lab_RWristYaw, NaoJointPositionInterface::SERVO_r_wrist_yaw));
00395   hsc_RHand->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RHand, lab_RHand, NaoJointPositionInterface::SERVO_r_hand));
00396   hsc_LShoulderPitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LShoulderPitch, lab_LShoulderPitch, NaoJointPositionInterface::SERVO_l_shoulder_pitch));
00397   hsc_LShoulderRoll->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LShoulderRoll, lab_LShoulderRoll, NaoJointPositionInterface::SERVO_l_shoulder_roll));
00398   hsc_LElbowYaw->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LElbowYaw, lab_LElbowYaw, NaoJointPositionInterface::SERVO_l_elbow_yaw));
00399   hsc_LElbowRoll->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LElbowRoll, lab_LElbowRoll, NaoJointPositionInterface::SERVO_l_elbow_roll));
00400   hsc_LWristYaw->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LWristYaw, lab_LWristYaw, NaoJointPositionInterface::SERVO_r_wrist_yaw));
00401   hsc_LHand->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LHand, lab_LHand, NaoJointPositionInterface::SERVO_r_hand));
00402   hsc_RHipYawPitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RHipYawPitch, lab_RHipYawPitch, NaoJointPositionInterface::SERVO_r_hip_yaw_pitch));
00403   hsc_RHipPitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RHipPitch, lab_RHipPitch, NaoJointPositionInterface::SERVO_r_hip_pitch));
00404   hsc_RHipRoll->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RHipRoll, lab_RHipRoll, NaoJointPositionInterface::SERVO_r_hip_roll));
00405   hsc_RKneePitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RKneePitch, lab_RKneePitch, NaoJointPositionInterface::SERVO_r_knee_pitch));
00406   hsc_RAnklePitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RAnklePitch, lab_RAnklePitch, NaoJointPositionInterface::SERVO_r_ankle_pitch));
00407   hsc_RAnkleRoll->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RAnkleRoll, lab_RAnkleRoll, NaoJointPositionInterface::SERVO_r_ankle_roll));
00408   hsc_LHipYawPitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LHipYawPitch, lab_LHipYawPitch, NaoJointPositionInterface::SERVO_l_hip_yaw_pitch));
00409   hsc_LHipPitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LHipPitch, lab_LHipPitch, NaoJointPositionInterface::SERVO_l_hip_pitch));
00410   hsc_LHipRoll->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LHipRoll, lab_LHipRoll, NaoJointPositionInterface::SERVO_l_hip_roll));
00411   hsc_LKneePitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LKneePitch, lab_LKneePitch, NaoJointPositionInterface::SERVO_l_knee_pitch));
00412   hsc_LAnklePitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LAnklePitch, lab_LAnklePitch, NaoJointPositionInterface::SERVO_l_ankle_pitch));
00413   hsc_LAnkleRoll->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LAnkleRoll, lab_LAnkleRoll, NaoJointPositionInterface::SERVO_l_ankle_roll));
00414   hsc_speed->signal_value_changed().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_changed_speed));
00415   tb_connection->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_connection_clicked));
00416   tb_stiffness->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_stiffness_clicked));
00417   tb_control->signal_toggled().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_control_toggled));
00418   tb_parkpos->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_goto_parkpos_clicked));
00419   tb_zeroall->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_goto_zero_all_clicked));
00420   tb_getup->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_get_up_clicked));
00421   tb_exit->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_exit_clicked));
00422   but_sv_copy->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_sv_copy_clicked));
00423   but_us_auto->signal_toggled().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_us_auto_toggled));
00424   but_us_emit->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_us_emit_clicked));
00425   but_walkvel_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_walkvel_exec_clicked));
00426   but_ws_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_ws_exec_clicked));
00427   but_stop->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_motion_stop_clicked));
00428   but_wsw_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_wsw_exec_clicked));
00429   but_wa_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_wa_exec_clicked));
00430   but_kick_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_kick_exec_clicked));
00431   but_standup_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_standup_exec_clicked));
00432   but_turn_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_turn_exec_clicked));
00433   but_nav_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_nav_exec_clicked));
00434   but_stiffness_read->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_stiffness_read_clicked));
00435   but_stiffness_write->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_stiffness_write_clicked));
00436   chb_stiffness_global->signal_toggled().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_stiffness_global_toggled));
00437 
00438   but_tts_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_tts_exec_clicked));
00439 
00440   tb_left_ear_0->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/0Deg", tb_left_ear_0));
00441   tb_left_ear_36->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/36Deg", tb_left_ear_36));
00442   tb_left_ear_72->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/72Deg", tb_left_ear_72));
00443   tb_left_ear_108->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/108Deg", tb_left_ear_108));
00444   tb_left_ear_144->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/144Deg", tb_left_ear_144));
00445   tb_left_ear_180->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/180Deg", tb_left_ear_180));
00446   tb_left_ear_216->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/216Deg", tb_left_ear_216));
00447   tb_left_ear_252->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/252Deg", tb_left_ear_252));
00448   tb_left_ear_288->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/288Deg", tb_left_ear_288));
00449   tb_left_ear_324->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Left/324Deg", tb_left_ear_324));
00450 
00451   tb_right_ear_0->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/0Deg", tb_right_ear_0));
00452   tb_right_ear_36->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/36Deg", tb_right_ear_36));
00453   tb_right_ear_72->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/72Deg", tb_right_ear_72));
00454   tb_right_ear_108->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/108Deg", tb_right_ear_108));
00455   tb_right_ear_144->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/144Deg", tb_right_ear_144));
00456   tb_right_ear_180->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/180Deg", tb_right_ear_180));
00457   tb_right_ear_216->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/216Deg", tb_right_ear_216));
00458   tb_right_ear_252->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/252Deg", tb_right_ear_252));
00459   tb_right_ear_288->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/288Deg", tb_right_ear_288));
00460   tb_right_ear_324->signal_toggled().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_tb_toggled), "Ears/Right/324Deg", tb_right_ear_324));
00461 
00462   scl_chest_r->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "ChestBoard/Red", scl_chest_r));
00463   scl_chest_g->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "ChestBoard/Green", scl_chest_g));
00464   scl_chest_b->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "ChestBoard/Blue", scl_chest_b));
00465   scl_left_eye_r->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "Face/Red/Left", scl_left_eye_r));
00466   scl_left_eye_g->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "Face/Green/Left", scl_left_eye_g));
00467   scl_left_eye_b->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "Face/Blue/Left", scl_left_eye_b));
00468   scl_right_eye_r->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "Face/Red/Right", scl_right_eye_r));
00469   scl_right_eye_g->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "Face/Green/Right", scl_right_eye_g));
00470   scl_right_eye_b->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "Face/Blue/Right", scl_right_eye_b));
00471   scl_left_foot_r->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "LFoot/Red", scl_left_foot_r));
00472   scl_left_foot_g->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "LFoot/Green", scl_left_foot_g));
00473   scl_left_foot_b->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "LFoot/Blue", scl_left_foot_b));
00474   scl_right_foot_r->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "RFoot/Red", scl_right_foot_r));
00475   scl_right_foot_g->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "RFoot/Green", scl_right_foot_g));
00476   scl_right_foot_b->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "RFoot/Blue", scl_right_foot_b));
00477 
00478   scl_chest_r->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "ChestBoard/Red", scl_chest_r));
00479   scl_chest_g->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "ChestBoard/Green", scl_chest_g));
00480   scl_chest_b->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "ChestBoard/Blue", scl_chest_b));
00481   scl_left_eye_r->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "Face/Red/Left", scl_left_eye_r));
00482   scl_left_eye_g->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "Face/Green/Left", scl_left_eye_g));
00483   scl_left_eye_b->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "Face/Blue/Left", scl_left_eye_b));
00484   scl_right_eye_r->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "Face/Red/Right", scl_right_eye_r));
00485   scl_right_eye_g->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "Face/Green/Right", scl_right_eye_g));
00486   scl_right_eye_b->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "Face/Blue/Right", scl_right_eye_b));
00487   scl_left_foot_r->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "LFoot/Red", scl_left_foot_r));
00488   scl_left_foot_g->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "LFoot/Green", scl_left_foot_g));
00489   scl_left_foot_b->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "LFoot/Blue", scl_left_foot_b));
00490   scl_right_foot_r->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "RFoot/Red", scl_right_foot_r));
00491   scl_right_foot_g->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "RFoot/Green", scl_right_foot_g));
00492   scl_right_foot_b->signal_button_release_event().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_button_release), "RFoot/Blue", scl_right_foot_b));
00493 
00494   but_chestbut->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Chest"));
00495   but_chestbut->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Chest"));
00496   but_head_front->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Head Front"));
00497   but_head_front->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Head Front"));
00498   but_head_middle->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Head Middle"));
00499   but_head_middle->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Head Middle"));
00500   but_head_rear->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Head Rear"));
00501   but_head_rear->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Head Rear"));
00502   but_lfoot_bumper->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Foot Left"));
00503   but_lfoot_bumper->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Foot Left"));
00504   but_rfoot_bumper->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Foot Right"));
00505   but_rfoot_bumper->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Foot Right"));
00506 
00507   connection_dispatcher.signal_connected().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_connect));
00508   connection_dispatcher.signal_disconnected().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_disconnect));
00509 
00510   tb_control_leds->signal_toggled().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_control_leds_toggled));
00511 
00512   on_control_toggled();
00513   on_changed_speed();
00514   init();
00515 }
00516 
00517 
00518 /** Destructor. */
00519 NaoGuiGtkWindow::~NaoGuiGtkWindow()
00520 {
00521   on_disconnect();
00522 }
00523 
00524 /**
00525  * Sets the default values (locale dependent)
00526  */
00527 void
00528 NaoGuiGtkWindow::init()
00529 {
00530   ent_walkvel_x->set_text(convert_float2str(0.5f, 1));
00531   ent_walkvel_y->set_text(convert_float2str(0.f, 1));
00532   ent_walkvel_theta->set_text(convert_float2str(0.f, 1));
00533   ent_walkvel_speed->set_text(convert_float2str(0.5f, 1));
00534 
00535   ent_ws_distance->set_text(convert_float2str(0.2f, 1));
00536 
00537   ent_wsw_distance->set_text(convert_float2str(0.2f, 1));
00538 
00539   ent_wa_angle->set_text(convert_float2str(1.f, 1));
00540   ent_wa_radius->set_text(convert_float2str(0.3f, 1));
00541 
00542   ent_turn_angle->set_text(convert_float2str(1.f, 1));
00543 
00544   ent_nav_x->set_text(convert_float2str(0.f, 1));
00545   ent_nav_y->set_text(convert_float2str(0.f, 1));
00546   ent_nav_ori->set_text(convert_float2str(0.f, 1));
00547 }
00548 
00549 /** Update a servo value.
00550  * @param hsc horizontal scale for value
00551  * @param label label for value
00552  * @param value servo value
00553  */
00554 void
00555 NaoGuiGtkWindow::update_servo_value(Gtk::HScale *hsc, Gtk::Label *label, float value)
00556 {
00557   float f = roundf(value * 100.);
00558   if ( ! tb_control->get_active() && (hsc->get_value() != f)) {
00559     hsc->set_value(f);
00560   }
00561   // this might seem more expensive than just setting it, but the really
00562   // expensive thing are screen updates, and they can be avoid much of the
00563   // time
00564   Glib::ustring fs = convert_float2str(value);
00565   if (label->get_text() != fs) {
00566     label->set_text(fs);
00567   }
00568 }
00569 
00570 
00571 /** Update sensor value.
00572  * @param label label for value
00573  * @param value sensor value
00574  * @param show_decimal true to show as decimal, false to show as integer
00575  */
00576 void
00577 NaoGuiGtkWindow::update_sensor_value(Gtk::Label *label, float value, bool show_decimal)
00578 {
00579   Glib::ustring s = convert_float2str(value, show_decimal ? 2 : 0);
00580   if (label->get_text() != s) {
00581     label->set_text(s);
00582   }
00583 }
00584 
00585 
00586 /** Update an entry value.
00587  * @param ent entry field for the value
00588  * @param value the value to set
00589  * @param width the number of positions after decimal point
00590  */
00591 void
00592 NaoGuiGtkWindow::update_entry_value(Gtk::Entry *ent, float value, unsigned int width)
00593 {
00594   if ( value != -123456 ) {
00595     ent->set_text(convert_float2str(value, width));
00596   } else {
00597     ent->set_text("");
00598   }
00599 }
00600 
00601 
00602 /** Update ultrasonic direction field.
00603  * @param direction direction value from interface field
00604  */
00605 void
00606 NaoGuiGtkWindow::update_ultrasonic_direction()
00607 {
00608   if ( ! sensor_if || ! sensor_if->is_valid()) return;
00609 
00610   switch (sensor_if->ultrasonic_direction()) {
00611   case NaoSensorInterface::USD_LEFT_LEFT:
00612     lab_ultrasonic_direction->set_text("l-l"); break;
00613   case NaoSensorInterface::USD_LEFT_RIGHT:
00614     lab_ultrasonic_direction->set_text("l-r"); break;
00615   case NaoSensorInterface::USD_RIGHT_RIGHT:
00616     lab_ultrasonic_direction->set_text("r-r"); break;
00617   case NaoSensorInterface::USD_RIGHT_LEFT:
00618     lab_ultrasonic_direction->set_text("r-l"); break;
00619   case NaoSensorInterface::USD_BOTH_BOTH:
00620     lab_ultrasonic_direction->set_text("b-b"); break;
00621   default:
00622     lab_ultrasonic_direction->set_text("none"); break;
00623   }
00624 }
00625 
00626 
00627 /** Send servo message.
00628  * @param hsc scale from which to take the value from
00629  * @param servo the servo constant for the servo to set from the
00630  * NaoJointPositionInterface
00631  */
00632 void
00633 NaoGuiGtkWindow::send_servo_msg(Gtk::HScale *hsc, unsigned int servo)
00634 {
00635   if ( jointpos_if && tb_control->get_active() ) {
00636     jointpos_if->read();
00637 
00638     if (servo == NaoJointPositionInterface::SERVO_head_pitch ||
00639         servo == NaoJointPositionInterface::SERVO_head_yaw)
00640     {
00641       HumanoidMotionInterface::MoveHeadMessage *m = new
00642         HumanoidMotionInterface::MoveHeadMessage(hsc_HeadYaw->get_value() / 100.f,
00643                                                  hsc_HeadPitch->get_value() /100.f,
00644                                                  hsc_speed->get_value() / 100.f);
00645 
00646       if ( rad_motion_fawkes->get_active() ) {
00647         hummot_fawkes_if->msgq_enqueue(m);
00648       } else {
00649         hummot_naoqi_if->msgq_enqueue(m);
00650       }
00651     }
00652     else {
00653       NaoJointPositionInterface::MoveServoMessage *m = new
00654         NaoJointPositionInterface::MoveServoMessage(servo,
00655                                                     hsc->get_value() / 100.f,
00656                                                     hsc_speed->get_value() /100.f);
00657       jointpos_if->msgq_enqueue(m);
00658     }
00659   }
00660 }
00661 
00662 
00663 /** Event handler for slider changes.
00664  * @param hsc horizontal slider that caused the event
00665  * @param label label to set with the value
00666  * @param servo the servo constant for the servo to set from the
00667  * NaoJointPositionInterface
00668  */
00669 void
00670 NaoGuiGtkWindow::on_slider_changed(Gtk::HScale *hsc, Gtk::Label *label,
00671                                    unsigned int servo)
00672 {
00673   send_servo_msg(hsc, servo);
00674 }
00675 
00676 
00677 /** Speed change event handler. */
00678 void
00679 NaoGuiGtkWindow::on_changed_speed()
00680 {
00681   Glib::ustring s = convert_float2str(hsc_speed->get_value() / 100.f, 2);
00682   lab_speed->set_text(s);
00683 }
00684 
00685 
00686 /** Update joint position values.
00687  * Called whenever the NaoJointPositionInterface changes.
00688  * @param force true to force an update, false to update depending
00689  * on internal state
00690  */
00691 void
00692 NaoGuiGtkWindow::update_jointpos_values(bool force)
00693 {
00694   if ( ! jointpos_if ) return;
00695 
00696   /*
00697   // we take only each 10th update to decrease ffnaogui CPU usage
00698   if (! force && (++update_cycle % 10 != 0)) {
00699     return;
00700   }
00701   update_cycle = 0;
00702   */
00703 
00704   try {
00705     jointpos_if->read();
00706     update_servo_value(hsc_HeadYaw, lab_HeadYaw, jointpos_if->head_yaw());
00707     update_servo_value(hsc_HeadPitch, lab_HeadPitch, jointpos_if->head_pitch());
00708     update_servo_value(hsc_RShoulderPitch, lab_RShoulderPitch, jointpos_if->r_shoulder_pitch());
00709     update_servo_value(hsc_RShoulderRoll, lab_RShoulderRoll, jointpos_if->r_shoulder_roll());
00710     update_servo_value(hsc_RElbowYaw, lab_RElbowYaw, jointpos_if->r_elbow_yaw());
00711     update_servo_value(hsc_RElbowRoll, lab_RElbowRoll, jointpos_if->r_elbow_roll());
00712     update_servo_value(hsc_RWristYaw, lab_RWristYaw, jointpos_if->r_wrist_yaw());
00713     update_servo_value(hsc_RHand, lab_RHand, jointpos_if->r_hand());
00714     update_servo_value(hsc_LShoulderPitch, lab_LShoulderPitch, jointpos_if->l_shoulder_pitch());
00715     update_servo_value(hsc_LShoulderRoll, lab_LShoulderRoll, jointpos_if->l_shoulder_roll());
00716     update_servo_value(hsc_LElbowYaw, lab_LElbowYaw, jointpos_if->l_elbow_yaw());
00717     update_servo_value(hsc_LElbowRoll, lab_LElbowRoll, jointpos_if->l_elbow_roll());
00718     update_servo_value(hsc_LWristYaw, lab_LWristYaw, jointpos_if->l_wrist_yaw());
00719     update_servo_value(hsc_LHand, lab_LHand, jointpos_if->l_hand());
00720     update_servo_value(hsc_RHipYawPitch, lab_RHipYawPitch, jointpos_if->r_hip_yaw_pitch());
00721     update_servo_value(hsc_RHipPitch, lab_RHipPitch, jointpos_if->r_hip_pitch());
00722     update_servo_value(hsc_RHipRoll, lab_RHipRoll, jointpos_if->r_hip_roll());
00723     update_servo_value(hsc_RKneePitch, lab_RKneePitch, jointpos_if->r_knee_pitch());
00724     update_servo_value(hsc_RAnklePitch, lab_RAnklePitch, jointpos_if->r_ankle_pitch());
00725     update_servo_value(hsc_RAnkleRoll, lab_RAnkleRoll, jointpos_if->r_ankle_roll());
00726     update_servo_value(hsc_LHipYawPitch, lab_LHipYawPitch, jointpos_if->l_hip_yaw_pitch());
00727     update_servo_value(hsc_LHipPitch, lab_LHipPitch, jointpos_if->l_hip_pitch());
00728     update_servo_value(hsc_LHipRoll, lab_LHipRoll, jointpos_if->l_hip_roll());
00729     update_servo_value(hsc_LKneePitch, lab_LKneePitch, jointpos_if->l_knee_pitch());
00730     update_servo_value(hsc_LAnklePitch, lab_LAnklePitch, jointpos_if->l_ankle_pitch());
00731     update_servo_value(hsc_LAnkleRoll, lab_LAnkleRoll, jointpos_if->l_ankle_roll());
00732 
00733     bool currently_servo_enabled = servos_enabled();
00734 
00735     if ( currently_servo_enabled && ! servo_enabled ) {
00736       tb_stiffness->set_stock_id(Gtk::Stock::YES);
00737       tb_control->set_sensitive(true);
00738       tb_getup->set_sensitive(true);
00739       tb_parkpos->set_sensitive(true);
00740       tb_zeroall->set_sensitive(true);
00741     } else if (! currently_servo_enabled && servo_enabled ) {
00742       tb_stiffness->set_stock_id(Gtk::Stock::NO);
00743       tb_control->set_sensitive(false);
00744       tb_getup->set_sensitive(false);
00745       tb_parkpos->set_sensitive(false);
00746       tb_zeroall->set_sensitive(false);
00747     }
00748     servo_enabled = currently_servo_enabled;
00749   } catch (Exception &e) {
00750     // ignored, happens on disconnect while events are pending
00751   }
00752 }
00753 
00754 
00755 void
00756 NaoGuiGtkWindow::update_tts()
00757 {
00758   if ( ! speechsynth_if || ! speechsynth_if->is_valid()) return;
00759 
00760   speechsynth_if->read();
00761   if ((speechsynth_if->is_final() || (speechsynth_if->msgid() == 0)) &&
00762        (lab_tts_active->get_text() != "No"))
00763   {
00764     lab_tts_active->set_text("No");
00765 
00766   } else if (! speechsynth_if->is_final() &&
00767              (speechsynth_if->msgid() != 0) &&
00768              (lab_tts_active->get_text() != "Yes"))
00769   {
00770     lab_tts_active->set_text("Yes");
00771   }
00772 }
00773 
00774 
00775 /** Update sensor values.
00776  * Called whenever the NaoSensorInterface changes.
00777  * @param force true to force an update, false to update depending
00778  * on internal state
00779  */
00780 void
00781 NaoGuiGtkWindow::update_sensor_values(bool force)
00782 {
00783   if ( ! sensor_if ) return;
00784 
00785   try {
00786     sensor_if->read();
00787 
00788     update_sensor_value(lab_l_fsr_fl, sensor_if->l_fsr_fl());
00789     update_sensor_value(lab_l_fsr_fr, sensor_if->l_fsr_fr());
00790     update_sensor_value(lab_l_fsr_rl, sensor_if->l_fsr_rl());
00791     update_sensor_value(lab_l_fsr_rr, sensor_if->l_fsr_rr());
00792     update_sensor_value(lab_r_fsr_fl, sensor_if->r_fsr_fl());
00793     update_sensor_value(lab_r_fsr_fr, sensor_if->r_fsr_fr());
00794     update_sensor_value(lab_r_fsr_rl, sensor_if->r_fsr_rl());
00795     update_sensor_value(lab_r_fsr_rr, sensor_if->r_fsr_rr());
00796     update_sensor_value(lab_r_total_weight, sensor_if->r_total_weight());
00797     update_sensor_value(lab_l_total_weight, sensor_if->l_total_weight());
00798 
00799     update_sensor_value(lab_chest_button, sensor_if->chest_button(), false);
00800     update_sensor_value(lab_touch_front, sensor_if->head_touch_front(), false);
00801     update_sensor_value(lab_touch_middle, sensor_if->head_touch_middle(), false);
00802     update_sensor_value(lab_touch_rear, sensor_if->head_touch_rear(), false);
00803     update_sensor_value(lab_l_bumper_l, sensor_if->l_foot_bumper_l(), false);
00804     update_sensor_value(lab_l_bumper_r, sensor_if->l_foot_bumper_r(), false);
00805     update_sensor_value(lab_r_bumper_l, sensor_if->r_foot_bumper_l(), false);
00806     update_sensor_value(lab_r_bumper_r, sensor_if->r_foot_bumper_r(), false);
00807 
00808     update_sensor_value(lab_accel_x, sensor_if->accel_x());
00809     update_sensor_value(lab_accel_y, sensor_if->accel_y());
00810     update_sensor_value(lab_accel_z, sensor_if->accel_z());
00811     update_sensor_value(lab_gyro_x, sensor_if->gyro_x());
00812     update_sensor_value(lab_gyro_y, sensor_if->gyro_y());
00813     update_sensor_value(lab_gyro_ref, sensor_if->gyro_ref());
00814 
00815     update_sensor_value(lab_battery_charge, sensor_if->battery_charge());
00816 
00817     Glib::ustring l_cop =
00818       "(" + convert_float2str(sensor_if->l_cop_x(), 1) + ", " + 
00819       convert_float2str(sensor_if->l_cop_y(), 1) + ")";
00820     lab_l_cop->set_text(l_cop);
00821 
00822     Glib::ustring r_cop =
00823       "(" + convert_float2str(sensor_if->r_cop_x(), 1) + ", " + 
00824       convert_float2str(sensor_if->r_cop_y(), 1) + ")";
00825     lab_r_cop->set_text(r_cop);
00826 
00827     Glib::ustring angles_xy =
00828       convert_float2str(sensor_if->angle_x(), 2) + "/" + 
00829       convert_float2str(sensor_if->angle_y(), 2);
00830     lab_angles_xy->set_text(angles_xy);
00831 
00832     update_ultrasonic_direction();
00833     update_sensor_value(lab_ultrasonic_left0, sensor_if->ultrasonic_distance_left(0));
00834     update_sensor_value(lab_ultrasonic_left1, sensor_if->ultrasonic_distance_left(1));
00835     update_sensor_value(lab_ultrasonic_left2, sensor_if->ultrasonic_distance_left(2));
00836     update_sensor_value(lab_ultrasonic_left3, sensor_if->ultrasonic_distance_left(3));
00837     update_sensor_value(lab_ultrasonic_right0, sensor_if->ultrasonic_distance_right(0));
00838     update_sensor_value(lab_ultrasonic_right1, sensor_if->ultrasonic_distance_right(1));
00839     update_sensor_value(lab_ultrasonic_right2, sensor_if->ultrasonic_distance_right(2));
00840     update_sensor_value(lab_ultrasonic_right3, sensor_if->ultrasonic_distance_right(3));
00841 
00842   } catch (Exception &e) {
00843     // ignored, happens on disconnect while events are pending
00844   }
00845 }
00846 
00847 bool
00848 NaoGuiGtkWindow::servos_enabled() const
00849 {
00850   jointstiff_if->read();
00851   return (jointstiff_if->minimum() > 0.);
00852 }
00853 
00854 
00855 void
00856 NaoGuiGtkWindow::on_stiffness_clicked()
00857 {
00858   const float stiffness = servos_enabled() ? 0. : 1.;
00859   NaoJointStiffnessInterface::SetBodyStiffnessMessage *dmsg =
00860     new NaoJointStiffnessInterface::SetBodyStiffnessMessage(stiffness, 0.5);
00861   jointstiff_if->msgq_enqueue(dmsg);
00862 }
00863 
00864 
00865 /** Control toggle event handler. */
00866 void
00867 NaoGuiGtkWindow::on_control_toggled()
00868 {
00869   bool sensitive = tb_control->get_active();
00870   hsc_HeadYaw->set_sensitive(sensitive);
00871   hsc_HeadPitch->set_sensitive(sensitive);
00872   hsc_RShoulderPitch->set_sensitive(sensitive);
00873   hsc_RShoulderRoll->set_sensitive(sensitive);
00874   hsc_RElbowYaw->set_sensitive(sensitive);
00875   hsc_RElbowRoll->set_sensitive(sensitive);
00876   hsc_RWristYaw->set_sensitive(sensitive);
00877   hsc_RHand->set_sensitive(sensitive);
00878   hsc_LShoulderPitch->set_sensitive(sensitive);
00879   hsc_LShoulderRoll->set_sensitive(sensitive);
00880   hsc_LElbowYaw->set_sensitive(sensitive);
00881   hsc_LElbowRoll->set_sensitive(sensitive);
00882   hsc_LWristYaw->set_sensitive(sensitive);
00883   hsc_LHand->set_sensitive(sensitive);
00884   hsc_RHipYawPitch->set_sensitive(sensitive);
00885   hsc_RHipPitch->set_sensitive(sensitive);
00886   hsc_RHipRoll->set_sensitive(sensitive);
00887   hsc_RKneePitch->set_sensitive(sensitive);
00888   hsc_RAnklePitch->set_sensitive(sensitive);
00889   hsc_RAnkleRoll->set_sensitive(sensitive);
00890   hsc_LHipYawPitch->set_sensitive(sensitive);
00891   hsc_LHipPitch->set_sensitive(sensitive);
00892   hsc_LHipRoll->set_sensitive(sensitive);
00893   hsc_LKneePitch->set_sensitive(sensitive);
00894   hsc_LAnklePitch->set_sensitive(sensitive);
00895   hsc_LAnkleRoll->set_sensitive(sensitive);
00896 
00897   if ( ! sensitive ) {
00898     update_jointpos_values();
00899   }
00900 }
00901 
00902 
00903 void
00904 NaoGuiGtkWindow::on_sv_copy_clicked()
00905 {
00906   std::stringstream txt; 
00907   txt <<
00908     "head_yaw = " << (hsc_HeadYaw->get_value() / 100.f) << ",\n" <<
00909     "head_pitch = " << (hsc_HeadPitch->get_value() / 100.f) << ",\n" <<
00910     "l_shoulder_pitch = " << (hsc_LShoulderPitch->get_value() / 100.f) << ",\n" <<
00911     "l_shoulder_roll = " << (hsc_LShoulderRoll->get_value() / 100.f) << ",\n" <<
00912     "l_elbow_yaw = " << (hsc_LElbowYaw->get_value() / 100.f) << ",\n" <<
00913     "l_elbow_roll = " << (hsc_LElbowRoll->get_value() / 100.f) << ",\n" <<
00914     "l_wrist_yaw = " << (hsc_LWristYaw->get_value() / 100.f) << ",\n" <<
00915     "l_hand = " << (hsc_LHand->get_value() / 100.f) << ",\n" <<
00916     "r_shoulder_pitch = " << (hsc_RShoulderPitch->get_value() / 100.f) << ",\n" <<
00917     "r_shoulder_roll = " << (hsc_RShoulderRoll->get_value() / 100.f) << ",\n" <<
00918     "r_elbow_yaw = " << (hsc_RElbowYaw->get_value() / 100.f) << ",\n" <<
00919     "r_elbow_roll = " << (hsc_RElbowRoll->get_value() / 100.f) << ",\n" <<
00920     "r_wrist_yaw = " << (hsc_RWristYaw->get_value() / 100.f) << ",\n" <<
00921     "r_hand = " << (hsc_RHand->get_value() / 100.f) << ",\n" <<
00922     "l_hip_yaw_pitch = " << (hsc_LHipYawPitch->get_value() / 100.f) << ",\n" <<
00923     "l_hip_roll = " << (hsc_LHipRoll->get_value() / 100.f) << ",\n" <<
00924     "l_hip_pitch = " << (hsc_LHipPitch->get_value() / 100.f) << ",\n" <<
00925     "l_knee_pitch = " << (hsc_LKneePitch->get_value() / 100.f) << ",\n" <<
00926     "l_ankle_pitch = " << (hsc_LAnklePitch->get_value() / 100.f) << ",\n" <<
00927     "l_ankle_roll = " << (hsc_LAnkleRoll->get_value() / 100.f) << ",\n" <<
00928     "r_hip_yaw_pitch = " << (hsc_RHipYawPitch->get_value() / 100.f) << ",\n" <<
00929     "r_hip_roll = " << (hsc_RHipRoll->get_value() / 100.f) << ",\n" <<
00930     "r_hip_pitch = " << (hsc_RHipPitch->get_value() / 100.f) << ",\n" <<
00931     "r_knee_pitch = " << (hsc_RKneePitch->get_value() / 100.f) << ",\n" <<
00932     "r_ankle_pitch = " << (hsc_RAnklePitch->get_value() / 100.f) << ",\n" <<
00933     "r_ankle_roll = " << (hsc_RAnkleRoll->get_value() / 100.f);
00934   Gtk::Clipboard::get()->set_text(txt.str());
00935 }
00936 
00937 
00938 static NaoSensorInterface::UltrasonicDirection
00939 string_to_usd(std::string &value)
00940 {
00941   if (value == "l-l") {
00942     return NaoSensorInterface::USD_LEFT_LEFT;
00943   } else if (value == "l-r") {
00944     return NaoSensorInterface::USD_LEFT_RIGHT;
00945   } else if (value == "r-l") {
00946     return NaoSensorInterface::USD_RIGHT_LEFT;
00947   } else if (value == "r-r") {
00948     return NaoSensorInterface::USD_RIGHT_RIGHT;
00949   } else if (value == "b-b") {
00950     return NaoSensorInterface::USD_BOTH_BOTH;
00951   }
00952   return NaoSensorInterface::USD_NONE;
00953 }
00954 
00955 void
00956 NaoGuiGtkWindow::on_us_emit_clicked()
00957 {
00958   std::string value;
00959   cmb_us_direction->get_active()->get_value(0, value);
00960   NaoSensorInterface::UltrasonicDirection dirval = string_to_usd(value);
00961 
00962   NaoSensorInterface::EmitUltrasonicWaveMessage *m =
00963     new NaoSensorInterface::EmitUltrasonicWaveMessage(dirval);
00964   sensor_if->msgq_enqueue(m);
00965 }
00966 
00967 
00968 void
00969 NaoGuiGtkWindow::on_us_auto_toggled()
00970 {
00971   if (but_us_auto->get_active()) {
00972     std::string value;
00973     cmb_us_direction->get_active()->get_value(0, value);
00974     NaoSensorInterface::UltrasonicDirection dirval = string_to_usd(value);
00975 
00976     NaoSensorInterface::StartUltrasonicMessage *m =
00977       new NaoSensorInterface::StartUltrasonicMessage(dirval);
00978     sensor_if->msgq_enqueue(m);
00979   } else {
00980     NaoSensorInterface::StopUltrasonicMessage *m =
00981       new NaoSensorInterface::StopUltrasonicMessage();
00982     sensor_if->msgq_enqueue(m);
00983   }
00984 }
00985 
00986 /** Event handler for connection button. */
00987 void
00988 NaoGuiGtkWindow::on_connection_clicked()
00989 {
00990   if ( ! connection_dispatcher.get_client()->connected() ) {
00991     ServiceChooserDialog ssd(*this, connection_dispatcher.get_client());
00992     ssd.run_and_connect();
00993   } else {
00994     connection_dispatcher.get_client()->disconnect();
00995   }
00996 }
00997 
00998 /** Event handler for connected event. */
00999 void
01000 NaoGuiGtkWindow::on_connect()
01001 {
01002   try {
01003     bb = new RemoteBlackBoard(connection_dispatcher.get_client());
01004     jointpos_if =
01005       bb->open_for_reading<NaoJointPositionInterface>("Nao Joint Positions");
01006     jointstiff_if =
01007       bb->open_for_reading<NaoJointStiffnessInterface>("Nao Joint Stiffness");
01008     sensor_if =
01009       bb->open_for_reading<NaoSensorInterface>("Nao Sensors");
01010     nao_navi_if = bb->open_for_reading<NavigatorInterface>("Navigator");
01011     hummot_fawkes_if = bb->open_for_reading<HumanoidMotionInterface>("Nao Motion");
01012     hummot_naoqi_if =
01013       bb->open_for_reading<HumanoidMotionInterface>("NaoQi Motion");
01014     speechsynth_if = bb->open_for_reading<SpeechSynthInterface>("NaoQi TTS");
01015 
01016     std::list<fawkes::Interface *> led_ifs_l =
01017       bb->open_multiple_for_reading("LedInterface", "Nao LED *");
01018 
01019     std::list<fawkes::Interface *>::iterator li = led_ifs_l.begin();
01020     while (li != led_ifs_l.end()) {
01021       std::string id = (*li)->id();
01022       std::string::size_type last_space  = id.rfind(" ");
01023       std::string device = id.substr(last_space + 1);
01024 
01025       LedInterface *led_if = dynamic_cast<LedInterface *>(*li);
01026       if (led_if == NULL) {
01027         bb->close(*li);
01028         li = led_ifs_l.erase(li);
01029         continue;
01030       } else {
01031         ++li;
01032       }
01033 
01034       on_led_data_changed(led_if);
01035       led_ifs[device] = led_if;
01036     }
01037 
01038     std::list<fawkes::Interface *> button_ifs_l =
01039       bb->open_multiple_for_reading("SwitchInterface", "Nao Button *");
01040 
01041     li = button_ifs_l.begin();
01042     while (li != button_ifs_l.end()) {
01043       std::string id = (*li)->id();
01044       std::string::size_type start  = std::string("Nao Button ").size();
01045       std::string device = id.substr(start);
01046 
01047       SwitchInterface *switch_if = dynamic_cast<SwitchInterface *>(*li);
01048       if (switch_if == NULL) {
01049         bb->close(*li);
01050         li = button_ifs_l.erase(li);
01051         continue;
01052       } else {
01053         ++li;
01054       }
01055 
01056       on_button_data_changed(switch_if);
01057       button_ifs[device] = switch_if;
01058     }
01059 
01060     ifd_jointpos = new InterfaceDispatcher("NaoJointPosIfaceDisp", jointpos_if);
01061     ifd_sensor = new InterfaceDispatcher("NaoSensorIfaceDisp", sensor_if);
01062     ifd_tts = new InterfaceDispatcher("NaoTTSIfaceDisp", speechsynth_if);
01063     ifd_leds = new InterfaceDispatcher("NaoLedsIfaceDisp", led_ifs_l);
01064     ifd_buttons = new InterfaceDispatcher("NaoButtonsIfaceDisp", button_ifs_l);
01065     ifd_jointpos->signal_data_changed().connect(sigc::hide(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::update_jointpos_values), false)));
01066     ifd_sensor->signal_data_changed().connect(sigc::hide(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::update_sensor_values), false)));
01067     ifd_tts->signal_data_changed().connect(sigc::hide(sigc::mem_fun(*this, &NaoGuiGtkWindow::update_tts)));
01068     ifd_leds->signal_data_changed().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_data_changed));
01069     ifd_buttons->signal_data_changed().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_data_changed));
01070     bb->register_listener(ifd_jointpos, BlackBoard::BBIL_FLAG_DATA);
01071     bb->register_listener(ifd_sensor, BlackBoard::BBIL_FLAG_DATA);
01072     bb->register_listener(ifd_tts, BlackBoard::BBIL_FLAG_DATA);
01073     bb->register_listener(ifd_leds, BlackBoard::BBIL_FLAG_DATA);
01074     bb->register_listener(ifd_buttons, BlackBoard::BBIL_FLAG_DATA);
01075 
01076     tb_connection->set_stock_id(Gtk::Stock::DISCONNECT);
01077 
01078     frm_servos->set_sensitive(true);
01079     frm_sensors->set_sensitive(true);
01080     frm_ultrasonic->set_sensitive(true);
01081     tb_stiffness->set_sensitive(true);
01082     but_us_auto->set_sensitive(true);
01083     but_us_emit->set_sensitive(true);
01084     cmb_us_direction->set_sensitive(true);
01085 
01086     this->set_title(std::string("Nao GUI @ ") + connection_dispatcher.get_client()->get_hostname());
01087   } catch (Exception &e) {
01088     Glib::ustring message = *(e.begin());
01089     Gtk::MessageDialog md(*this, message, /* markup */ false,
01090                           Gtk::MESSAGE_ERROR, Gtk::BUTTONS_OK,
01091                           /* modal */ true);
01092     md.set_title("BlackBoard connection failed");
01093     md.run();
01094     if ( bb ) {
01095       bb->unregister_listener(ifd_jointpos);
01096       bb->unregister_listener(ifd_sensor);
01097       bb->unregister_listener(ifd_tts);
01098       bb->unregister_listener(ifd_leds);
01099       bb->unregister_listener(ifd_buttons);
01100       bb->close(jointpos_if);
01101       bb->close(jointstiff_if);
01102       bb->close(sensor_if);
01103       bb->close(nao_navi_if);
01104       bb->close(hummot_fawkes_if);
01105       bb->close(hummot_naoqi_if);
01106       bb->close(speechsynth_if);
01107       delete ifd_jointpos;
01108       delete ifd_sensor;
01109       delete ifd_tts;
01110       delete ifd_leds;
01111       delete bb;
01112       jointpos_if = NULL;
01113       jointstiff_if = NULL;
01114       sensor_if = NULL;
01115       nao_navi_if = NULL;
01116       hummot_fawkes_if = NULL;
01117       hummot_naoqi_if = NULL;
01118       speechsynth_if = NULL;
01119       bb = NULL;
01120       ifd_jointpos = NULL;
01121       ifd_sensor = NULL;
01122       ifd_tts = NULL;
01123       ifd_leds = NULL;
01124     }
01125 
01126     connection_dispatcher.get_client()->disconnect();
01127   }
01128 }
01129 
01130 /** Event handler for disconnected event. */
01131 void
01132 NaoGuiGtkWindow::on_disconnect()
01133 {
01134   if (tb_control->get_active()) tb_control->set_active(false);
01135   frm_servos->set_sensitive(false);
01136   frm_sensors->set_sensitive(false);
01137   frm_ultrasonic->set_sensitive(false);
01138   tb_stiffness->set_sensitive(false);
01139   tb_control->set_sensitive(false);
01140   tb_getup->set_sensitive(false);
01141   tb_parkpos->set_sensitive(false);
01142   tb_zeroall->set_sensitive(false);
01143   but_us_auto->set_sensitive(false);
01144   but_us_emit->set_sensitive(false);
01145   cmb_us_direction->set_sensitive(false);
01146 
01147   if (bb) {
01148     bb->unregister_listener(ifd_jointpos);
01149     bb->unregister_listener(ifd_sensor);
01150     bb->unregister_listener(ifd_tts);
01151     bb->unregister_listener(ifd_leds);
01152     bb->unregister_listener(ifd_buttons);
01153     bb->close(jointpos_if);
01154     bb->close(jointstiff_if);
01155     bb->close(sensor_if);
01156     bb->close(nao_navi_if);
01157     bb->close(hummot_fawkes_if);
01158     bb->close(hummot_naoqi_if);
01159     bb->close(speechsynth_if);
01160 
01161     std::map<std::string, fawkes::LedInterface *>::iterator li;
01162     for (li = led_ifs.begin(); li != led_ifs.end(); ++li) {
01163       bb->close(li->second);
01164     }
01165     led_ifs.clear();
01166 
01167     std::map<std::string, fawkes::SwitchInterface *>::iterator si;
01168     for (si = button_ifs.begin(); si != button_ifs.end(); ++si) {
01169       bb->close(si->second);
01170     }
01171     button_ifs.clear();
01172 
01173     delete ifd_jointpos;
01174     delete ifd_sensor;
01175     delete ifd_tts;
01176     delete ifd_leds;
01177     delete ifd_buttons;
01178     delete bb;
01179     jointpos_if = NULL;
01180     jointstiff_if = NULL;
01181     sensor_if = NULL;
01182     nao_navi_if = NULL;
01183     hummot_fawkes_if = NULL;
01184     hummot_naoqi_if = NULL;
01185     speechsynth_if = NULL;
01186     bb = NULL;
01187     ifd_jointpos = NULL;
01188     ifd_sensor = NULL;
01189     ifd_tts = NULL;
01190     ifd_leds = NULL;
01191     ifd_buttons = NULL;
01192   }
01193 
01194   tb_connection->set_stock_id(Gtk::Stock::CONNECT);
01195   if (servo_enabled) {
01196     servo_enabled = false;
01197     tb_stiffness->set_stock_id(Gtk::Stock::NO);
01198   }
01199   this->set_title("Nao GUI");
01200 }
01201 
01202 
01203 void
01204 NaoGuiGtkWindow::on_exit_clicked()
01205 {
01206   Gtk::Main::quit();
01207 }
01208 
01209 void
01210 NaoGuiGtkWindow::on_goto_parkpos_clicked()
01211 {
01212   if (hummot_naoqi_if && hummot_naoqi_if->has_writer() ) {
01213     HumanoidMotionInterface::ParkMessage *m =
01214       new HumanoidMotionInterface::ParkMessage();
01215     hummot_naoqi_if->msgq_enqueue(m);
01216   }
01217 }
01218 
01219 /**
01220  * Sets all servos to zeros (calibration configuration)
01221  */
01222 void
01223 NaoGuiGtkWindow::on_goto_zero_all_clicked()
01224 {
01225   if ( jointpos_if && jointpos_if->has_writer() ) {
01226     // NaoJointPositionInterface::GotoAnglesMessage *m =
01227     //   new NaoJointPositionInterface::GotoAnglesMessage(/*sec */ 3.0,
01228     //                                               NaoHardwareInterface::INTERPOLATION_SMOOTH,
01229     //                                               /* head */ 0., 0.,
01230     //                                               /* l shoulder */ 0., 0.,
01231     //                                               /* l elbow */ 0., 0.,
01232     //                                               /* l hip */ 0., 0., 0.,
01233     //                                               /* l knee */ 0.,
01234     //                                               /* l ankle */ 0., 0.,
01235     //                                               /* r hip */ 0., 0., 0.,
01236     //                                               /* r knee */ 0.,
01237     //                                               /* r ankle */ 0., 0.,
01238     //                                               /* r shoulder */ 0., 0.,
01239     //                                               /* r elbow */ 0., 0.);
01240     // jointpos_if->msgq_enqueue(m);
01241   }
01242 }
01243 
01244 void
01245 NaoGuiGtkWindow::on_get_up_clicked()
01246 {
01247   if (hummot_naoqi_if && hummot_naoqi_if->has_writer() ) {
01248     HumanoidMotionInterface::GetUpMessage *m =
01249       new HumanoidMotionInterface::GetUpMessage();
01250     hummot_naoqi_if->msgq_enqueue(m);
01251   }
01252 }
01253 
01254 
01255 /**
01256  * Converts a float value to a Glib::ustring (locale dependent)
01257  * @param f The float value
01258  * @param width The precision width
01259  * @return the formatted string
01260  */
01261 Glib::ustring
01262 NaoGuiGtkWindow::convert_float2str(float f, unsigned int width)
01263 {
01264 #if GLIBMM_MAJOR_VERSION > 2 || ( GLIBMM_MAJOR_VERSION == 2 && GLIBMM_MINOR_VERSION >= 16 )
01265   return Glib::ustring::format(std::fixed, std::setprecision(width), f);
01266 #else
01267   std::ostringstream ss;
01268   ss << std::fixed << std::setprecision(width);
01269   ss << f;
01270 
01271   return Glib::locale_to_utf8(ss.str());
01272 #endif
01273 }
01274 
01275 
01276 
01277 bool
01278 NaoGuiGtkWindow::convert_str2float(Glib::ustring sn, float *f)
01279 {
01280   char *endptr = NULL;
01281   *f = strtof(sn.c_str(), &endptr);
01282   if ( endptr[0] != 0 ) {
01283     Glib::ustring s("Could not convert string to valid number: ");
01284     s.append(sn, 0, sn.length() - strlen(endptr));
01285     s += "   &gt;&gt;&gt;<b>";
01286     s += endptr[0];
01287     s += "</b>&lt;&lt;&lt;   ";
01288     s.append(endptr + 1, strlen(endptr) - 1);
01289 
01290     Gtk::MessageDialog md(*this, s,
01291                           /* use markup */ true,
01292                           Gtk::MESSAGE_ERROR);
01293     md.set_title("Invalid value");
01294     md.run();
01295     md.hide();
01296     return false;
01297   } else {
01298     return true;
01299   }
01300 }
01301 
01302 void
01303 NaoGuiGtkWindow::on_ws_exec_clicked()
01304 {
01305   float f;
01306   if ( convert_str2float(ent_ws_distance->get_text(), &f) ) {
01307     HumanoidMotionInterface::WalkStraightMessage *m =
01308       new HumanoidMotionInterface::WalkStraightMessage(f);
01309     if ( rad_motion_fawkes->get_active() ) {
01310       hummot_fawkes_if->msgq_enqueue(m);
01311     } else {
01312       hummot_naoqi_if->msgq_enqueue(m);
01313     }
01314   }
01315 }
01316 
01317 
01318 void
01319 NaoGuiGtkWindow::on_walkvel_exec_clicked()
01320 {
01321   float x = 0., y = 0., theta = 0., speed = 0.;
01322   if ( convert_str2float(ent_walkvel_x->get_text(), &x) &&
01323        convert_str2float(ent_walkvel_y->get_text(), &y) &&
01324        convert_str2float(ent_walkvel_theta->get_text(), &theta) &&
01325        convert_str2float(ent_walkvel_speed->get_text(), &speed) )
01326   {
01327     HumanoidMotionInterface::WalkVelocityMessage *m =
01328       new HumanoidMotionInterface::WalkVelocityMessage(x, y, theta, speed);
01329     if ( rad_motion_fawkes->get_active() ) {
01330       hummot_fawkes_if->msgq_enqueue(m);
01331     } else {
01332       hummot_naoqi_if->msgq_enqueue(m);
01333     }
01334   }
01335 }
01336 
01337 
01338 void
01339 NaoGuiGtkWindow::on_kick_exec_clicked()
01340 {
01341   float f;
01342   if ( convert_str2float(ent_kick_strength->get_text(), &f) ) {
01343     HumanoidMotionInterface::LegEnum leg = HumanoidMotionInterface::LEG_LEFT;
01344     if ( cmb_kick_leg->get_active_row_number() == 1 ) {
01345       leg = HumanoidMotionInterface::LEG_RIGHT;
01346     }
01347     HumanoidMotionInterface::KickMessage *m =
01348       new HumanoidMotionInterface::KickMessage(leg, f);
01349     if ( rad_motion_fawkes->get_active() ) {
01350       hummot_fawkes_if->msgq_enqueue(m);
01351     } else {
01352       hummot_naoqi_if->msgq_enqueue(m);
01353     }
01354   }
01355 }
01356 
01357 
01358 void
01359 NaoGuiGtkWindow::on_standup_exec_clicked()
01360 {
01361   HumanoidMotionInterface::StandupEnum from =
01362     HumanoidMotionInterface::STANDUP_DETECT;
01363 
01364   if (cmb_standup_from->get_active_row_number() == 1) {
01365     from = HumanoidMotionInterface::STANDUP_FRONT;
01366   } else if (cmb_standup_from->get_active_row_number() == 2) {
01367     from = HumanoidMotionInterface::STANDUP_BACK;
01368   }
01369 
01370   HumanoidMotionInterface::StandupMessage *m =
01371     new HumanoidMotionInterface::StandupMessage(from);
01372   if ( rad_motion_fawkes->get_active() ) {
01373     hummot_fawkes_if->msgq_enqueue(m);
01374   } else {
01375     hummot_naoqi_if->msgq_enqueue(m);
01376   }
01377 }
01378 
01379 
01380 void
01381 NaoGuiGtkWindow::on_wsw_exec_clicked()
01382 {
01383   float f;
01384   if ( convert_str2float(ent_wsw_distance->get_text(), &f) ) {
01385     HumanoidMotionInterface::WalkSidewaysMessage *m =
01386       new HumanoidMotionInterface::WalkSidewaysMessage(f);
01387     if ( rad_motion_fawkes->get_active() ) {
01388       hummot_fawkes_if->msgq_enqueue(m);
01389     } else {
01390       hummot_naoqi_if->msgq_enqueue(m);
01391     }
01392   }
01393 }
01394 
01395 void
01396 NaoGuiGtkWindow::on_nav_exec_clicked()
01397 {
01398   float x,y,ori;
01399   if ( convert_str2float(ent_nav_x->get_text(), &x)
01400        && convert_str2float(ent_nav_y->get_text(), &y)
01401        && convert_str2float(ent_nav_ori->get_text(), &ori) ) {
01402     NavigatorInterface::CartesianGotoMessage *m =
01403       new NavigatorInterface::CartesianGotoMessage(x, y, ori);
01404     nao_navi_if->msgq_enqueue(m);
01405   }
01406 }
01407 
01408 
01409 void
01410 NaoGuiGtkWindow::on_wa_exec_clicked()
01411 {
01412   float angle, radius;
01413   if ( convert_str2float(ent_wa_angle->get_text(), &angle) &&
01414        convert_str2float(ent_wa_radius->get_text(), &radius) ) {
01415     HumanoidMotionInterface::WalkArcMessage *m =
01416       new HumanoidMotionInterface::WalkArcMessage(angle, radius);
01417     if ( rad_motion_fawkes->get_active() ) {
01418       hummot_fawkes_if->msgq_enqueue(m);
01419     } else {
01420       hummot_naoqi_if->msgq_enqueue(m);
01421     }
01422   }
01423 }
01424 
01425 
01426 void
01427 NaoGuiGtkWindow::on_turn_exec_clicked()
01428 {
01429   float f;
01430   if ( convert_str2float(ent_turn_angle->get_text(), &f) ) {
01431     HumanoidMotionInterface::TurnMessage *m =
01432       new HumanoidMotionInterface::TurnMessage(f);
01433     if ( rad_motion_fawkes->get_active() ) {
01434       hummot_fawkes_if->msgq_enqueue(m);
01435     } else {
01436       hummot_naoqi_if->msgq_enqueue(m);
01437     }
01438   }
01439 }
01440 
01441 
01442 void
01443 NaoGuiGtkWindow::on_motion_stop_clicked()
01444 {
01445   HumanoidMotionInterface::StopMessage *m =
01446     new HumanoidMotionInterface::StopMessage();
01447   if ( rad_motion_fawkes->get_active() ) {
01448     hummot_fawkes_if->msgq_enqueue(m);
01449   } else {
01450     hummot_naoqi_if->msgq_enqueue(m);
01451   }
01452 }
01453 
01454 
01455 void
01456 NaoGuiGtkWindow::on_stiffness_global_toggled()
01457 {
01458   bool is_active = chb_stiffness_global->get_active();
01459 
01460   spb_HeadYaw->set_sensitive(! is_active);
01461   spb_HeadPitch->set_sensitive(! is_active);
01462   spb_RShoulderPitch->set_sensitive(! is_active);
01463   spb_RShoulderRoll->set_sensitive(! is_active);
01464   spb_RElbowYaw->set_sensitive(! is_active);
01465   spb_RElbowRoll->set_sensitive(! is_active);
01466   spb_RWristYaw->set_sensitive(! is_active);
01467   spb_RHand->set_sensitive(! is_active);
01468   spb_LShoulderPitch->set_sensitive(! is_active);
01469   spb_LShoulderRoll->set_sensitive(! is_active);
01470   spb_LElbowYaw->set_sensitive(! is_active);
01471   spb_LElbowRoll->set_sensitive(! is_active);
01472   spb_LWristYaw->set_sensitive(! is_active);
01473   spb_LHand->set_sensitive(! is_active);
01474   spb_RHipYawPitch->set_sensitive(! is_active);
01475   spb_RHipPitch->set_sensitive(! is_active);
01476   spb_RHipRoll->set_sensitive(! is_active);
01477   spb_RKneePitch->set_sensitive(! is_active);
01478   spb_RAnklePitch->set_sensitive(! is_active);
01479   spb_RAnkleRoll->set_sensitive(! is_active);
01480   spb_LHipYawPitch->set_sensitive(! is_active);
01481   spb_LHipPitch->set_sensitive(! is_active);
01482   spb_LHipRoll->set_sensitive(! is_active);
01483   spb_LKneePitch->set_sensitive(! is_active);
01484   spb_LAnklePitch->set_sensitive(! is_active);
01485   spb_LAnkleRoll->set_sensitive(! is_active);
01486 
01487   spb_stiffness_global->set_sensitive(is_active);
01488 
01489   lab_stiff_HeadYaw->set_sensitive(! is_active);
01490   lab_stiff_HeadPitch->set_sensitive(! is_active);
01491   lab_stiff_RShoulderPitch->set_sensitive(! is_active);
01492   lab_stiff_RShoulderRoll->set_sensitive(! is_active);
01493   lab_stiff_RElbowYaw->set_sensitive(! is_active);
01494   lab_stiff_RElbowRoll->set_sensitive(! is_active);
01495   lab_stiff_RWristYaw->set_sensitive(! is_active);
01496   lab_stiff_RHand->set_sensitive(! is_active);
01497   lab_stiff_LShoulderPitch->set_sensitive(! is_active);
01498   lab_stiff_LShoulderRoll->set_sensitive(! is_active);
01499   lab_stiff_LElbowYaw->set_sensitive(! is_active);
01500   lab_stiff_LElbowRoll->set_sensitive(! is_active);
01501   lab_stiff_LWristYaw->set_sensitive(! is_active);
01502   lab_stiff_LHand->set_sensitive(! is_active);
01503   lab_stiff_RHipYawPitch->set_sensitive(! is_active);
01504   lab_stiff_RHipPitch->set_sensitive(! is_active);
01505   lab_stiff_RHipRoll->set_sensitive(! is_active);
01506   lab_stiff_RKneePitch->set_sensitive(! is_active);
01507   lab_stiff_RAnklePitch->set_sensitive(! is_active);
01508   lab_stiff_RAnkleRoll->set_sensitive(! is_active);
01509   lab_stiff_LHipYawPitch->set_sensitive(! is_active);
01510   lab_stiff_LHipPitch->set_sensitive(! is_active);
01511   lab_stiff_LHipRoll->set_sensitive(! is_active);
01512   lab_stiff_LKneePitch->set_sensitive(! is_active);
01513   lab_stiff_LAnklePitch->set_sensitive(! is_active);
01514   lab_stiff_LAnkleRoll->set_sensitive(! is_active);
01515 }
01516 
01517 void
01518 NaoGuiGtkWindow::on_stiffness_write_clicked()
01519 {
01520 
01521 
01522   if (chb_stiffness_global->get_active()) {
01523     float stiff_global = spb_stiffness_global->get_value();
01524     NaoJointStiffnessInterface::SetBodyStiffnessMessage *m =
01525       new NaoJointStiffnessInterface::SetBodyStiffnessMessage(stiff_global, .5);
01526     jointstiff_if->msgq_enqueue(m);
01527   } else {
01528     float stiff_head_yaw = spb_HeadYaw->get_value(),
01529       stiff_head_pitch = spb_HeadPitch->get_value(),
01530       stiff_l_shoulder_pitch = spb_LShoulderPitch->get_value(),
01531       stiff_l_shoulder_roll = spb_LShoulderRoll->get_value(),
01532       stiff_l_elbow_yaw = spb_LElbowYaw->get_value(),
01533       stiff_l_elbow_roll = spb_LElbowRoll->get_value(),
01534       stiff_l_wrist_yaw = spb_LWristYaw->get_value(),
01535       stiff_l_hand = spb_LHand->get_value(),
01536       stiff_l_hip_yaw_pitch = spb_LHipYawPitch->get_value(),
01537       stiff_l_hip_pitch = spb_LHipPitch->get_value(),
01538       stiff_l_hip_roll = spb_LHipRoll->get_value(),
01539       stiff_l_knee_pitch = spb_LKneePitch->get_value(),
01540       stiff_l_ankle_roll = spb_LAnkleRoll->get_value(),
01541       stiff_l_ankle_pitch = spb_LAnklePitch->get_value(),
01542       stiff_r_shoulder_pitch = spb_RShoulderPitch->get_value(),
01543       stiff_r_shoulder_roll = spb_RShoulderRoll->get_value(),
01544       stiff_r_elbow_yaw = spb_RElbowYaw->get_value(),
01545       stiff_r_elbow_roll = spb_RElbowRoll->get_value(),
01546       stiff_r_wrist_yaw = spb_RWristYaw->get_value(),
01547       stiff_r_hand = spb_RHand->get_value(),
01548       stiff_r_hip_yaw_pitch = spb_RHipYawPitch->get_value(),
01549       stiff_r_hippitch = spb_RHipPitch->get_value(),
01550       stiff_r_hiproll = spb_RHipRoll->get_value(),
01551       stiff_r_knee_pitch = spb_RKneePitch->get_value(),
01552       stiff_r_ankle_roll = spb_RAnkleRoll->get_value(),
01553       stiff_r_ankle_pitch = spb_RAnklePitch->get_value();
01554 
01555     NaoJointStiffnessInterface::SetStiffnessesMessage *m =
01556       new NaoJointStiffnessInterface::SetStiffnessesMessage
01557              (0.5,
01558               stiff_head_yaw, stiff_head_pitch,
01559               stiff_l_shoulder_pitch, stiff_l_shoulder_roll,
01560               stiff_l_elbow_yaw, stiff_l_elbow_roll,
01561               stiff_l_wrist_yaw, stiff_l_hand,
01562               stiff_l_hip_yaw_pitch,
01563               stiff_l_hip_roll, stiff_l_hip_pitch, stiff_l_knee_pitch,
01564               stiff_l_ankle_pitch, stiff_l_ankle_roll,
01565               stiff_r_shoulder_pitch, stiff_r_shoulder_roll,
01566               stiff_r_elbow_yaw, stiff_r_elbow_roll,
01567               stiff_r_wrist_yaw, stiff_r_hand,
01568               stiff_r_hip_yaw_pitch, stiff_r_hiproll, stiff_r_hippitch,
01569               stiff_r_knee_pitch, stiff_r_ankle_roll, stiff_r_ankle_pitch);
01570 
01571     jointstiff_if->msgq_enqueue(m);
01572   }
01573 }
01574 
01575 void
01576 NaoGuiGtkWindow::on_stiffness_read_clicked()
01577 {
01578   jointstiff_if->read();
01579 
01580   spb_HeadYaw->set_value(jointstiff_if->head_yaw());
01581   spb_HeadPitch->set_value(jointstiff_if->head_pitch());
01582   spb_RShoulderPitch->set_value(jointstiff_if->r_shoulder_pitch());
01583   spb_RShoulderRoll->set_value(jointstiff_if->r_shoulder_roll());
01584   spb_RElbowYaw->set_value(jointstiff_if->r_elbow_yaw());
01585   spb_RElbowRoll->set_value(jointstiff_if->r_elbow_roll());
01586   spb_RWristYaw->set_value(jointstiff_if->r_wrist_yaw());
01587   spb_RHand->set_value(jointstiff_if->r_hand());
01588   spb_LShoulderPitch->set_value(jointstiff_if->l_shoulder_pitch());
01589   spb_LShoulderRoll->set_value(jointstiff_if->l_shoulder_roll());
01590   spb_LElbowYaw->set_value(jointstiff_if->l_elbow_yaw());
01591   spb_LElbowRoll->set_value(jointstiff_if->l_elbow_roll());
01592   spb_LWristYaw->set_value(jointstiff_if->l_wrist_yaw());
01593   spb_LHand->set_value(jointstiff_if->l_hand());
01594   spb_RHipYawPitch->set_value(jointstiff_if->r_hip_yaw_pitch());
01595   spb_RHipPitch->set_value(jointstiff_if->r_hip_pitch());
01596   spb_RHipRoll->set_value(jointstiff_if->r_hip_roll());
01597   spb_RKneePitch->set_value(jointstiff_if->r_knee_pitch());
01598   spb_RAnklePitch->set_value(jointstiff_if->r_ankle_pitch());
01599   spb_RAnkleRoll->set_value(jointstiff_if->r_ankle_roll());
01600   spb_LHipYawPitch->set_value(jointstiff_if->l_hip_yaw_pitch());
01601   spb_LHipPitch->set_value(jointstiff_if->l_hip_pitch());
01602   spb_LHipRoll->set_value(jointstiff_if->l_hip_roll());
01603   spb_LKneePitch->set_value(jointstiff_if->l_knee_pitch());
01604   spb_LAnklePitch->set_value(jointstiff_if->l_ankle_pitch());
01605   spb_LAnkleRoll->set_value(jointstiff_if->l_ankle_roll());
01606 
01607   spb_stiffness_global->set_value(jointstiff_if->minimum());
01608 }
01609 
01610 void
01611 NaoGuiGtkWindow::on_tts_exec_clicked()
01612 {
01613   SpeechSynthInterface::SayMessage *m =
01614     new SpeechSynthInterface::SayMessage(ent_tts->get_text().c_str());
01615   speechsynth_if->msgq_enqueue(m);
01616 }
01617 
01618 
01619 void
01620 NaoGuiGtkWindow::on_led_tb_toggled(std::string iface_id,
01621                                    Gtk::ToggleButton *tb)
01622 {
01623   if ( (led_ifs.find(iface_id) != led_ifs.end()) && led_ifs[iface_id]->is_valid())
01624   {
01625     try {
01626       if (tb->get_active()) {
01627         led_ifs[iface_id]->msgq_enqueue(new LedInterface::TurnOnMessage());
01628       } else {
01629         led_ifs[iface_id]->msgq_enqueue(new LedInterface::TurnOffMessage());
01630       }
01631     } catch (Exception &e) {
01632       e.print_trace();
01633     }
01634   }
01635 }
01636 
01637 void
01638 NaoGuiGtkWindow::on_control_leds_toggled()
01639 {
01640   bool is_active = tb_control_leds->get_active();
01641 
01642   scl_chest_r->set_sensitive(is_active);
01643   scl_chest_g->set_sensitive(is_active);
01644   scl_chest_b->set_sensitive(is_active);
01645   scl_left_eye_r->set_sensitive(is_active);
01646   scl_left_eye_g->set_sensitive(is_active);
01647   scl_left_eye_b->set_sensitive(is_active);
01648   scl_right_eye_r->set_sensitive(is_active);
01649   scl_right_eye_g->set_sensitive(is_active);
01650   scl_right_eye_b->set_sensitive(is_active);
01651   scl_left_foot_r->set_sensitive(is_active);
01652   scl_left_foot_g->set_sensitive(is_active);
01653   scl_left_foot_b->set_sensitive(is_active);
01654   scl_right_foot_r->set_sensitive(is_active);
01655   scl_right_foot_g->set_sensitive(is_active);
01656   scl_right_foot_b->set_sensitive(is_active);
01657 
01658   tb_left_ear_0->set_sensitive(is_active);
01659   tb_left_ear_36->set_sensitive(is_active);
01660   tb_left_ear_72->set_sensitive(is_active);
01661   tb_left_ear_108->set_sensitive(is_active);
01662   tb_left_ear_144->set_sensitive(is_active);
01663   tb_left_ear_180->set_sensitive(is_active);
01664   tb_left_ear_216->set_sensitive(is_active);
01665   tb_left_ear_252->set_sensitive(is_active);
01666   tb_left_ear_288->set_sensitive(is_active);
01667   tb_left_ear_324->set_sensitive(is_active);
01668 
01669   tb_right_ear_0->set_sensitive(is_active);
01670   tb_right_ear_36->set_sensitive(is_active);
01671   tb_right_ear_72->set_sensitive(is_active);
01672   tb_right_ear_108->set_sensitive(is_active);
01673   tb_right_ear_144->set_sensitive(is_active);
01674   tb_right_ear_180->set_sensitive(is_active);
01675   tb_right_ear_216->set_sensitive(is_active);
01676   tb_right_ear_252->set_sensitive(is_active);
01677   tb_right_ear_288->set_sensitive(is_active);
01678   tb_right_ear_324->set_sensitive(is_active);
01679 }
01680 
01681 
01682 void
01683 NaoGuiGtkWindow::on_led_slider_changed(std::string iface_id, Gtk::Scale *scl)
01684 {
01685   if ( (led_ifs.find(iface_id) != led_ifs.end()) && led_ifs[iface_id]->is_valid())
01686   {
01687     try {
01688       LedInterface::SetIntensityMessage *sim =
01689         new LedInterface::SetIntensityMessage(0., scl->get_value());
01690       led_ifs[iface_id]->msgq_enqueue(sim);
01691     } catch (Exception &e) {
01692       e.print_trace();
01693     }
01694   }  
01695 }
01696 
01697 
01698 bool
01699 NaoGuiGtkWindow::on_led_slider_button_release(GdkEventButton *event,
01700                                               std::string iface_id,
01701                                               Gtk::Scale *scl)
01702 {
01703   if ( (led_ifs.find(iface_id) != led_ifs.end()) && led_ifs[iface_id]->is_valid())
01704   {
01705     on_led_data_changed(led_ifs[iface_id]);
01706   }
01707   return true;
01708 }
01709 
01710 void
01711 NaoGuiGtkWindow::on_led_data_changed(fawkes::Interface *iface)
01712 {
01713   if (tb_control_leds->get_active())  return;
01714 
01715   std::string id = iface->id();
01716   std::string::size_type last_space  = id.rfind(" ");
01717   std::string device = id.substr(last_space + 1);
01718 
01719   LedInterface *led_if = dynamic_cast<LedInterface *>(iface);
01720   if ((led_if != NULL) && (led_scales.find(device) != led_scales.end())) {
01721     try {
01722       led_if->read();
01723       led_scales[device]->set_value(led_if->intensity());
01724     } catch (Exception &e) {}
01725   }
01726 
01727   if ((led_if != NULL) && (led_buttons.find(device) != led_buttons.end())) {
01728     try {
01729       led_if->read();
01730       led_buttons[device]->set_active(led_if->intensity() >= .5);
01731     } catch (Exception &e) {}
01732   }
01733 }
01734 
01735 
01736 void
01737 NaoGuiGtkWindow::on_button_click_pressed(std::string iface_id)
01738 {
01739   if (button_ifs.find(iface_id) != button_ifs.end()) {
01740     try {
01741       SwitchInterface::EnableSwitchMessage *msg =
01742         new SwitchInterface::EnableSwitchMessage();
01743       button_ifs[iface_id]->msgq_enqueue(msg);
01744     } catch (Exception &e) {}
01745   }
01746 }
01747 
01748 void
01749 NaoGuiGtkWindow::on_button_click_released(std::string iface_id)
01750 {
01751   if (button_ifs.find(iface_id) != button_ifs.end()) {
01752     try {
01753       SwitchInterface::DisableSwitchMessage *msg =
01754         new SwitchInterface::DisableSwitchMessage();
01755       button_ifs[iface_id]->msgq_enqueue(msg);
01756     } catch (Exception &e) {}
01757   }
01758 }
01759 
01760 void
01761 NaoGuiGtkWindow::on_button_data_changed(fawkes::Interface *iface)
01762 {
01763   std::string id = iface->id();
01764   std::string::size_type start  = std::string("Nao Button ").size();
01765   std::string device = id.substr(start);
01766 
01767   SwitchInterface *switch_if = dynamic_cast<SwitchInterface *>(iface);
01768   if ((switch_if != NULL) && (button_labels.find(device) != button_labels.end())) {
01769     try {
01770       switch_if->read();
01771       ButtonLabelSet &l = button_labels[device];
01772       if (switch_if->is_enabled() && l.lab_enabled->get_text() == "No") {
01773         l.lab_enabled->set_text("Yes");
01774       }
01775       if (! switch_if->is_enabled() && l.lab_enabled->get_text() == "Yes") {
01776         l.lab_enabled->set_text("No");
01777       }
01778 
01779       Glib::ustring s;
01780       s = convert_float2str(switch_if->history(), 3);
01781       if (l.lab_history->get_text() != s)  l.lab_history->set_text(s);
01782 
01783       s = convert_float2str(switch_if->value(), 1);
01784       if (l.lab_value->get_text() != s)  l.lab_value->set_text(s);
01785 
01786       s = convert_float2str(switch_if->short_activations(), 0);
01787       if (l.lab_short->get_text() != s)  l.lab_short->set_text(s);
01788 
01789       s = convert_float2str(switch_if->long_activations(), 0);
01790       if (l.lab_long->get_text() != s)  l.lab_long->set_text(s);
01791 
01792       s = convert_float2str(switch_if->activation_count(), 0);
01793       if (l.lab_total->get_text() != s)  l.lab_total->set_text(s);
01794     } catch (Exception &e) {}
01795   } else {
01796     printf("Could not find %s\n", device.c_str());
01797   }
01798 }
01799