Fawkes API  Fawkes Development Version
naogui.cpp
1 
2 /***************************************************************************
3  * naogui.cpp - Nao GUI
4  *
5  * Created: Mon Apr 14 12:54:19 2008
6  * Copyright 2008-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "naogui.h"
24 
25 #include <utils/system/argparser.h>
26 #include <blackboard/remote.h>
27 #include <interfaces/NaoJointPositionInterface.h>
28 #include <interfaces/NaoJointStiffnessInterface.h>
29 #include <interfaces/NaoSensorInterface.h>
30 #include <interfaces/HumanoidMotionInterface.h>
31 #include <interfaces/NavigatorInterface.h>
32 #include <interfaces/SpeechSynthInterface.h>
33 #include <interfaces/LedInterface.h>
34 #include <interfaces/SwitchInterface.h>
35 #include <netcomm/fawkes/client.h>
36 
37 #include <gui_utils/service_chooser_dialog.h>
38 #include <gui_utils/interface_dispatcher.h>
39 
40 #include <cstring>
41 #include <string>
42 #include <sstream>
43 #include <iomanip>
44 #include <sstream>
45 #include <gtkmm/clipboard.h>
46 
47 using namespace fawkes;
48 
49 
50 /** @class NaoGuiGtkWindow "naogui.h"
51  * Nao GUI main window.
52  * The Nao GUI provides access to the most basic servo, sensor and motion
53  * data and commands.
54  * @author Tim Niemueller
55  */
56 
57 /** Constructor.
58  * @param cobject C base object
59  * @param builder Gtk builder to get widgets from
60  */
61 NaoGuiGtkWindow::NaoGuiGtkWindow(BaseObjectType* cobject,
62  const Glib::RefPtr<Gtk::Builder> &builder)
63  : Gtk::Window(cobject)
64 {
65  bb = NULL;
66  jointpos_if = NULL;
67  jointstiff_if = NULL;
68  sensor_if = NULL;
69  nao_navi_if = NULL;
70  hummot_naoqi_if = NULL;
71  hummot_fawkes_if = NULL;
72  speechsynth_if = NULL;
73  servo_enabled = false;
74 
75  update_cycle = 0;
76 
77  builder->get_widget("frm_servos", frm_servos);
78  builder->get_widget("frm_sensors", frm_sensors);
79  builder->get_widget("frm_ultrasonic", frm_ultrasonic);
80 
81  builder->get_widget("lab_HeadYaw", lab_HeadYaw);
82  builder->get_widget("lab_HeadPitch", lab_HeadPitch);
83  builder->get_widget("lab_RShoulderPitch", lab_RShoulderPitch);
84  builder->get_widget("lab_RShoulderRoll", lab_RShoulderRoll);
85  builder->get_widget("lab_LShoulderPitch", lab_LShoulderPitch);
86  builder->get_widget("lab_LShoulderRoll", lab_LShoulderRoll);
87  builder->get_widget("lab_LElbowYaw", lab_LElbowYaw);
88  builder->get_widget("lab_LElbowRoll", lab_LElbowRoll);
89  builder->get_widget("lab_LWristYaw", lab_LWristYaw);
90  builder->get_widget("lab_LHand", lab_LHand);
91  builder->get_widget("lab_RElbowYaw", lab_RElbowYaw);
92  builder->get_widget("lab_RElbowRoll", lab_RElbowRoll);
93  builder->get_widget("lab_RWristYaw", lab_RWristYaw);
94  builder->get_widget("lab_RHand", lab_RHand);
95  builder->get_widget("lab_RHipYawPitch", lab_RHipYawPitch);
96  builder->get_widget("lab_RHipPitch", lab_RHipPitch);
97  builder->get_widget("lab_RHipRoll", lab_RHipRoll);
98  builder->get_widget("lab_RKneePitch", lab_RKneePitch);
99  builder->get_widget("lab_RAnklePitch", lab_RAnklePitch);
100  builder->get_widget("lab_RAnkleRoll", lab_RAnkleRoll);
101  builder->get_widget("lab_LHipYawPitch", lab_LHipYawPitch);
102  builder->get_widget("lab_LHipPitch", lab_LHipPitch);
103  builder->get_widget("lab_LHipRoll", lab_LHipRoll);
104  builder->get_widget("lab_LKneePitch", lab_LKneePitch);
105  builder->get_widget("lab_LAnklePitch", lab_LAnklePitch);
106  builder->get_widget("lab_LAnkleRoll", lab_LAnkleRoll);
107  builder->get_widget("lab_stiff_HeadYaw", lab_stiff_HeadYaw);
108  builder->get_widget("lab_stiff_HeadPitch", lab_stiff_HeadPitch);
109  builder->get_widget("lab_stiff_RShoulderPitch", lab_stiff_RShoulderPitch);
110  builder->get_widget("lab_stiff_RShoulderRoll", lab_stiff_RShoulderRoll);
111  builder->get_widget("lab_stiff_LShoulderPitch", lab_stiff_LShoulderPitch);
112  builder->get_widget("lab_stiff_LShoulderRoll", lab_stiff_LShoulderRoll);
113  builder->get_widget("lab_stiff_LElbowYaw", lab_stiff_LElbowYaw);
114  builder->get_widget("lab_stiff_LElbowRoll", lab_stiff_LElbowRoll);
115  builder->get_widget("lab_stiff_LWristYaw", lab_stiff_LWristYaw);
116  builder->get_widget("lab_stiff_LHand", lab_stiff_LHand);
117  builder->get_widget("lab_stiff_RElbowYaw", lab_stiff_RElbowYaw);
118  builder->get_widget("lab_stiff_RElbowRoll", lab_stiff_RElbowRoll);
119  builder->get_widget("lab_stiff_RWristYaw", lab_stiff_RWristYaw);
120  builder->get_widget("lab_stiff_RHand", lab_stiff_RHand);
121  builder->get_widget("lab_stiff_RHipYawPitch", lab_stiff_RHipYawPitch);
122  builder->get_widget("lab_stiff_RHipPitch", lab_stiff_RHipPitch);
123  builder->get_widget("lab_stiff_RHipRoll", lab_stiff_RHipRoll);
124  builder->get_widget("lab_stiff_RKneePitch", lab_stiff_RKneePitch);
125  builder->get_widget("lab_stiff_RAnklePitch", lab_stiff_RAnklePitch);
126  builder->get_widget("lab_stiff_RAnkleRoll", lab_stiff_RAnkleRoll);
127  builder->get_widget("lab_stiff_LHipYawPitch", lab_stiff_LHipYawPitch);
128  builder->get_widget("lab_stiff_LHipPitch", lab_stiff_LHipPitch);
129  builder->get_widget("lab_stiff_LHipRoll", lab_stiff_LHipRoll);
130  builder->get_widget("lab_stiff_LKneePitch", lab_stiff_LKneePitch);
131  builder->get_widget("lab_stiff_LAnklePitch", lab_stiff_LAnklePitch);
132  builder->get_widget("lab_stiff_LAnkleRoll", lab_stiff_LAnkleRoll);
133  builder->get_widget("hsc_HeadYaw", hsc_HeadYaw);
134  builder->get_widget("hsc_HeadPitch", hsc_HeadPitch);
135  builder->get_widget("hsc_RShoulderPitch", hsc_RShoulderPitch);
136  builder->get_widget("hsc_RShoulderRoll", hsc_RShoulderRoll);
137  builder->get_widget("hsc_RElbowYaw", hsc_RElbowYaw);
138  builder->get_widget("hsc_RElbowRoll", hsc_RElbowRoll);
139  builder->get_widget("hsc_RWristYaw", hsc_RWristYaw);
140  builder->get_widget("hsc_RHand", hsc_RHand);
141  builder->get_widget("hsc_LShoulderPitch", hsc_LShoulderPitch);
142  builder->get_widget("hsc_LShoulderRoll", hsc_LShoulderRoll);
143  builder->get_widget("hsc_LElbowYaw", hsc_LElbowYaw);
144  builder->get_widget("hsc_LElbowRoll", hsc_LElbowRoll);
145  builder->get_widget("hsc_LWristYaw", hsc_LWristYaw);
146  builder->get_widget("hsc_LHand", hsc_LHand);
147  builder->get_widget("hsc_RHipYawPitch", hsc_RHipYawPitch);
148  builder->get_widget("hsc_RHipPitch", hsc_RHipPitch);
149  builder->get_widget("hsc_RHipRoll", hsc_RHipRoll);
150  builder->get_widget("hsc_RKneePitch", hsc_RKneePitch);
151  builder->get_widget("hsc_RAnklePitch", hsc_RAnklePitch);
152  builder->get_widget("hsc_RAnkleRoll", hsc_RAnkleRoll);
153  builder->get_widget("hsc_LHipYawPitch", hsc_LHipYawPitch);
154  builder->get_widget("hsc_LHipPitch", hsc_LHipPitch);
155  builder->get_widget("hsc_LHipRoll", hsc_LHipRoll);
156  builder->get_widget("hsc_LKneePitch", hsc_LKneePitch);
157  builder->get_widget("hsc_LAnklePitch", hsc_LAnklePitch);
158  builder->get_widget("hsc_LAnkleRoll", hsc_LAnkleRoll);
159  builder->get_widget("hsc_speed", hsc_speed);
160  builder->get_widget("lab_speed", lab_speed);
161  builder->get_widget("tb_connection", tb_connection);
162  builder->get_widget("tb_stiffness", tb_stiffness);
163  builder->get_widget("tb_control", tb_control);
164  builder->get_widget("tb_getup", tb_getup);
165  builder->get_widget("tb_parkpos", tb_parkpos);
166  builder->get_widget("tb_zeroall", tb_zeroall);
167  builder->get_widget("tb_exit", tb_exit);
168  builder->get_widget("lab_l_fsr_fl", lab_l_fsr_fl);
169  builder->get_widget("lab_l_fsr_fr", lab_l_fsr_fr);
170  builder->get_widget("lab_l_fsr_rl", lab_l_fsr_rl);
171  builder->get_widget("lab_l_fsr_rr", lab_l_fsr_rr);
172  builder->get_widget("lab_r_fsr_fl", lab_r_fsr_fl);
173  builder->get_widget("lab_r_fsr_fr", lab_r_fsr_fr);
174  builder->get_widget("lab_r_fsr_rl", lab_r_fsr_rl);
175  builder->get_widget("lab_r_fsr_rr", lab_r_fsr_rr);
176  builder->get_widget("lab_l_cop", lab_l_cop);
177  builder->get_widget("lab_r_cop", lab_r_cop);
178  builder->get_widget("lab_l_total_weight", lab_l_total_weight);
179  builder->get_widget("lab_r_total_weight", lab_r_total_weight);
180  builder->get_widget("lab_chest_button", lab_chest_button);
181  builder->get_widget("lab_touch_front", lab_touch_front);
182  builder->get_widget("lab_touch_middle", lab_touch_middle);
183  builder->get_widget("lab_touch_rear", lab_touch_rear);
184  builder->get_widget("lab_l_bumper_l", lab_l_bumper_l);
185  builder->get_widget("lab_l_bumper_r", lab_l_bumper_r);
186  builder->get_widget("lab_r_bumper_l", lab_r_bumper_l);
187  builder->get_widget("lab_r_bumper_r", lab_r_bumper_r);
188  builder->get_widget("lab_accel_x", lab_accel_x);
189  builder->get_widget("lab_accel_y", lab_accel_y);
190  builder->get_widget("lab_accel_z", lab_accel_z);
191  builder->get_widget("lab_gyro_x", lab_gyro_x);
192  builder->get_widget("lab_gyro_y", lab_gyro_y);
193  builder->get_widget("lab_gyro_ref", lab_gyro_ref);
194  builder->get_widget("lab_angles_xy", lab_angles_xy);
195  builder->get_widget("lab_ultrasonic_direction", lab_ultrasonic_direction);
196  builder->get_widget("lab_ultrasonic_left0", lab_ultrasonic_left0);
197  builder->get_widget("lab_ultrasonic_left1", lab_ultrasonic_left1);
198  builder->get_widget("lab_ultrasonic_left2", lab_ultrasonic_left2);
199  builder->get_widget("lab_ultrasonic_left3", lab_ultrasonic_left3);
200  builder->get_widget("lab_ultrasonic_right0", lab_ultrasonic_right0);
201  builder->get_widget("lab_ultrasonic_right1", lab_ultrasonic_right1);
202  builder->get_widget("lab_ultrasonic_right2", lab_ultrasonic_right2);
203  builder->get_widget("lab_ultrasonic_right3", lab_ultrasonic_right3);
204  builder->get_widget("lab_battery_charge", lab_battery_charge);
205  builder->get_widget("but_sv_copy", but_sv_copy);
206  builder->get_widget("cmb_us_direction", cmb_us_direction);
207  builder->get_widget("but_us_emit", but_us_emit);
208  builder->get_widget("but_us_auto", but_us_auto);
209  builder->get_widget("but_stop", but_stop);
210  builder->get_widget("but_ws_exec", but_ws_exec);
211  builder->get_widget("ent_ws_distance", ent_ws_distance);
212  builder->get_widget("but_wsw_exec", but_wsw_exec);
213  builder->get_widget("ent_wsw_distance", ent_wsw_distance);
214  builder->get_widget("but_wa_exec", but_wa_exec);
215  builder->get_widget("ent_wa_angle", ent_wa_angle);
216  builder->get_widget("ent_wa_radius", ent_wa_radius);
217  builder->get_widget("but_turn_exec", but_turn_exec);
218  builder->get_widget("cmb_kick_leg", cmb_kick_leg);
219  builder->get_widget("ent_kick_strength", ent_kick_strength);
220  builder->get_widget("but_kick_exec", but_kick_exec);
221  builder->get_widget("cmb_standup_from", cmb_standup_from);
222  builder->get_widget("but_standup_exec", but_standup_exec);
223  builder->get_widget("ent_turn_angle", ent_turn_angle);
224  builder->get_widget("rad_motion_fawkes", rad_motion_fawkes);
225  builder->get_widget("rad_motion_naoqi", rad_motion_naoqi);
226  builder->get_widget("ent_walkvel_x", ent_walkvel_x);
227  builder->get_widget("ent_walkvel_y", ent_walkvel_y);
228  builder->get_widget("ent_walkvel_theta", ent_walkvel_theta);
229  builder->get_widget("ent_walkvel_speed", ent_walkvel_speed);
230  builder->get_widget("but_walkvel_exec", but_walkvel_exec);
231 
232  builder->get_widget("but_stiffness_read", but_stiffness_read);
233  builder->get_widget("but_stiffness_write", but_stiffness_write);
234  builder->get_widget("chb_stiffness_global", chb_stiffness_global);
235  builder->get_widget("spb_stiffness_global", spb_stiffness_global);
236  builder->get_widget("spb_HeadYaw", spb_HeadYaw);
237  builder->get_widget("spb_HeadPitch", spb_HeadPitch);
238  builder->get_widget("spb_RShoulderPitch", spb_RShoulderPitch);
239  builder->get_widget("spb_RShoulderRoll", spb_RShoulderRoll);
240  builder->get_widget("spb_RElbowYaw", spb_RElbowYaw);
241  builder->get_widget("spb_RElbowRoll", spb_RElbowRoll);
242  builder->get_widget("spb_RWristYaw", spb_RWristYaw);
243  builder->get_widget("spb_RHand", spb_RHand);
244  builder->get_widget("spb_LShoulderPitch", spb_LShoulderPitch);
245  builder->get_widget("spb_LShoulderRoll", spb_LShoulderRoll);
246  builder->get_widget("spb_LElbowYaw", spb_LElbowYaw);
247  builder->get_widget("spb_LElbowRoll", spb_LElbowRoll);
248  builder->get_widget("spb_LWristYaw", spb_LWristYaw);
249  builder->get_widget("spb_LHand", spb_LHand);
250  builder->get_widget("spb_RHipYawPitch", spb_RHipYawPitch);
251  builder->get_widget("spb_RHipPitch", spb_RHipPitch);
252  builder->get_widget("spb_RHipRoll", spb_RHipRoll);
253  builder->get_widget("spb_RKneePitch", spb_RKneePitch);
254  builder->get_widget("spb_RAnklePitch", spb_RAnklePitch);
255  builder->get_widget("spb_RAnkleRoll", spb_RAnkleRoll);
256  builder->get_widget("spb_LHipYawPitch", spb_LHipYawPitch);
257  builder->get_widget("spb_LHipPitch", spb_LHipPitch);
258  builder->get_widget("spb_LHipRoll", spb_LHipRoll);
259  builder->get_widget("spb_LKneePitch", spb_LKneePitch);
260  builder->get_widget("spb_LAnklePitch", spb_LAnklePitch);
261  builder->get_widget("spb_LAnkleRoll", spb_LAnkleRoll);
262 
263  builder->get_widget("ent_nav_x", ent_nav_x);
264  builder->get_widget("ent_nav_y", ent_nav_y);
265  builder->get_widget("ent_nav_ori", ent_nav_ori);
266  builder->get_widget("but_nav_exec", but_nav_exec);
267 
268  builder->get_widget("ent_tts", ent_tts);
269  builder->get_widget("but_tts_exec", but_tts_exec);
270  builder->get_widget("lab_tts_active", lab_tts_active);
271 
272  builder->get_widget("scl_chest_r", scl_chest_r);
273  builder->get_widget("scl_chest_g", scl_chest_g);
274  builder->get_widget("scl_chest_b", scl_chest_b);
275  builder->get_widget("scl_left_eye_r", scl_left_eye_r);
276  builder->get_widget("scl_left_eye_g", scl_left_eye_g);
277  builder->get_widget("scl_left_eye_b", scl_left_eye_b);
278  builder->get_widget("scl_right_eye_r", scl_right_eye_r);
279  builder->get_widget("scl_right_eye_g", scl_right_eye_g);
280  builder->get_widget("scl_right_eye_b", scl_right_eye_b);
281  builder->get_widget("scl_left_foot_r", scl_left_foot_r);
282  builder->get_widget("scl_left_foot_g", scl_left_foot_g);
283  builder->get_widget("scl_left_foot_b", scl_left_foot_b);
284  builder->get_widget("scl_right_foot_r", scl_right_foot_r);
285  builder->get_widget("scl_right_foot_g", scl_right_foot_g);
286  builder->get_widget("scl_right_foot_b", scl_right_foot_b);
287 
288  builder->get_widget("tb_left_ear_0", tb_left_ear_0);
289  builder->get_widget("tb_left_ear_36", tb_left_ear_36);
290  builder->get_widget("tb_left_ear_72", tb_left_ear_72);
291  builder->get_widget("tb_left_ear_108", tb_left_ear_108);
292  builder->get_widget("tb_left_ear_144", tb_left_ear_144);
293  builder->get_widget("tb_left_ear_180", tb_left_ear_180);
294  builder->get_widget("tb_left_ear_216", tb_left_ear_216);
295  builder->get_widget("tb_left_ear_252", tb_left_ear_252);
296  builder->get_widget("tb_left_ear_288", tb_left_ear_288);
297  builder->get_widget("tb_left_ear_324", tb_left_ear_324);
298 
299  builder->get_widget("tb_right_ear_0", tb_right_ear_0);
300  builder->get_widget("tb_right_ear_36", tb_right_ear_36);
301  builder->get_widget("tb_right_ear_72", tb_right_ear_72);
302  builder->get_widget("tb_right_ear_108", tb_right_ear_108);
303  builder->get_widget("tb_right_ear_144", tb_right_ear_144);
304  builder->get_widget("tb_right_ear_180", tb_right_ear_180);
305  builder->get_widget("tb_right_ear_216", tb_right_ear_216);
306  builder->get_widget("tb_right_ear_252", tb_right_ear_252);
307  builder->get_widget("tb_right_ear_288", tb_right_ear_288);
308  builder->get_widget("tb_right_ear_324", tb_right_ear_324);
309 
310  builder->get_widget("tb_control_leds", tb_control_leds);
311 
312  // required for LedInterface data callback
313  led_scales["ChestBoard/Red"] = scl_chest_r;
314  led_scales["ChestBoard/Green"] = scl_chest_g;
315  led_scales["ChestBoard/Blue"] = scl_chest_b;
316  led_scales["Face/Red/Left"] = scl_left_eye_r;
317  led_scales["Face/Green/Left"] = scl_left_eye_g;
318  led_scales["Face/Blue/Left"] = scl_left_eye_b;
319  led_scales["Face/Red/Right"] = scl_right_eye_r;
320  led_scales["Face/Green/Right"] = scl_right_eye_g;
321  led_scales["Face/Blue/Right"] = scl_right_eye_b;
322  led_scales["LFoot/Red"] = scl_left_foot_r;
323  led_scales["LFoot/Green"] = scl_left_foot_g;
324  led_scales["LFoot/Blue"] = scl_left_foot_b;
325  led_scales["RFoot/Red"] = scl_right_foot_r;
326  led_scales["RFoot/Green"] = scl_right_foot_g;
327  led_scales["RFoot/Blue"] = scl_right_foot_b;
328 
329  led_buttons["Ears/Left/0Deg"] = tb_left_ear_0;
330  led_buttons["Ears/Left/36Deg"] = tb_left_ear_36;
331  led_buttons["Ears/Left/72Deg"] = tb_left_ear_72;
332  led_buttons["Ears/Left/108Deg"] = tb_left_ear_108;
333  led_buttons["Ears/Left/144Deg"] = tb_left_ear_144;
334  led_buttons["Ears/Left/180Deg"] = tb_left_ear_180;
335  led_buttons["Ears/Left/216Deg"] = tb_left_ear_216;
336  led_buttons["Ears/Left/252Deg"] = tb_left_ear_252;
337  led_buttons["Ears/Left/288Deg"] = tb_left_ear_288;
338  led_buttons["Ears/Left/324Deg"] = tb_left_ear_324;
339 
340  led_buttons["Ears/Right/0Deg"] = tb_right_ear_0;
341  led_buttons["Ears/Right/36Deg"] = tb_right_ear_36;
342  led_buttons["Ears/Right/72Deg"] = tb_right_ear_72;
343  led_buttons["Ears/Right/108Deg"] = tb_right_ear_108;
344  led_buttons["Ears/Right/144Deg"] = tb_right_ear_144;
345  led_buttons["Ears/Right/180Deg"] = tb_right_ear_180;
346  led_buttons["Ears/Right/216Deg"] = tb_right_ear_216;
347  led_buttons["Ears/Right/252Deg"] = tb_right_ear_252;
348  led_buttons["Ears/Right/288Deg"] = tb_right_ear_288;
349  led_buttons["Ears/Right/324Deg"] = tb_right_ear_324;
350 
351 
352  builder->get_widget("but_chestbut", but_chestbut);
353  builder->get_widget("but_head_front", but_head_front);
354  builder->get_widget("but_head_middle", but_head_middle);
355  builder->get_widget("but_head_rear", but_head_rear);
356  builder->get_widget("but_lfoot_bumper", but_lfoot_bumper);
357  builder->get_widget("but_rfoot_bumper", but_rfoot_bumper);
358 
359  std::map<std::string, std::string> widget_if_map;
360  widget_if_map["Chest"] = "chestbut";
361  widget_if_map["Foot Left"] = "lfoot_bumper";
362  widget_if_map["Foot Right"] = "rfoot_bumper";
363  widget_if_map["Head Front"] = "head_front";
364  widget_if_map["Head Middle"] = "head_middle";
365  widget_if_map["Head Rear"] = "head_rear";
366 
367  std::map<std::string, std::string>::iterator m;
368  for (m = widget_if_map.begin(); m != widget_if_map.end(); ++m) {
369  ButtonLabelSet labels;
370 
371  builder->get_widget("lab_enabled_" + m->second, labels.lab_enabled);
372  builder->get_widget("lab_history_" + m->second, labels.lab_history);
373  builder->get_widget("lab_value_" + m->second, labels.lab_value);
374  builder->get_widget("lab_short_" + m->second, labels.lab_short);
375  builder->get_widget("lab_long_" + m->second, labels.lab_long);
376  builder->get_widget("lab_total_" + m->second, labels.lab_total);
377 
378  button_labels[m->first] = labels;
379  }
380 
381  cmb_kick_leg->set_active(0);
382  cmb_standup_from->set_active(0);
383  cmb_us_direction->set_active(0);
384  frm_servos->set_sensitive(false);
385  frm_sensors->set_sensitive(false);
386  frm_ultrasonic->set_sensitive(false);
387 
388  hsc_HeadYaw->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_HeadYaw, lab_HeadYaw, NaoJointPositionInterface::SERVO_head_yaw));
389  hsc_HeadPitch->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_HeadPitch, lab_HeadPitch, NaoJointPositionInterface::SERVO_head_pitch));
390  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));
391  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));
392  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));
393  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));
394  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));
395  hsc_RHand->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_RHand, lab_RHand, NaoJointPositionInterface::SERVO_r_hand));
396  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));
397  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));
398  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));
399  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));
400  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));
401  hsc_LHand->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_slider_changed), hsc_LHand, lab_LHand, NaoJointPositionInterface::SERVO_r_hand));
402  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));
403  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));
404  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));
405  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));
406  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));
407  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));
408  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));
409  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));
410  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));
411  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));
412  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));
413  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));
414  hsc_speed->signal_value_changed().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_changed_speed));
415  tb_connection->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_connection_clicked));
416  tb_stiffness->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_stiffness_clicked));
417  tb_control->signal_toggled().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_control_toggled));
418  tb_parkpos->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_goto_parkpos_clicked));
419  tb_zeroall->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_goto_zero_all_clicked));
420  tb_getup->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_get_up_clicked));
421  tb_exit->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_exit_clicked));
422  but_sv_copy->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_sv_copy_clicked));
423  but_us_auto->signal_toggled().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_us_auto_toggled));
424  but_us_emit->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_us_emit_clicked));
425  but_walkvel_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_walkvel_exec_clicked));
426  but_ws_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_ws_exec_clicked));
427  but_stop->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_motion_stop_clicked));
428  but_wsw_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_wsw_exec_clicked));
429  but_wa_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_wa_exec_clicked));
430  but_kick_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_kick_exec_clicked));
431  but_standup_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_standup_exec_clicked));
432  but_turn_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_turn_exec_clicked));
433  but_nav_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_nav_exec_clicked));
434  but_stiffness_read->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_stiffness_read_clicked));
435  but_stiffness_write->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_stiffness_write_clicked));
436  chb_stiffness_global->signal_toggled().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_stiffness_global_toggled));
437 
438  but_tts_exec->signal_clicked().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_tts_exec_clicked));
439 
440  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));
441  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));
442  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));
443  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));
444  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));
445  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));
446  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));
447  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));
448  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));
449  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));
450 
451  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));
452  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));
453  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));
454  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));
455  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));
456  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));
457  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));
458  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));
459  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));
460  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));
461 
462  scl_chest_r->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "ChestBoard/Red", scl_chest_r));
463  scl_chest_g->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "ChestBoard/Green", scl_chest_g));
464  scl_chest_b->signal_value_changed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_slider_changed), "ChestBoard/Blue", scl_chest_b));
465  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));
466  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));
467  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));
468  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));
469  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));
470  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));
471  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));
472  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));
473  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));
474  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));
475  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));
476  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));
477 
478  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));
479  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));
480  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));
481  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));
482  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));
483  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));
484  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));
485  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));
486  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));
487  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));
488  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));
489  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));
490  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));
491  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));
492  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));
493 
494  but_chestbut->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Chest"));
495  but_chestbut->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Chest"));
496  but_head_front->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Head Front"));
497  but_head_front->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Head Front"));
498  but_head_middle->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Head Middle"));
499  but_head_middle->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Head Middle"));
500  but_head_rear->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Head Rear"));
501  but_head_rear->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Head Rear"));
502  but_lfoot_bumper->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Foot Left"));
503  but_lfoot_bumper->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Foot Left"));
504  but_rfoot_bumper->signal_pressed().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_pressed), "Foot Right"));
505  but_rfoot_bumper->signal_released().connect(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_click_released), "Foot Right"));
506 
507  connection_dispatcher.signal_connected().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_connect));
508  connection_dispatcher.signal_disconnected().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_disconnect));
509 
510  tb_control_leds->signal_toggled().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_control_leds_toggled));
511 
512  on_control_toggled();
513  on_changed_speed();
514  init();
515 }
516 
517 
518 /** Destructor. */
520 {
521  on_disconnect();
522 }
523 
524 /**
525  * Sets the default values (locale dependent)
526  */
527 void
528 NaoGuiGtkWindow::init()
529 {
530  ent_walkvel_x->set_text(convert_float2str(0.5f, 1));
531  ent_walkvel_y->set_text(convert_float2str(0.f, 1));
532  ent_walkvel_theta->set_text(convert_float2str(0.f, 1));
533  ent_walkvel_speed->set_text(convert_float2str(0.5f, 1));
534 
535  ent_ws_distance->set_text(convert_float2str(0.2f, 1));
536 
537  ent_wsw_distance->set_text(convert_float2str(0.2f, 1));
538 
539  ent_wa_angle->set_text(convert_float2str(1.f, 1));
540  ent_wa_radius->set_text(convert_float2str(0.3f, 1));
541 
542  ent_turn_angle->set_text(convert_float2str(1.f, 1));
543 
544  ent_nav_x->set_text(convert_float2str(0.f, 1));
545  ent_nav_y->set_text(convert_float2str(0.f, 1));
546  ent_nav_ori->set_text(convert_float2str(0.f, 1));
547 }
548 
549 /** Update a servo value.
550  * @param hsc horizontal scale for value
551  * @param label label for value
552  * @param value servo value
553  */
554 void
555 NaoGuiGtkWindow::update_servo_value(Gtk::HScale *hsc, Gtk::Label *label, float value)
556 {
557  float f = roundf(value * 100.);
558  if ( ! tb_control->get_active() && (hsc->get_value() != f)) {
559  hsc->set_value(f);
560  }
561  // this might seem more expensive than just setting it, but the really
562  // expensive thing are screen updates, and they can be avoid much of the
563  // time
564  Glib::ustring fs = convert_float2str(value);
565  if (label->get_text() != fs) {
566  label->set_text(fs);
567  }
568 }
569 
570 
571 /** Update sensor value.
572  * @param label label for value
573  * @param value sensor value
574  * @param show_decimal true to show as decimal, false to show as integer
575  */
576 void
577 NaoGuiGtkWindow::update_sensor_value(Gtk::Label *label, float value, bool show_decimal)
578 {
579  Glib::ustring s = convert_float2str(value, show_decimal ? 2 : 0);
580  if (label->get_text() != s) {
581  label->set_text(s);
582  }
583 }
584 
585 
586 /** Update an entry value.
587  * @param ent entry field for the value
588  * @param value the value to set
589  * @param width the number of positions after decimal point
590  */
591 void
592 NaoGuiGtkWindow::update_entry_value(Gtk::Entry *ent, float value, unsigned int width)
593 {
594  if ( value != -123456 ) {
595  ent->set_text(convert_float2str(value, width));
596  } else {
597  ent->set_text("");
598  }
599 }
600 
601 
602 /** Update ultrasonic direction field.
603  * @param direction direction value from interface field
604  */
605 void
606 NaoGuiGtkWindow::update_ultrasonic_direction()
607 {
608  if ( ! sensor_if || ! sensor_if->is_valid()) return;
609 
610  switch (sensor_if->ultrasonic_direction()) {
611  case NaoSensorInterface::USD_LEFT_LEFT:
612  lab_ultrasonic_direction->set_text("l-l"); break;
613  case NaoSensorInterface::USD_LEFT_RIGHT:
614  lab_ultrasonic_direction->set_text("l-r"); break;
615  case NaoSensorInterface::USD_RIGHT_RIGHT:
616  lab_ultrasonic_direction->set_text("r-r"); break;
617  case NaoSensorInterface::USD_RIGHT_LEFT:
618  lab_ultrasonic_direction->set_text("r-l"); break;
619  case NaoSensorInterface::USD_BOTH_BOTH:
620  lab_ultrasonic_direction->set_text("b-b"); break;
621  default:
622  lab_ultrasonic_direction->set_text("none"); break;
623  }
624 }
625 
626 
627 /** Send servo message.
628  * @param hsc scale from which to take the value from
629  * @param servo the servo constant for the servo to set from the
630  * NaoJointPositionInterface
631  */
632 void
633 NaoGuiGtkWindow::send_servo_msg(Gtk::HScale *hsc, unsigned int servo)
634 {
635  if ( jointpos_if && tb_control->get_active() ) {
636  jointpos_if->read();
637 
638  if (servo == NaoJointPositionInterface::SERVO_head_pitch ||
639  servo == NaoJointPositionInterface::SERVO_head_yaw)
640  {
642  HumanoidMotionInterface::MoveHeadMessage(hsc_HeadYaw->get_value() / 100.f,
643  hsc_HeadPitch->get_value() /100.f,
644  hsc_speed->get_value() / 100.f);
645 
646  if ( rad_motion_fawkes->get_active() ) {
647  hummot_fawkes_if->msgq_enqueue(m);
648  } else {
649  hummot_naoqi_if->msgq_enqueue(m);
650  }
651  }
652  else {
655  hsc->get_value() / 100.f,
656  hsc_speed->get_value() /100.f);
657  jointpos_if->msgq_enqueue(m);
658  }
659  }
660 }
661 
662 
663 /** Event handler for slider changes.
664  * @param hsc horizontal slider that caused the event
665  * @param label label to set with the value
666  * @param servo the servo constant for the servo to set from the
667  * NaoJointPositionInterface
668  */
669 void
670 NaoGuiGtkWindow::on_slider_changed(Gtk::HScale *hsc, Gtk::Label *label,
671  unsigned int servo)
672 {
673  send_servo_msg(hsc, servo);
674 }
675 
676 
677 /** Speed change event handler. */
678 void
679 NaoGuiGtkWindow::on_changed_speed()
680 {
681  Glib::ustring s = convert_float2str(hsc_speed->get_value() / 100.f, 2);
682  lab_speed->set_text(s);
683 }
684 
685 
686 /** Update joint position values.
687  * Called whenever the NaoJointPositionInterface changes.
688  * @param force true to force an update, false to update depending
689  * on internal state
690  */
691 void
692 NaoGuiGtkWindow::update_jointpos_values(bool force)
693 {
694  if ( ! jointpos_if ) return;
695 
696  /*
697  // we take only each 10th update to decrease ffnaogui CPU usage
698  if (! force && (++update_cycle % 10 != 0)) {
699  return;
700  }
701  update_cycle = 0;
702  */
703 
704  try {
705  jointpos_if->read();
706  update_servo_value(hsc_HeadYaw, lab_HeadYaw, jointpos_if->head_yaw());
707  update_servo_value(hsc_HeadPitch, lab_HeadPitch, jointpos_if->head_pitch());
708  update_servo_value(hsc_RShoulderPitch, lab_RShoulderPitch, jointpos_if->r_shoulder_pitch());
709  update_servo_value(hsc_RShoulderRoll, lab_RShoulderRoll, jointpos_if->r_shoulder_roll());
710  update_servo_value(hsc_RElbowYaw, lab_RElbowYaw, jointpos_if->r_elbow_yaw());
711  update_servo_value(hsc_RElbowRoll, lab_RElbowRoll, jointpos_if->r_elbow_roll());
712  update_servo_value(hsc_RWristYaw, lab_RWristYaw, jointpos_if->r_wrist_yaw());
713  update_servo_value(hsc_RHand, lab_RHand, jointpos_if->r_hand());
714  update_servo_value(hsc_LShoulderPitch, lab_LShoulderPitch, jointpos_if->l_shoulder_pitch());
715  update_servo_value(hsc_LShoulderRoll, lab_LShoulderRoll, jointpos_if->l_shoulder_roll());
716  update_servo_value(hsc_LElbowYaw, lab_LElbowYaw, jointpos_if->l_elbow_yaw());
717  update_servo_value(hsc_LElbowRoll, lab_LElbowRoll, jointpos_if->l_elbow_roll());
718  update_servo_value(hsc_LWristYaw, lab_LWristYaw, jointpos_if->l_wrist_yaw());
719  update_servo_value(hsc_LHand, lab_LHand, jointpos_if->l_hand());
720  update_servo_value(hsc_RHipYawPitch, lab_RHipYawPitch, jointpos_if->r_hip_yaw_pitch());
721  update_servo_value(hsc_RHipPitch, lab_RHipPitch, jointpos_if->r_hip_pitch());
722  update_servo_value(hsc_RHipRoll, lab_RHipRoll, jointpos_if->r_hip_roll());
723  update_servo_value(hsc_RKneePitch, lab_RKneePitch, jointpos_if->r_knee_pitch());
724  update_servo_value(hsc_RAnklePitch, lab_RAnklePitch, jointpos_if->r_ankle_pitch());
725  update_servo_value(hsc_RAnkleRoll, lab_RAnkleRoll, jointpos_if->r_ankle_roll());
726  update_servo_value(hsc_LHipYawPitch, lab_LHipYawPitch, jointpos_if->l_hip_yaw_pitch());
727  update_servo_value(hsc_LHipPitch, lab_LHipPitch, jointpos_if->l_hip_pitch());
728  update_servo_value(hsc_LHipRoll, lab_LHipRoll, jointpos_if->l_hip_roll());
729  update_servo_value(hsc_LKneePitch, lab_LKneePitch, jointpos_if->l_knee_pitch());
730  update_servo_value(hsc_LAnklePitch, lab_LAnklePitch, jointpos_if->l_ankle_pitch());
731  update_servo_value(hsc_LAnkleRoll, lab_LAnkleRoll, jointpos_if->l_ankle_roll());
732 
733  bool currently_servo_enabled = servos_enabled();
734 
735  if ( currently_servo_enabled && ! servo_enabled ) {
736  tb_stiffness->set_stock_id(Gtk::Stock::YES);
737  tb_control->set_sensitive(true);
738  tb_getup->set_sensitive(true);
739  tb_parkpos->set_sensitive(true);
740  tb_zeroall->set_sensitive(true);
741  } else if (! currently_servo_enabled && servo_enabled ) {
742  tb_stiffness->set_stock_id(Gtk::Stock::NO);
743  tb_control->set_sensitive(false);
744  tb_getup->set_sensitive(false);
745  tb_parkpos->set_sensitive(false);
746  tb_zeroall->set_sensitive(false);
747  }
748  servo_enabled = currently_servo_enabled;
749  } catch (Exception &e) {
750  // ignored, happens on disconnect while events are pending
751  }
752 }
753 
754 
755 void
756 NaoGuiGtkWindow::update_tts()
757 {
758  if ( ! speechsynth_if || ! speechsynth_if->is_valid()) return;
759 
760  speechsynth_if->read();
761  if ((speechsynth_if->is_final() || (speechsynth_if->msgid() == 0)) &&
762  (lab_tts_active->get_text() != "No"))
763  {
764  lab_tts_active->set_text("No");
765 
766  } else if (! speechsynth_if->is_final() &&
767  (speechsynth_if->msgid() != 0) &&
768  (lab_tts_active->get_text() != "Yes"))
769  {
770  lab_tts_active->set_text("Yes");
771  }
772 }
773 
774 
775 /** Update sensor values.
776  * Called whenever the NaoSensorInterface changes.
777  * @param force true to force an update, false to update depending
778  * on internal state
779  */
780 void
781 NaoGuiGtkWindow::update_sensor_values(bool force)
782 {
783  if ( ! sensor_if ) return;
784 
785  try {
786  sensor_if->read();
787 
788  update_sensor_value(lab_l_fsr_fl, sensor_if->l_fsr_fl());
789  update_sensor_value(lab_l_fsr_fr, sensor_if->l_fsr_fr());
790  update_sensor_value(lab_l_fsr_rl, sensor_if->l_fsr_rl());
791  update_sensor_value(lab_l_fsr_rr, sensor_if->l_fsr_rr());
792  update_sensor_value(lab_r_fsr_fl, sensor_if->r_fsr_fl());
793  update_sensor_value(lab_r_fsr_fr, sensor_if->r_fsr_fr());
794  update_sensor_value(lab_r_fsr_rl, sensor_if->r_fsr_rl());
795  update_sensor_value(lab_r_fsr_rr, sensor_if->r_fsr_rr());
796  update_sensor_value(lab_r_total_weight, sensor_if->r_total_weight());
797  update_sensor_value(lab_l_total_weight, sensor_if->l_total_weight());
798 
799  update_sensor_value(lab_chest_button, sensor_if->chest_button(), false);
800  update_sensor_value(lab_touch_front, sensor_if->head_touch_front(), false);
801  update_sensor_value(lab_touch_middle, sensor_if->head_touch_middle(), false);
802  update_sensor_value(lab_touch_rear, sensor_if->head_touch_rear(), false);
803  update_sensor_value(lab_l_bumper_l, sensor_if->l_foot_bumper_l(), false);
804  update_sensor_value(lab_l_bumper_r, sensor_if->l_foot_bumper_r(), false);
805  update_sensor_value(lab_r_bumper_l, sensor_if->r_foot_bumper_l(), false);
806  update_sensor_value(lab_r_bumper_r, sensor_if->r_foot_bumper_r(), false);
807 
808  update_sensor_value(lab_accel_x, sensor_if->accel_x());
809  update_sensor_value(lab_accel_y, sensor_if->accel_y());
810  update_sensor_value(lab_accel_z, sensor_if->accel_z());
811  update_sensor_value(lab_gyro_x, sensor_if->gyro_x());
812  update_sensor_value(lab_gyro_y, sensor_if->gyro_y());
813  update_sensor_value(lab_gyro_ref, sensor_if->gyro_ref());
814 
815  update_sensor_value(lab_battery_charge, sensor_if->battery_charge());
816 
817  Glib::ustring l_cop =
818  "(" + convert_float2str(sensor_if->l_cop_x(), 1) + ", " +
819  convert_float2str(sensor_if->l_cop_y(), 1) + ")";
820  lab_l_cop->set_text(l_cop);
821 
822  Glib::ustring r_cop =
823  "(" + convert_float2str(sensor_if->r_cop_x(), 1) + ", " +
824  convert_float2str(sensor_if->r_cop_y(), 1) + ")";
825  lab_r_cop->set_text(r_cop);
826 
827  Glib::ustring angles_xy =
828  convert_float2str(sensor_if->angle_x(), 2) + "/" +
829  convert_float2str(sensor_if->angle_y(), 2);
830  lab_angles_xy->set_text(angles_xy);
831 
832  update_ultrasonic_direction();
833  update_sensor_value(lab_ultrasonic_left0, sensor_if->ultrasonic_distance_left(0));
834  update_sensor_value(lab_ultrasonic_left1, sensor_if->ultrasonic_distance_left(1));
835  update_sensor_value(lab_ultrasonic_left2, sensor_if->ultrasonic_distance_left(2));
836  update_sensor_value(lab_ultrasonic_left3, sensor_if->ultrasonic_distance_left(3));
837  update_sensor_value(lab_ultrasonic_right0, sensor_if->ultrasonic_distance_right(0));
838  update_sensor_value(lab_ultrasonic_right1, sensor_if->ultrasonic_distance_right(1));
839  update_sensor_value(lab_ultrasonic_right2, sensor_if->ultrasonic_distance_right(2));
840  update_sensor_value(lab_ultrasonic_right3, sensor_if->ultrasonic_distance_right(3));
841 
842  } catch (Exception &e) {
843  // ignored, happens on disconnect while events are pending
844  }
845 }
846 
847 bool
848 NaoGuiGtkWindow::servos_enabled() const
849 {
850  jointstiff_if->read();
851  return (jointstiff_if->minimum() > 0.);
852 }
853 
854 
855 void
856 NaoGuiGtkWindow::on_stiffness_clicked()
857 {
858  const float stiffness = servos_enabled() ? 0. : 1.;
861  jointstiff_if->msgq_enqueue(dmsg);
862 }
863 
864 
865 /** Control toggle event handler. */
866 void
867 NaoGuiGtkWindow::on_control_toggled()
868 {
869  bool sensitive = tb_control->get_active();
870  hsc_HeadYaw->set_sensitive(sensitive);
871  hsc_HeadPitch->set_sensitive(sensitive);
872  hsc_RShoulderPitch->set_sensitive(sensitive);
873  hsc_RShoulderRoll->set_sensitive(sensitive);
874  hsc_RElbowYaw->set_sensitive(sensitive);
875  hsc_RElbowRoll->set_sensitive(sensitive);
876  hsc_RWristYaw->set_sensitive(sensitive);
877  hsc_RHand->set_sensitive(sensitive);
878  hsc_LShoulderPitch->set_sensitive(sensitive);
879  hsc_LShoulderRoll->set_sensitive(sensitive);
880  hsc_LElbowYaw->set_sensitive(sensitive);
881  hsc_LElbowRoll->set_sensitive(sensitive);
882  hsc_LWristYaw->set_sensitive(sensitive);
883  hsc_LHand->set_sensitive(sensitive);
884  hsc_RHipYawPitch->set_sensitive(sensitive);
885  hsc_RHipPitch->set_sensitive(sensitive);
886  hsc_RHipRoll->set_sensitive(sensitive);
887  hsc_RKneePitch->set_sensitive(sensitive);
888  hsc_RAnklePitch->set_sensitive(sensitive);
889  hsc_RAnkleRoll->set_sensitive(sensitive);
890  hsc_LHipYawPitch->set_sensitive(sensitive);
891  hsc_LHipPitch->set_sensitive(sensitive);
892  hsc_LHipRoll->set_sensitive(sensitive);
893  hsc_LKneePitch->set_sensitive(sensitive);
894  hsc_LAnklePitch->set_sensitive(sensitive);
895  hsc_LAnkleRoll->set_sensitive(sensitive);
896 
897  if ( ! sensitive ) {
898  update_jointpos_values();
899  }
900 }
901 
902 
903 void
904 NaoGuiGtkWindow::on_sv_copy_clicked()
905 {
906  std::stringstream txt;
907  txt <<
908  "head_yaw = " << (hsc_HeadYaw->get_value() / 100.f) << ",\n" <<
909  "head_pitch = " << (hsc_HeadPitch->get_value() / 100.f) << ",\n" <<
910  "l_shoulder_pitch = " << (hsc_LShoulderPitch->get_value() / 100.f) << ",\n" <<
911  "l_shoulder_roll = " << (hsc_LShoulderRoll->get_value() / 100.f) << ",\n" <<
912  "l_elbow_yaw = " << (hsc_LElbowYaw->get_value() / 100.f) << ",\n" <<
913  "l_elbow_roll = " << (hsc_LElbowRoll->get_value() / 100.f) << ",\n" <<
914  "l_wrist_yaw = " << (hsc_LWristYaw->get_value() / 100.f) << ",\n" <<
915  "l_hand = " << (hsc_LHand->get_value() / 100.f) << ",\n" <<
916  "r_shoulder_pitch = " << (hsc_RShoulderPitch->get_value() / 100.f) << ",\n" <<
917  "r_shoulder_roll = " << (hsc_RShoulderRoll->get_value() / 100.f) << ",\n" <<
918  "r_elbow_yaw = " << (hsc_RElbowYaw->get_value() / 100.f) << ",\n" <<
919  "r_elbow_roll = " << (hsc_RElbowRoll->get_value() / 100.f) << ",\n" <<
920  "r_wrist_yaw = " << (hsc_RWristYaw->get_value() / 100.f) << ",\n" <<
921  "r_hand = " << (hsc_RHand->get_value() / 100.f) << ",\n" <<
922  "l_hip_yaw_pitch = " << (hsc_LHipYawPitch->get_value() / 100.f) << ",\n" <<
923  "l_hip_roll = " << (hsc_LHipRoll->get_value() / 100.f) << ",\n" <<
924  "l_hip_pitch = " << (hsc_LHipPitch->get_value() / 100.f) << ",\n" <<
925  "l_knee_pitch = " << (hsc_LKneePitch->get_value() / 100.f) << ",\n" <<
926  "l_ankle_pitch = " << (hsc_LAnklePitch->get_value() / 100.f) << ",\n" <<
927  "l_ankle_roll = " << (hsc_LAnkleRoll->get_value() / 100.f) << ",\n" <<
928  "r_hip_yaw_pitch = " << (hsc_RHipYawPitch->get_value() / 100.f) << ",\n" <<
929  "r_hip_roll = " << (hsc_RHipRoll->get_value() / 100.f) << ",\n" <<
930  "r_hip_pitch = " << (hsc_RHipPitch->get_value() / 100.f) << ",\n" <<
931  "r_knee_pitch = " << (hsc_RKneePitch->get_value() / 100.f) << ",\n" <<
932  "r_ankle_pitch = " << (hsc_RAnklePitch->get_value() / 100.f) << ",\n" <<
933  "r_ankle_roll = " << (hsc_RAnkleRoll->get_value() / 100.f);
934  Gtk::Clipboard::get()->set_text(txt.str());
935 }
936 
937 
939 string_to_usd(std::string &value)
940 {
941  if (value == "l-l") {
942  return NaoSensorInterface::USD_LEFT_LEFT;
943  } else if (value == "l-r") {
944  return NaoSensorInterface::USD_LEFT_RIGHT;
945  } else if (value == "r-l") {
946  return NaoSensorInterface::USD_RIGHT_LEFT;
947  } else if (value == "r-r") {
948  return NaoSensorInterface::USD_RIGHT_RIGHT;
949  } else if (value == "b-b") {
950  return NaoSensorInterface::USD_BOTH_BOTH;
951  }
952  return NaoSensorInterface::USD_NONE;
953 }
954 
955 void
956 NaoGuiGtkWindow::on_us_emit_clicked()
957 {
958  std::string value;
959  cmb_us_direction->get_active()->get_value(0, value);
960  NaoSensorInterface::UltrasonicDirection dirval = string_to_usd(value);
961 
964  sensor_if->msgq_enqueue(m);
965 }
966 
967 
968 void
969 NaoGuiGtkWindow::on_us_auto_toggled()
970 {
971  if (but_us_auto->get_active()) {
972  std::string value;
973  cmb_us_direction->get_active()->get_value(0, value);
974  NaoSensorInterface::UltrasonicDirection dirval = string_to_usd(value);
975 
978  sensor_if->msgq_enqueue(m);
979  } else {
982  sensor_if->msgq_enqueue(m);
983  }
984 }
985 
986 /** Event handler for connection button. */
987 void
988 NaoGuiGtkWindow::on_connection_clicked()
989 {
990  if ( ! connection_dispatcher.get_client()->connected() ) {
991  ServiceChooserDialog ssd(*this, connection_dispatcher.get_client());
992  ssd.run_and_connect();
993  } else {
994  connection_dispatcher.get_client()->disconnect();
995  }
996 }
997 
998 /** Event handler for connected event. */
999 void
1000 NaoGuiGtkWindow::on_connect()
1001 {
1002  try {
1003  bb = new RemoteBlackBoard(connection_dispatcher.get_client());
1004  jointpos_if =
1005  bb->open_for_reading<NaoJointPositionInterface>("Nao Joint Positions");
1006  jointstiff_if =
1007  bb->open_for_reading<NaoJointStiffnessInterface>("Nao Joint Stiffness");
1008  sensor_if =
1009  bb->open_for_reading<NaoSensorInterface>("Nao Sensors");
1010  nao_navi_if = bb->open_for_reading<NavigatorInterface>("Navigator");
1011  hummot_fawkes_if = bb->open_for_reading<HumanoidMotionInterface>("Nao Motion");
1012  hummot_naoqi_if =
1013  bb->open_for_reading<HumanoidMotionInterface>("NaoQi Motion");
1014  speechsynth_if = bb->open_for_reading<SpeechSynthInterface>("NaoQi TTS");
1015 
1016  std::list<fawkes::Interface *> led_ifs_l =
1017  bb->open_multiple_for_reading("LedInterface", "Nao LED *");
1018 
1019  std::list<fawkes::Interface *>::iterator li = led_ifs_l.begin();
1020  while (li != led_ifs_l.end()) {
1021  std::string id = (*li)->id();
1022  std::string::size_type last_space = id.rfind(" ");
1023  std::string device = id.substr(last_space + 1);
1024 
1025  LedInterface *led_if = dynamic_cast<LedInterface *>(*li);
1026  if (led_if == NULL) {
1027  bb->close(*li);
1028  li = led_ifs_l.erase(li);
1029  continue;
1030  } else {
1031  ++li;
1032  }
1033 
1034  on_led_data_changed(led_if);
1035  led_ifs[device] = led_if;
1036  }
1037 
1038  std::list<fawkes::Interface *> button_ifs_l =
1039  bb->open_multiple_for_reading("SwitchInterface", "Nao Button *");
1040 
1041  li = button_ifs_l.begin();
1042  while (li != button_ifs_l.end()) {
1043  std::string id = (*li)->id();
1044  std::string::size_type start = std::string("Nao Button ").size();
1045  std::string device = id.substr(start);
1046 
1047  SwitchInterface *switch_if = dynamic_cast<SwitchInterface *>(*li);
1048  if (switch_if == NULL) {
1049  bb->close(*li);
1050  li = button_ifs_l.erase(li);
1051  continue;
1052  } else {
1053  ++li;
1054  }
1055 
1056  on_button_data_changed(switch_if);
1057  button_ifs[device] = switch_if;
1058  }
1059 
1060  ifd_jointpos = new InterfaceDispatcher("NaoJointPosIfaceDisp", jointpos_if);
1061  ifd_sensor = new InterfaceDispatcher("NaoSensorIfaceDisp", sensor_if);
1062  ifd_tts = new InterfaceDispatcher("NaoTTSIfaceDisp", speechsynth_if);
1063  ifd_leds = new InterfaceDispatcher("NaoLedsIfaceDisp", led_ifs_l);
1064  ifd_buttons = new InterfaceDispatcher("NaoButtonsIfaceDisp", button_ifs_l);
1065  ifd_jointpos->signal_data_changed().connect(sigc::hide(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::update_jointpos_values), false)));
1066  ifd_sensor->signal_data_changed().connect(sigc::hide(sigc::bind(sigc::mem_fun(*this, &NaoGuiGtkWindow::update_sensor_values), false)));
1067  ifd_tts->signal_data_changed().connect(sigc::hide(sigc::mem_fun(*this, &NaoGuiGtkWindow::update_tts)));
1068  ifd_leds->signal_data_changed().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_led_data_changed));
1069  ifd_buttons->signal_data_changed().connect(sigc::mem_fun(*this, &NaoGuiGtkWindow::on_button_data_changed));
1070  bb->register_listener(ifd_jointpos, BlackBoard::BBIL_FLAG_DATA);
1071  bb->register_listener(ifd_sensor, BlackBoard::BBIL_FLAG_DATA);
1072  bb->register_listener(ifd_tts, BlackBoard::BBIL_FLAG_DATA);
1073  bb->register_listener(ifd_leds, BlackBoard::BBIL_FLAG_DATA);
1074  bb->register_listener(ifd_buttons, BlackBoard::BBIL_FLAG_DATA);
1075 
1076  tb_connection->set_stock_id(Gtk::Stock::DISCONNECT);
1077 
1078  frm_servos->set_sensitive(true);
1079  frm_sensors->set_sensitive(true);
1080  frm_ultrasonic->set_sensitive(true);
1081  tb_stiffness->set_sensitive(true);
1082  but_us_auto->set_sensitive(true);
1083  but_us_emit->set_sensitive(true);
1084  cmb_us_direction->set_sensitive(true);
1085 
1086  this->set_title(std::string("Nao GUI @ ") + connection_dispatcher.get_client()->get_hostname());
1087  } catch (Exception &e) {
1088  Glib::ustring message = *(e.begin());
1089  Gtk::MessageDialog md(*this, message, /* markup */ false,
1090  Gtk::MESSAGE_ERROR, Gtk::BUTTONS_OK,
1091  /* modal */ true);
1092  md.set_title("BlackBoard connection failed");
1093  md.run();
1094  if ( bb ) {
1095  bb->unregister_listener(ifd_jointpos);
1096  bb->unregister_listener(ifd_sensor);
1097  bb->unregister_listener(ifd_tts);
1098  bb->unregister_listener(ifd_leds);
1099  bb->unregister_listener(ifd_buttons);
1100  bb->close(jointpos_if);
1101  bb->close(jointstiff_if);
1102  bb->close(sensor_if);
1103  bb->close(nao_navi_if);
1104  bb->close(hummot_fawkes_if);
1105  bb->close(hummot_naoqi_if);
1106  bb->close(speechsynth_if);
1107  delete ifd_jointpos;
1108  delete ifd_sensor;
1109  delete ifd_tts;
1110  delete ifd_leds;
1111  delete bb;
1112  jointpos_if = NULL;
1113  jointstiff_if = NULL;
1114  sensor_if = NULL;
1115  nao_navi_if = NULL;
1116  hummot_fawkes_if = NULL;
1117  hummot_naoqi_if = NULL;
1118  speechsynth_if = NULL;
1119  bb = NULL;
1120  ifd_jointpos = NULL;
1121  ifd_sensor = NULL;
1122  ifd_tts = NULL;
1123  ifd_leds = NULL;
1124  }
1125 
1126  connection_dispatcher.get_client()->disconnect();
1127  }
1128 }
1129 
1130 /** Event handler for disconnected event. */
1131 void
1132 NaoGuiGtkWindow::on_disconnect()
1133 {
1134  if (tb_control->get_active()) tb_control->set_active(false);
1135  frm_servos->set_sensitive(false);
1136  frm_sensors->set_sensitive(false);
1137  frm_ultrasonic->set_sensitive(false);
1138  tb_stiffness->set_sensitive(false);
1139  tb_control->set_sensitive(false);
1140  tb_getup->set_sensitive(false);
1141  tb_parkpos->set_sensitive(false);
1142  tb_zeroall->set_sensitive(false);
1143  but_us_auto->set_sensitive(false);
1144  but_us_emit->set_sensitive(false);
1145  cmb_us_direction->set_sensitive(false);
1146 
1147  if (bb) {
1148  bb->unregister_listener(ifd_jointpos);
1149  bb->unregister_listener(ifd_sensor);
1150  bb->unregister_listener(ifd_tts);
1151  bb->unregister_listener(ifd_leds);
1152  bb->unregister_listener(ifd_buttons);
1153  bb->close(jointpos_if);
1154  bb->close(jointstiff_if);
1155  bb->close(sensor_if);
1156  bb->close(nao_navi_if);
1157  bb->close(hummot_fawkes_if);
1158  bb->close(hummot_naoqi_if);
1159  bb->close(speechsynth_if);
1160 
1161  std::map<std::string, fawkes::LedInterface *>::iterator li;
1162  for (li = led_ifs.begin(); li != led_ifs.end(); ++li) {
1163  bb->close(li->second);
1164  }
1165  led_ifs.clear();
1166 
1167  std::map<std::string, fawkes::SwitchInterface *>::iterator si;
1168  for (si = button_ifs.begin(); si != button_ifs.end(); ++si) {
1169  bb->close(si->second);
1170  }
1171  button_ifs.clear();
1172 
1173  delete ifd_jointpos;
1174  delete ifd_sensor;
1175  delete ifd_tts;
1176  delete ifd_leds;
1177  delete ifd_buttons;
1178  delete bb;
1179  jointpos_if = NULL;
1180  jointstiff_if = NULL;
1181  sensor_if = NULL;
1182  nao_navi_if = NULL;
1183  hummot_fawkes_if = NULL;
1184  hummot_naoqi_if = NULL;
1185  speechsynth_if = NULL;
1186  bb = NULL;
1187  ifd_jointpos = NULL;
1188  ifd_sensor = NULL;
1189  ifd_tts = NULL;
1190  ifd_leds = NULL;
1191  ifd_buttons = NULL;
1192  }
1193 
1194  tb_connection->set_stock_id(Gtk::Stock::CONNECT);
1195  if (servo_enabled) {
1196  servo_enabled = false;
1197  tb_stiffness->set_stock_id(Gtk::Stock::NO);
1198  }
1199  this->set_title("Nao GUI");
1200 }
1201 
1202 
1203 void
1204 NaoGuiGtkWindow::on_exit_clicked()
1205 {
1206  Gtk::Main::quit();
1207 }
1208 
1209 void
1210 NaoGuiGtkWindow::on_goto_parkpos_clicked()
1211 {
1212  if (hummot_naoqi_if && hummot_naoqi_if->has_writer() ) {
1215  hummot_naoqi_if->msgq_enqueue(m);
1216  }
1217 }
1218 
1219 /**
1220  * Sets all servos to zeros (calibration configuration)
1221  */
1222 void
1223 NaoGuiGtkWindow::on_goto_zero_all_clicked()
1224 {
1225  if ( jointpos_if && jointpos_if->has_writer() ) {
1226  // NaoJointPositionInterface::GotoAnglesMessage *m =
1227  // new NaoJointPositionInterface::GotoAnglesMessage(/*sec */ 3.0,
1228  // NaoHardwareInterface::INTERPOLATION_SMOOTH,
1229  // /* head */ 0., 0.,
1230  // /* l shoulder */ 0., 0.,
1231  // /* l elbow */ 0., 0.,
1232  // /* l hip */ 0., 0., 0.,
1233  // /* l knee */ 0.,
1234  // /* l ankle */ 0., 0.,
1235  // /* r hip */ 0., 0., 0.,
1236  // /* r knee */ 0.,
1237  // /* r ankle */ 0., 0.,
1238  // /* r shoulder */ 0., 0.,
1239  // /* r elbow */ 0., 0.);
1240  // jointpos_if->msgq_enqueue(m);
1241  }
1242 }
1243 
1244 void
1245 NaoGuiGtkWindow::on_get_up_clicked()
1246 {
1247  if (hummot_naoqi_if && hummot_naoqi_if->has_writer() ) {
1250  hummot_naoqi_if->msgq_enqueue(m);
1251  }
1252 }
1253 
1254 
1255 /**
1256  * Converts a float value to a Glib::ustring (locale dependent)
1257  * @param f The float value
1258  * @param width The precision width
1259  * @return the formatted string
1260  */
1261 Glib::ustring
1262 NaoGuiGtkWindow::convert_float2str(float f, unsigned int width)
1263 {
1264 #if GLIBMM_MAJOR_VERSION > 2 || ( GLIBMM_MAJOR_VERSION == 2 && GLIBMM_MINOR_VERSION >= 16 )
1265  return Glib::ustring::format(std::fixed, std::setprecision(width), f);
1266 #else
1267  std::ostringstream ss;
1268  ss << std::fixed << std::setprecision(width);
1269  ss << f;
1270 
1271  return Glib::locale_to_utf8(ss.str());
1272 #endif
1273 }
1274 
1275 
1276 
1277 bool
1278 NaoGuiGtkWindow::convert_str2float(Glib::ustring sn, float *f)
1279 {
1280  char *endptr = NULL;
1281  *f = strtof(sn.c_str(), &endptr);
1282  if ( endptr[0] != 0 ) {
1283  Glib::ustring s("Could not convert string to valid number: ");
1284  s.append(sn, 0, sn.length() - strlen(endptr));
1285  s += " &gt;&gt;&gt;<b>";
1286  s += endptr[0];
1287  s += "</b>&lt;&lt;&lt; ";
1288  s.append(endptr + 1, strlen(endptr) - 1);
1289 
1290  Gtk::MessageDialog md(*this, s,
1291  /* use markup */ true,
1292  Gtk::MESSAGE_ERROR);
1293  md.set_title("Invalid value");
1294  md.run();
1295  md.hide();
1296  return false;
1297  } else {
1298  return true;
1299  }
1300 }
1301 
1302 void
1303 NaoGuiGtkWindow::on_ws_exec_clicked()
1304 {
1305  float f;
1306  if ( convert_str2float(ent_ws_distance->get_text(), &f) ) {
1309  if ( rad_motion_fawkes->get_active() ) {
1310  hummot_fawkes_if->msgq_enqueue(m);
1311  } else {
1312  hummot_naoqi_if->msgq_enqueue(m);
1313  }
1314  }
1315 }
1316 
1317 
1318 void
1319 NaoGuiGtkWindow::on_walkvel_exec_clicked()
1320 {
1321  float x = 0., y = 0., theta = 0., speed = 0.;
1322  if ( convert_str2float(ent_walkvel_x->get_text(), &x) &&
1323  convert_str2float(ent_walkvel_y->get_text(), &y) &&
1324  convert_str2float(ent_walkvel_theta->get_text(), &theta) &&
1325  convert_str2float(ent_walkvel_speed->get_text(), &speed) )
1326  {
1328  new HumanoidMotionInterface::WalkVelocityMessage(x, y, theta, speed);
1329  if ( rad_motion_fawkes->get_active() ) {
1330  hummot_fawkes_if->msgq_enqueue(m);
1331  } else {
1332  hummot_naoqi_if->msgq_enqueue(m);
1333  }
1334  }
1335 }
1336 
1337 
1338 void
1339 NaoGuiGtkWindow::on_kick_exec_clicked()
1340 {
1341  float f;
1342  if ( convert_str2float(ent_kick_strength->get_text(), &f) ) {
1343  HumanoidMotionInterface::LegEnum leg = HumanoidMotionInterface::LEG_LEFT;
1344  if ( cmb_kick_leg->get_active_row_number() == 1 ) {
1345  leg = HumanoidMotionInterface::LEG_RIGHT;
1346  }
1349  if ( rad_motion_fawkes->get_active() ) {
1350  hummot_fawkes_if->msgq_enqueue(m);
1351  } else {
1352  hummot_naoqi_if->msgq_enqueue(m);
1353  }
1354  }
1355 }
1356 
1357 
1358 void
1359 NaoGuiGtkWindow::on_standup_exec_clicked()
1360 {
1362  HumanoidMotionInterface::STANDUP_DETECT;
1363 
1364  if (cmb_standup_from->get_active_row_number() == 1) {
1365  from = HumanoidMotionInterface::STANDUP_FRONT;
1366  } else if (cmb_standup_from->get_active_row_number() == 2) {
1367  from = HumanoidMotionInterface::STANDUP_BACK;
1368  }
1369 
1372  if ( rad_motion_fawkes->get_active() ) {
1373  hummot_fawkes_if->msgq_enqueue(m);
1374  } else {
1375  hummot_naoqi_if->msgq_enqueue(m);
1376  }
1377 }
1378 
1379 
1380 void
1381 NaoGuiGtkWindow::on_wsw_exec_clicked()
1382 {
1383  float f;
1384  if ( convert_str2float(ent_wsw_distance->get_text(), &f) ) {
1387  if ( rad_motion_fawkes->get_active() ) {
1388  hummot_fawkes_if->msgq_enqueue(m);
1389  } else {
1390  hummot_naoqi_if->msgq_enqueue(m);
1391  }
1392  }
1393 }
1394 
1395 void
1396 NaoGuiGtkWindow::on_nav_exec_clicked()
1397 {
1398  float x,y,ori;
1399  if ( convert_str2float(ent_nav_x->get_text(), &x)
1400  && convert_str2float(ent_nav_y->get_text(), &y)
1401  && convert_str2float(ent_nav_ori->get_text(), &ori) ) {
1404  nao_navi_if->msgq_enqueue(m);
1405  }
1406 }
1407 
1408 
1409 void
1410 NaoGuiGtkWindow::on_wa_exec_clicked()
1411 {
1412  float angle, radius;
1413  if ( convert_str2float(ent_wa_angle->get_text(), &angle) &&
1414  convert_str2float(ent_wa_radius->get_text(), &radius) ) {
1416  new HumanoidMotionInterface::WalkArcMessage(angle, radius);
1417  if ( rad_motion_fawkes->get_active() ) {
1418  hummot_fawkes_if->msgq_enqueue(m);
1419  } else {
1420  hummot_naoqi_if->msgq_enqueue(m);
1421  }
1422  }
1423 }
1424 
1425 
1426 void
1427 NaoGuiGtkWindow::on_turn_exec_clicked()
1428 {
1429  float f;
1430  if ( convert_str2float(ent_turn_angle->get_text(), &f) ) {
1433  if ( rad_motion_fawkes->get_active() ) {
1434  hummot_fawkes_if->msgq_enqueue(m);
1435  } else {
1436  hummot_naoqi_if->msgq_enqueue(m);
1437  }
1438  }
1439 }
1440 
1441 
1442 void
1443 NaoGuiGtkWindow::on_motion_stop_clicked()
1444 {
1447  if ( rad_motion_fawkes->get_active() ) {
1448  hummot_fawkes_if->msgq_enqueue(m);
1449  } else {
1450  hummot_naoqi_if->msgq_enqueue(m);
1451  }
1452 }
1453 
1454 
1455 void
1456 NaoGuiGtkWindow::on_stiffness_global_toggled()
1457 {
1458  bool is_active = chb_stiffness_global->get_active();
1459 
1460  spb_HeadYaw->set_sensitive(! is_active);
1461  spb_HeadPitch->set_sensitive(! is_active);
1462  spb_RShoulderPitch->set_sensitive(! is_active);
1463  spb_RShoulderRoll->set_sensitive(! is_active);
1464  spb_RElbowYaw->set_sensitive(! is_active);
1465  spb_RElbowRoll->set_sensitive(! is_active);
1466  spb_RWristYaw->set_sensitive(! is_active);
1467  spb_RHand->set_sensitive(! is_active);
1468  spb_LShoulderPitch->set_sensitive(! is_active);
1469  spb_LShoulderRoll->set_sensitive(! is_active);
1470  spb_LElbowYaw->set_sensitive(! is_active);
1471  spb_LElbowRoll->set_sensitive(! is_active);
1472  spb_LWristYaw->set_sensitive(! is_active);
1473  spb_LHand->set_sensitive(! is_active);
1474  spb_RHipYawPitch->set_sensitive(! is_active);
1475  spb_RHipPitch->set_sensitive(! is_active);
1476  spb_RHipRoll->set_sensitive(! is_active);
1477  spb_RKneePitch->set_sensitive(! is_active);
1478  spb_RAnklePitch->set_sensitive(! is_active);
1479  spb_RAnkleRoll->set_sensitive(! is_active);
1480  spb_LHipYawPitch->set_sensitive(! is_active);
1481  spb_LHipPitch->set_sensitive(! is_active);
1482  spb_LHipRoll->set_sensitive(! is_active);
1483  spb_LKneePitch->set_sensitive(! is_active);
1484  spb_LAnklePitch->set_sensitive(! is_active);
1485  spb_LAnkleRoll->set_sensitive(! is_active);
1486 
1487  spb_stiffness_global->set_sensitive(is_active);
1488 
1489  lab_stiff_HeadYaw->set_sensitive(! is_active);
1490  lab_stiff_HeadPitch->set_sensitive(! is_active);
1491  lab_stiff_RShoulderPitch->set_sensitive(! is_active);
1492  lab_stiff_RShoulderRoll->set_sensitive(! is_active);
1493  lab_stiff_RElbowYaw->set_sensitive(! is_active);
1494  lab_stiff_RElbowRoll->set_sensitive(! is_active);
1495  lab_stiff_RWristYaw->set_sensitive(! is_active);
1496  lab_stiff_RHand->set_sensitive(! is_active);
1497  lab_stiff_LShoulderPitch->set_sensitive(! is_active);
1498  lab_stiff_LShoulderRoll->set_sensitive(! is_active);
1499  lab_stiff_LElbowYaw->set_sensitive(! is_active);
1500  lab_stiff_LElbowRoll->set_sensitive(! is_active);
1501  lab_stiff_LWristYaw->set_sensitive(! is_active);
1502  lab_stiff_LHand->set_sensitive(! is_active);
1503  lab_stiff_RHipYawPitch->set_sensitive(! is_active);
1504  lab_stiff_RHipPitch->set_sensitive(! is_active);
1505  lab_stiff_RHipRoll->set_sensitive(! is_active);
1506  lab_stiff_RKneePitch->set_sensitive(! is_active);
1507  lab_stiff_RAnklePitch->set_sensitive(! is_active);
1508  lab_stiff_RAnkleRoll->set_sensitive(! is_active);
1509  lab_stiff_LHipYawPitch->set_sensitive(! is_active);
1510  lab_stiff_LHipPitch->set_sensitive(! is_active);
1511  lab_stiff_LHipRoll->set_sensitive(! is_active);
1512  lab_stiff_LKneePitch->set_sensitive(! is_active);
1513  lab_stiff_LAnklePitch->set_sensitive(! is_active);
1514  lab_stiff_LAnkleRoll->set_sensitive(! is_active);
1515 }
1516 
1517 void
1518 NaoGuiGtkWindow::on_stiffness_write_clicked()
1519 {
1520 
1521 
1522  if (chb_stiffness_global->get_active()) {
1523  float stiff_global = spb_stiffness_global->get_value();
1526  jointstiff_if->msgq_enqueue(m);
1527  } else {
1528  float stiff_head_yaw = spb_HeadYaw->get_value(),
1529  stiff_head_pitch = spb_HeadPitch->get_value(),
1530  stiff_l_shoulder_pitch = spb_LShoulderPitch->get_value(),
1531  stiff_l_shoulder_roll = spb_LShoulderRoll->get_value(),
1532  stiff_l_elbow_yaw = spb_LElbowYaw->get_value(),
1533  stiff_l_elbow_roll = spb_LElbowRoll->get_value(),
1534  stiff_l_wrist_yaw = spb_LWristYaw->get_value(),
1535  stiff_l_hand = spb_LHand->get_value(),
1536  stiff_l_hip_yaw_pitch = spb_LHipYawPitch->get_value(),
1537  stiff_l_hip_pitch = spb_LHipPitch->get_value(),
1538  stiff_l_hip_roll = spb_LHipRoll->get_value(),
1539  stiff_l_knee_pitch = spb_LKneePitch->get_value(),
1540  stiff_l_ankle_roll = spb_LAnkleRoll->get_value(),
1541  stiff_l_ankle_pitch = spb_LAnklePitch->get_value(),
1542  stiff_r_shoulder_pitch = spb_RShoulderPitch->get_value(),
1543  stiff_r_shoulder_roll = spb_RShoulderRoll->get_value(),
1544  stiff_r_elbow_yaw = spb_RElbowYaw->get_value(),
1545  stiff_r_elbow_roll = spb_RElbowRoll->get_value(),
1546  stiff_r_wrist_yaw = spb_RWristYaw->get_value(),
1547  stiff_r_hand = spb_RHand->get_value(),
1548  stiff_r_hip_yaw_pitch = spb_RHipYawPitch->get_value(),
1549  stiff_r_hippitch = spb_RHipPitch->get_value(),
1550  stiff_r_hiproll = spb_RHipRoll->get_value(),
1551  stiff_r_knee_pitch = spb_RKneePitch->get_value(),
1552  stiff_r_ankle_roll = spb_RAnkleRoll->get_value(),
1553  stiff_r_ankle_pitch = spb_RAnklePitch->get_value();
1554 
1557  (0.5,
1558  stiff_head_yaw, stiff_head_pitch,
1559  stiff_l_shoulder_pitch, stiff_l_shoulder_roll,
1560  stiff_l_elbow_yaw, stiff_l_elbow_roll,
1561  stiff_l_wrist_yaw, stiff_l_hand,
1562  stiff_l_hip_yaw_pitch,
1563  stiff_l_hip_roll, stiff_l_hip_pitch, stiff_l_knee_pitch,
1564  stiff_l_ankle_pitch, stiff_l_ankle_roll,
1565  stiff_r_shoulder_pitch, stiff_r_shoulder_roll,
1566  stiff_r_elbow_yaw, stiff_r_elbow_roll,
1567  stiff_r_wrist_yaw, stiff_r_hand,
1568  stiff_r_hip_yaw_pitch, stiff_r_hiproll, stiff_r_hippitch,
1569  stiff_r_knee_pitch, stiff_r_ankle_roll, stiff_r_ankle_pitch);
1570 
1571  jointstiff_if->msgq_enqueue(m);
1572  }
1573 }
1574 
1575 void
1576 NaoGuiGtkWindow::on_stiffness_read_clicked()
1577 {
1578  jointstiff_if->read();
1579 
1580  spb_HeadYaw->set_value(jointstiff_if->head_yaw());
1581  spb_HeadPitch->set_value(jointstiff_if->head_pitch());
1582  spb_RShoulderPitch->set_value(jointstiff_if->r_shoulder_pitch());
1583  spb_RShoulderRoll->set_value(jointstiff_if->r_shoulder_roll());
1584  spb_RElbowYaw->set_value(jointstiff_if->r_elbow_yaw());
1585  spb_RElbowRoll->set_value(jointstiff_if->r_elbow_roll());
1586  spb_RWristYaw->set_value(jointstiff_if->r_wrist_yaw());
1587  spb_RHand->set_value(jointstiff_if->r_hand());
1588  spb_LShoulderPitch->set_value(jointstiff_if->l_shoulder_pitch());
1589  spb_LShoulderRoll->set_value(jointstiff_if->l_shoulder_roll());
1590  spb_LElbowYaw->set_value(jointstiff_if->l_elbow_yaw());
1591  spb_LElbowRoll->set_value(jointstiff_if->l_elbow_roll());
1592  spb_LWristYaw->set_value(jointstiff_if->l_wrist_yaw());
1593  spb_LHand->set_value(jointstiff_if->l_hand());
1594  spb_RHipYawPitch->set_value(jointstiff_if->r_hip_yaw_pitch());
1595  spb_RHipPitch->set_value(jointstiff_if->r_hip_pitch());
1596  spb_RHipRoll->set_value(jointstiff_if->r_hip_roll());
1597  spb_RKneePitch->set_value(jointstiff_if->r_knee_pitch());
1598  spb_RAnklePitch->set_value(jointstiff_if->r_ankle_pitch());
1599  spb_RAnkleRoll->set_value(jointstiff_if->r_ankle_roll());
1600  spb_LHipYawPitch->set_value(jointstiff_if->l_hip_yaw_pitch());
1601  spb_LHipPitch->set_value(jointstiff_if->l_hip_pitch());
1602  spb_LHipRoll->set_value(jointstiff_if->l_hip_roll());
1603  spb_LKneePitch->set_value(jointstiff_if->l_knee_pitch());
1604  spb_LAnklePitch->set_value(jointstiff_if->l_ankle_pitch());
1605  spb_LAnkleRoll->set_value(jointstiff_if->l_ankle_roll());
1606 
1607  spb_stiffness_global->set_value(jointstiff_if->minimum());
1608 }
1609 
1610 void
1611 NaoGuiGtkWindow::on_tts_exec_clicked()
1612 {
1614  new SpeechSynthInterface::SayMessage(ent_tts->get_text().c_str());
1615  speechsynth_if->msgq_enqueue(m);
1616 }
1617 
1618 
1619 void
1620 NaoGuiGtkWindow::on_led_tb_toggled(std::string iface_id,
1621  Gtk::ToggleButton *tb)
1622 {
1623  if ( (led_ifs.find(iface_id) != led_ifs.end()) && led_ifs[iface_id]->is_valid())
1624  {
1625  try {
1626  if (tb->get_active()) {
1627  led_ifs[iface_id]->msgq_enqueue(new LedInterface::TurnOnMessage());
1628  } else {
1629  led_ifs[iface_id]->msgq_enqueue(new LedInterface::TurnOffMessage());
1630  }
1631  } catch (Exception &e) {
1632  e.print_trace();
1633  }
1634  }
1635 }
1636 
1637 void
1638 NaoGuiGtkWindow::on_control_leds_toggled()
1639 {
1640  bool is_active = tb_control_leds->get_active();
1641 
1642  scl_chest_r->set_sensitive(is_active);
1643  scl_chest_g->set_sensitive(is_active);
1644  scl_chest_b->set_sensitive(is_active);
1645  scl_left_eye_r->set_sensitive(is_active);
1646  scl_left_eye_g->set_sensitive(is_active);
1647  scl_left_eye_b->set_sensitive(is_active);
1648  scl_right_eye_r->set_sensitive(is_active);
1649  scl_right_eye_g->set_sensitive(is_active);
1650  scl_right_eye_b->set_sensitive(is_active);
1651  scl_left_foot_r->set_sensitive(is_active);
1652  scl_left_foot_g->set_sensitive(is_active);
1653  scl_left_foot_b->set_sensitive(is_active);
1654  scl_right_foot_r->set_sensitive(is_active);
1655  scl_right_foot_g->set_sensitive(is_active);
1656  scl_right_foot_b->set_sensitive(is_active);
1657 
1658  tb_left_ear_0->set_sensitive(is_active);
1659  tb_left_ear_36->set_sensitive(is_active);
1660  tb_left_ear_72->set_sensitive(is_active);
1661  tb_left_ear_108->set_sensitive(is_active);
1662  tb_left_ear_144->set_sensitive(is_active);
1663  tb_left_ear_180->set_sensitive(is_active);
1664  tb_left_ear_216->set_sensitive(is_active);
1665  tb_left_ear_252->set_sensitive(is_active);
1666  tb_left_ear_288->set_sensitive(is_active);
1667  tb_left_ear_324->set_sensitive(is_active);
1668 
1669  tb_right_ear_0->set_sensitive(is_active);
1670  tb_right_ear_36->set_sensitive(is_active);
1671  tb_right_ear_72->set_sensitive(is_active);
1672  tb_right_ear_108->set_sensitive(is_active);
1673  tb_right_ear_144->set_sensitive(is_active);
1674  tb_right_ear_180->set_sensitive(is_active);
1675  tb_right_ear_216->set_sensitive(is_active);
1676  tb_right_ear_252->set_sensitive(is_active);
1677  tb_right_ear_288->set_sensitive(is_active);
1678  tb_right_ear_324->set_sensitive(is_active);
1679 }
1680 
1681 
1682 void
1683 NaoGuiGtkWindow::on_led_slider_changed(std::string iface_id, Gtk::Scale *scl)
1684 {
1685  if ( (led_ifs.find(iface_id) != led_ifs.end()) && led_ifs[iface_id]->is_valid())
1686  {
1687  try {
1689  new LedInterface::SetIntensityMessage(0., scl->get_value());
1690  led_ifs[iface_id]->msgq_enqueue(sim);
1691  } catch (Exception &e) {
1692  e.print_trace();
1693  }
1694  }
1695 }
1696 
1697 
1698 bool
1699 NaoGuiGtkWindow::on_led_slider_button_release(GdkEventButton *event,
1700  std::string iface_id,
1701  Gtk::Scale *scl)
1702 {
1703  if ( (led_ifs.find(iface_id) != led_ifs.end()) && led_ifs[iface_id]->is_valid())
1704  {
1705  on_led_data_changed(led_ifs[iface_id]);
1706  }
1707  return true;
1708 }
1709 
1710 void
1711 NaoGuiGtkWindow::on_led_data_changed(fawkes::Interface *iface)
1712 {
1713  if (tb_control_leds->get_active()) return;
1714 
1715  std::string id = iface->id();
1716  std::string::size_type last_space = id.rfind(" ");
1717  std::string device = id.substr(last_space + 1);
1718 
1719  LedInterface *led_if = dynamic_cast<LedInterface *>(iface);
1720  if ((led_if != NULL) && (led_scales.find(device) != led_scales.end())) {
1721  try {
1722  led_if->read();
1723  led_scales[device]->set_value(led_if->intensity());
1724  } catch (Exception &e) {}
1725  }
1726 
1727  if ((led_if != NULL) && (led_buttons.find(device) != led_buttons.end())) {
1728  try {
1729  led_if->read();
1730  led_buttons[device]->set_active(led_if->intensity() >= .5);
1731  } catch (Exception &e) {}
1732  }
1733 }
1734 
1735 
1736 void
1737 NaoGuiGtkWindow::on_button_click_pressed(std::string iface_id)
1738 {
1739  if (button_ifs.find(iface_id) != button_ifs.end()) {
1740  try {
1743  button_ifs[iface_id]->msgq_enqueue(msg);
1744  } catch (Exception &e) {}
1745  }
1746 }
1747 
1748 void
1749 NaoGuiGtkWindow::on_button_click_released(std::string iface_id)
1750 {
1751  if (button_ifs.find(iface_id) != button_ifs.end()) {
1752  try {
1755  button_ifs[iface_id]->msgq_enqueue(msg);
1756  } catch (Exception &e) {}
1757  }
1758 }
1759 
1760 void
1761 NaoGuiGtkWindow::on_button_data_changed(fawkes::Interface *iface)
1762 {
1763  std::string id = iface->id();
1764  std::string::size_type start = std::string("Nao Button ").size();
1765  std::string device = id.substr(start);
1766 
1767  SwitchInterface *switch_if = dynamic_cast<SwitchInterface *>(iface);
1768  if ((switch_if != NULL) && (button_labels.find(device) != button_labels.end())) {
1769  try {
1770  switch_if->read();
1771  ButtonLabelSet &l = button_labels[device];
1772  if (switch_if->is_enabled() && l.lab_enabled->get_text() == "No") {
1773  l.lab_enabled->set_text("Yes");
1774  }
1775  if (! switch_if->is_enabled() && l.lab_enabled->get_text() == "Yes") {
1776  l.lab_enabled->set_text("No");
1777  }
1778 
1779  Glib::ustring s;
1780  s = convert_float2str(switch_if->history(), 3);
1781  if (l.lab_history->get_text() != s) l.lab_history->set_text(s);
1782 
1783  s = convert_float2str(switch_if->value(), 1);
1784  if (l.lab_value->get_text() != s) l.lab_value->set_text(s);
1785 
1786  s = convert_float2str(switch_if->short_activations(), 0);
1787  if (l.lab_short->get_text() != s) l.lab_short->set_text(s);
1788 
1789  s = convert_float2str(switch_if->long_activations(), 0);
1790  if (l.lab_long->get_text() != s) l.lab_long->set_text(s);
1791 
1792  s = convert_float2str(switch_if->activation_count(), 0);
1793  if (l.lab_total->get_text() != s) l.lab_total->set_text(s);
1794  } catch (Exception &e) {}
1795  } else {
1796  printf("Could not find %s\n", device.c_str());
1797  }
1798 }
1799 
TurnOffMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:106
uint8_t r_foot_bumper_r() const
Get r_foot_bumper_r value.
float head_pitch() const
Get head_pitch value.
NaoJointPositionInterface Fawkes BlackBoard Interface.
sigc::signal< void > signal_disconnected()
Get "disconnected" signal.
sigc::signal< void > signal_connected()
Get "connected" signal.
StandupMessage Fawkes BlackBoard Interface Message.
TurnMessage Fawkes BlackBoard Interface Message.
uint32_t msgid() const
Get msgid value.
float l_knee_pitch() const
Get l_knee_pitch value.
KickMessage Fawkes BlackBoard Interface Message.
StopUltrasonicMessage Fawkes BlackBoard Interface Message.
float l_shoulder_pitch() const
Get l_shoulder_pitch value.
uint8_t l_foot_bumper_l() const
Get l_foot_bumper_l value.
float r_elbow_yaw() const
Get r_elbow_yaw value.
float l_hip_pitch() const
Get l_hip_pitch value.
float r_fsr_fr() const
Get r_fsr_fr value.
Fawkes library namespace.
float r_shoulder_roll() const
Get r_shoulder_roll value.
float l_fsr_rl() const
Get l_fsr_rl value.
float l_wrist_yaw() const
Get l_wrist_yaw value.
void disconnect()
Disconnect socket.
Definition: client.cpp:529
float l_fsr_fl() const
Get l_fsr_fl value.
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
uint32_t short_activations() const
Get short_activations value.
float l_fsr_rr() const
Get l_fsr_rr value.
uint32_t activation_count() const
Get activation_count value.
float accel_x() const
Get accel_x value.
StartUltrasonicMessage Fawkes BlackBoard Interface Message.
float head_yaw() const
Get head_yaw value.
WalkVelocityMessage Fawkes BlackBoard Interface Message.
float head_yaw() const
Get head_yaw value.
float l_hip_yaw_pitch() const
Get l_hip_yaw_pitch value.
UltrasonicDirection ultrasonic_direction() const
Get ultrasonic_direction value.
float l_elbow_yaw() const
Get l_elbow_yaw value.
uint8_t l_foot_bumper_r() const
Get l_foot_bumper_r value.
SayMessage Fawkes BlackBoard Interface Message.
SetBodyStiffnessMessage Fawkes BlackBoard Interface Message.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:218
NaoSensorInterface Fawkes BlackBoard Interface.
float r_elbow_yaw() const
Get r_elbow_yaw value.
GetUpMessage Fawkes BlackBoard Interface Message.
uint32_t long_activations() const
Get long_activations value.
StandupEnum
From which position to standup.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
float r_hand() const
Get r_hand value.
NaoJointStiffnessInterface Fawkes BlackBoard Interface.
float * ultrasonic_distance_right() const
Get ultrasonic_distance_right value.
float minimum() const
Get minimum value.
float angle_x() const
Get angle_x value.
bool is_valid() const
Check validity of interface.
Definition: interface.cpp:466
float r_ankle_pitch() const
Get r_ankle_pitch value.
float r_hip_roll() const
Get r_hip_roll value.
float l_elbow_yaw() const
Get l_elbow_yaw value.
float * ultrasonic_distance_left() const
Get ultrasonic_distance_left value.
NaoGuiGtkWindow(BaseObjectType *cobject, const Glib::RefPtr< Gtk::Builder > &builder)
Constructor.
Definition: naogui.cpp:61
float r_shoulder_pitch() const
Get r_shoulder_pitch value.
float l_hip_pitch() const
Get l_hip_pitch value.
LegEnum
Type to determinate leg side.
float value() const
Get value value.
float l_wrist_yaw() const
Get l_wrist_yaw value.
float r_ankle_roll() const
Get r_ankle_roll value.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:190
float r_fsr_fl() const
Get r_fsr_fl value.
SwitchInterface Fawkes BlackBoard Interface.
WalkArcMessage Fawkes BlackBoard Interface Message.
float r_knee_pitch() const
Get r_knee_pitch value.
float intensity() const
Get intensity value.
UltrasonicDirection
This determines the chosen sender/receiver.
float head_pitch() const
Get head_pitch value.
float r_elbow_roll() const
Get r_elbow_roll value.
HumanoidMotionInterface Fawkes BlackBoard Interface.
float l_fsr_fr() const
Get l_fsr_fr value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
float r_wrist_yaw() const
Get r_wrist_yaw value.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
float gyro_ref() const
Get gyro_ref value.
float r_cop_y() const
Get r_cop_y value.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float l_ankle_roll() const
Get l_ankle_roll value.
float l_elbow_roll() const
Get l_elbow_roll value.
float accel_y() const
Get accel_y value.
void run_and_connect()
Run dialog and try to connect.
float r_wrist_yaw() const
Get r_wrist_yaw value.
float r_hip_yaw_pitch() const
Get r_hip_yaw_pitch value.
float accel_z() const
Get accel_z value.
bool is_final() const
Get final value.
float l_shoulder_roll() const
Get l_shoulder_roll value.
uint8_t chest_button() const
Get chest_button value.
float l_ankle_pitch() const
Get l_ankle_pitch value.
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
DisableSwitchMessage Fawkes BlackBoard Interface Message.
SetIntensityMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:55
iterator begin()
Get iterator for messages.
Definition: exception.cpp:700
float history() const
Get history value.
float r_knee_pitch() const
Get r_knee_pitch value.
SetStiffnessesMessage Fawkes BlackBoard Interface Message.
float l_cop_y() const
Get l_cop_y value.
float angle_y() const
Get angle_y value.
bool is_enabled() const
Get enabled value.
WalkSidewaysMessage Fawkes BlackBoard Interface Message.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*", const char *owner=NULL)=0
Open multiple interfaces for reading.
float gyro_y() const
Get gyro_y value.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
float r_fsr_rl() const
Get r_fsr_rl value.
float l_knee_pitch() const
Get l_knee_pitch value.
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
float l_ankle_roll() const
Get l_ankle_roll value.
float r_ankle_roll() const
Get r_ankle_roll value.
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:33
EnableSwitchMessage Fawkes BlackBoard Interface Message.
float r_hip_pitch() const
Get r_hip_pitch value.
float l_shoulder_pitch() const
Get l_shoulder_pitch value.
float l_total_weight() const
Get l_total_weight value.
float r_hip_pitch() const
Get r_hip_pitch value.
float l_elbow_roll() const
Get l_elbow_roll value.
Remote BlackBoard.
Definition: remote.h:48
float r_shoulder_roll() const
Get r_shoulder_roll value.
TurnOnMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:86
float r_cop_x() const
Get r_cop_x value.
ParkMessage Fawkes BlackBoard Interface Message.
float gyro_x() const
Get gyro_x value.
WalkStraightMessage Fawkes BlackBoard Interface Message.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
float l_ankle_pitch() const
Get l_ankle_pitch value.
uint8_t head_touch_rear() const
Get head_touch_rear value.
float r_hip_roll() const
Get r_hip_roll value.
float r_elbow_roll() const
Get r_elbow_roll value.
uint8_t r_foot_bumper_l() const
Get r_foot_bumper_l value.
MoveServoMessage Fawkes BlackBoard Interface Message.
const char * get_hostname() const
Get the client&#39;s hostname.
Definition: client.cpp:856
float r_hip_yaw_pitch() const
Get r_hip_yaw_pitch value.
EmitUltrasonicWaveMessage Fawkes BlackBoard Interface Message.
float l_hip_roll() const
Get l_hip_roll value.
MoveHeadMessage Fawkes BlackBoard Interface Message.
bool connected() const
Check if connection is alive.
Definition: client.cpp:823
~NaoGuiGtkWindow()
Destructor.
Definition: naogui.cpp:519
StopMessage Fawkes BlackBoard Interface Message.
FawkesNetworkClient * get_client()
Get client.
float battery_charge() const
Get battery_charge value.
float r_shoulder_pitch() const
Get r_shoulder_pitch value.
float r_ankle_pitch() const
Get r_ankle_pitch value.
uint8_t head_touch_front() const
Get head_touch_front value.
float l_cop_x() const
Get l_cop_x value.
float l_shoulder_roll() const
Get l_shoulder_roll value.
float r_fsr_rr() const
Get r_fsr_rr value.
Interface listener with dispatcher.
float r_total_weight() const
Get r_total_weight value.
float l_hip_yaw_pitch() const
Get l_hip_yaw_pitch value.
SpeechSynthInterface Fawkes BlackBoard Interface.
sigc::signal< void, Interface * > signal_data_changed()
Get "data changed" signal.
uint8_t head_touch_middle() const
Get head_touch_middle value.
float l_hip_roll() const
Get l_hip_roll value.
NavigatorInterface Fawkes BlackBoard Interface.
float l_hand() const
Get l_hand value.
virtual void close(Interface *interface)=0
Close interface.