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