Fawkes API
Fawkes Development Version
|
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 += " >>><b>"; 01286 s += endptr[0]; 01287 s += "</b><<< "; 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