Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * button_thread.cpp - Provide Nao buttons to Fawkes 00004 * 00005 * Created: Mon Aug 15 11:02:49 2011 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "button_thread.h" 00024 00025 #include <alproxies/allauncherproxy.h> 00026 #include <alproxies/dcmproxy.h> 00027 #include <alproxies/alaudioplayerproxy.h> 00028 #include <alproxies/almemoryproxy.h> 00029 #include <alproxies/alsentinelproxy.h> 00030 #include <alcore/alerror.h> 00031 #include <almemoryfastaccess/almemoryfastaccess.h> 00032 00033 #include <interfaces/NaoSensorInterface.h> 00034 #include <interfaces/SwitchInterface.h> 00035 00036 #include <boost/bind.hpp> 00037 #include <cstring> 00038 #include <cerrno> 00039 #include <sys/stat.h> 00040 00041 using namespace fawkes; 00042 00043 #define POWEROFF_PATH "/sbin/poweroff" 00044 00045 00046 /** @class NaoQiButtonThread "dcm_thread.h" 00047 * Thread to provide buttons to Fawkes. 00048 * This thread reads the sensors data from the DCM thread and 00049 * processes the included button data. From that it derives short and 00050 * long activations as well as a basic pattern (three times long 00051 * press) to turn off the robot. It also plays audio samples based on 00052 * the activations. 00053 * 00054 * @author Tim Niemueller 00055 */ 00056 00057 /** Constructor. */ 00058 NaoQiButtonThread::NaoQiButtonThread() 00059 : Thread("NaoQiButtonThread", Thread::OPMODE_WAITFORWAKEUP), 00060 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS) 00061 { 00062 } 00063 00064 00065 /** Destructor. */ 00066 NaoQiButtonThread::~NaoQiButtonThread() 00067 { 00068 } 00069 00070 00071 void 00072 NaoQiButtonThread::init() 00073 { 00074 __sound_longpling = __sound_pling = -1; 00075 __sound_bumper_left = __sound_bumper_right = -1; 00076 last_shutdown_actcount = 0; 00077 00078 __cfg_chest_triple_long_click_shutdown = false; 00079 try { 00080 __cfg_chest_triple_long_click_shutdown = 00081 config->get_bool("/hardware/nao/chestbut_triple_long_click_shutdown"); 00082 } catch (Exception &e) {} // ignored 00083 00084 // Is the audio player loaded? 00085 try { 00086 AL::ALPtr<AL::ALLauncherProxy> launcher(new AL::ALLauncherProxy(naoqi_broker)); 00087 bool is_auplayer_available = launcher->isModulePresent("ALAudioPlayer"); 00088 bool is_alsentinel_available = launcher->isModulePresent("ALSentinel"); 00089 00090 if (! is_auplayer_available) { 00091 logger->log_warn(name(), "ALAudioPlayer not available, disabling sounds"); 00092 } else { 00093 __auplayer = 00094 AL::ALPtr<AL::ALAudioPlayerProxy>(new AL::ALAudioPlayerProxy(naoqi_broker)); 00095 __sound_longpling = __auplayer->loadFile(RESDIR"/sounds/longpling.wav"); 00096 __sound_pling = __auplayer->loadFile(RESDIR"/sounds/pling.wav"); 00097 __sound_bumper_left = 00098 __auplayer->loadFile(RESDIR"/sounds/metal_click_1_left.wav"); 00099 __sound_bumper_right = 00100 __auplayer->loadFile(RESDIR"/sounds/metal_click_1_right.wav"); 00101 } 00102 00103 if (is_alsentinel_available) { 00104 logger->log_warn(name(), "ALSentinel loaded, disabling its button handling"); 00105 AL::ALPtr<AL::ALSentinelProxy> 00106 alsentinel(new AL::ALSentinelProxy(naoqi_broker)); 00107 alsentinel->enableDefaultActionSimpleClick(false); 00108 alsentinel->enableDefaultActionDoubleClick(false); 00109 alsentinel->enableDefaultActionTripleClick(false); 00110 } 00111 } catch (AL::ALError& e) { 00112 throw Exception("Checking module availability failed: %s", 00113 e.toString().c_str()); 00114 } 00115 00116 __sensor_if = 00117 blackboard->open_for_reading<NaoSensorInterface>("Nao Sensors"); 00118 00119 __chestbut_if = 00120 blackboard->open_for_writing<SwitchInterface>("Nao Button Chest"); 00121 __lfoot_bumper_if = 00122 blackboard->open_for_writing<SwitchInterface>("Nao Button Foot Left"); 00123 __rfoot_bumper_if = 00124 blackboard->open_for_writing<SwitchInterface>("Nao Button Foot Right"); 00125 __head_front_if = 00126 blackboard->open_for_writing<SwitchInterface>("Nao Button Head Front"); 00127 __head_middle_if = 00128 blackboard->open_for_writing<SwitchInterface>("Nao Button Head Middle"); 00129 __head_rear_if = 00130 blackboard->open_for_writing<SwitchInterface>("Nao Button Head Rear"); 00131 00132 __chestbut_if->resize_buffers(1); 00133 __lfoot_bumper_if->resize_buffers(1); 00134 __rfoot_bumper_if->resize_buffers(1); 00135 __head_front_if->resize_buffers(1); 00136 __head_middle_if->resize_buffers(1); 00137 __head_rear_if->resize_buffers(1); 00138 00139 __chestbut_remote_enabled = false; 00140 __lfoot_bumper_remote_enabled = __rfoot_bumper_remote_enabled = false; 00141 __head_front_remote_enabled = __head_middle_remote_enabled = 00142 __head_rear_remote_enabled = false; 00143 00144 now.set_clock(clock); 00145 last.set_clock(clock); 00146 now.stamp(); 00147 last.stamp(); 00148 } 00149 00150 00151 void 00152 NaoQiButtonThread::finalize() 00153 { 00154 blackboard->close(__chestbut_if); 00155 blackboard->close(__lfoot_bumper_if); 00156 blackboard->close(__rfoot_bumper_if); 00157 blackboard->close(__head_front_if); 00158 blackboard->close(__head_middle_if); 00159 blackboard->close(__head_rear_if); 00160 blackboard->close(__sensor_if); 00161 __chestbut_if = NULL; 00162 __lfoot_bumper_if = NULL; 00163 __rfoot_bumper_if = NULL; 00164 __head_front_if = NULL; 00165 __head_middle_if = NULL; 00166 __head_rear_if = NULL; 00167 __sensor_if = NULL; 00168 00169 if (__auplayer) { 00170 __auplayer->unloadFile(__sound_longpling); 00171 __auplayer->unloadFile(__sound_pling); 00172 __auplayer->unloadFile(__sound_bumper_left); 00173 __auplayer->unloadFile(__sound_bumper_right); 00174 __auplayer.reset(); 00175 } 00176 } 00177 00178 00179 00180 void 00181 NaoQiButtonThread::loop() 00182 { 00183 now.stamp(); 00184 float time_diff_sec = now - &last; 00185 last = now; 00186 00187 __sensor_if->read(); 00188 00189 process_pattern_button(__chestbut_if, __sensor_if->chest_button(), 00190 time_diff_sec, __chestbut_remote_enabled, 00191 __sound_pling, __sound_longpling); 00192 process_pattern_button(__head_front_if, __sensor_if->head_touch_front(), 00193 time_diff_sec, __head_front_remote_enabled); 00194 process_pattern_button(__head_middle_if, __sensor_if->head_touch_middle(), 00195 time_diff_sec, __head_middle_remote_enabled); 00196 process_pattern_button(__head_rear_if, __sensor_if->head_touch_rear(), 00197 time_diff_sec, __head_rear_remote_enabled); 00198 00199 process_bumpers(__lfoot_bumper_if, __sensor_if->l_foot_bumper_l(), 00200 __sensor_if->l_foot_bumper_r(), time_diff_sec, 00201 __lfoot_bumper_remote_enabled, __sound_bumper_left); 00202 00203 process_bumpers(__rfoot_bumper_if, __sensor_if->r_foot_bumper_l(), 00204 __sensor_if->r_foot_bumper_r(), time_diff_sec, 00205 __rfoot_bumper_remote_enabled, __sound_bumper_right); 00206 00207 if (__cfg_chest_triple_long_click_shutdown && 00208 __chestbut_if->long_activations() == 3 && 00209 __chestbut_if->activation_count() != last_shutdown_actcount) 00210 { 00211 logger->log_debug(name(), "Shutting down"); 00212 last_shutdown_actcount = __chestbut_if->activation_count(); 00213 if (__auplayer) __auplayer->playFile(RESDIR"/sounds/naoshutdown.wav"); 00214 00215 struct stat s; 00216 if (stat(POWEROFF_PATH, &s) == -1) { 00217 logger->log_error(name(), "Cannot stat '%s': %s", POWEROFF_PATH, 00218 strerror(errno)); 00219 } else { 00220 if (s.st_mode & S_ISUID) { 00221 int rv = system(POWEROFF_PATH); 00222 if (rv == -1 || (WEXITSTATUS(rv) != 0)) { 00223 logger->log_error(name(), "Failed to execute shutdown command"); 00224 } 00225 } else { 00226 logger->log_error(name(), "SetUID bit on '%s' not set, cannot shutdown.", 00227 POWEROFF_PATH); 00228 } 00229 } 00230 } 00231 } 00232 00233 00234 void 00235 NaoQiButtonThread::set_interface(SwitchInterface *switch_if, 00236 bool enabled, float value, float history, 00237 unsigned int activations, 00238 unsigned int short_act, unsigned int long_act) 00239 { 00240 switch_if->copy_shared_to_buffer(0); 00241 00242 switch_if->set_enabled(enabled); 00243 switch_if->set_value(value); 00244 switch_if->set_history(history); 00245 switch_if->set_activation_count(activations); 00246 switch_if->set_short_activations(short_act); 00247 switch_if->set_long_activations(long_act); 00248 00249 if (switch_if->compare_buffers(0) != 0) switch_if->write(); 00250 } 00251 00252 00253 void 00254 NaoQiButtonThread::process_pattern_button(SwitchInterface *switch_if, 00255 float sensor_value, float time_diff_sec, 00256 bool &remote_enabled, 00257 int sound_short, int sound_long) 00258 { 00259 float value = 0; 00260 process_messages(switch_if, remote_enabled, value); 00261 value = std::max(value, sensor_value); 00262 00263 bool enabled = false; 00264 float history = switch_if->history(); 00265 unsigned int activations = switch_if->activation_count(); 00266 unsigned int short_act = switch_if->short_activations(); 00267 unsigned int long_act = switch_if->long_activations(); 00268 00269 pattern_button_logic(value, time_diff_sec, enabled, history, 00270 activations, short_act, long_act, 00271 sound_short, sound_long); 00272 00273 set_interface(switch_if, enabled, value, history, 00274 activations, short_act, long_act); 00275 } 00276 00277 00278 void 00279 NaoQiButtonThread::process_bumpers(SwitchInterface *switch_if, 00280 float left_value, float right_value, 00281 float time_diff_sec, 00282 bool &remote_enabled, int sound_id) 00283 { 00284 float value = 0; 00285 process_messages(switch_if, remote_enabled, value); 00286 value = std::max(std::max(value, left_value), right_value); 00287 00288 bool enabled = false; 00289 float history = switch_if->history(); 00290 unsigned int activations = switch_if->activation_count(); 00291 unsigned int short_act = switch_if->short_activations(); 00292 unsigned int long_act = switch_if->long_activations(); 00293 00294 bumpers_logic(value, time_diff_sec, enabled, history, activations, sound_id); 00295 00296 set_interface(switch_if, enabled, value, history, 00297 activations, short_act, long_act); 00298 } 00299 00300 00301 00302 void 00303 NaoQiButtonThread::process_messages(SwitchInterface *switch_if, 00304 bool &remote_enabled, float &value) 00305 { 00306 while ( ! switch_if->msgq_empty() ) { 00307 00308 if (SwitchInterface::SetMessage *msg = switch_if->msgq_first_safe(msg)) { 00309 if (msg->is_enabled()) { 00310 value = std::min(0.5f, msg->value()); 00311 } else { 00312 value = std::max(0.49f, msg->value()); 00313 } 00314 00315 } else if (SwitchInterface::EnableSwitchMessage *msg = 00316 switch_if->msgq_first_safe(msg)) 00317 { 00318 logger->log_debug(name(), "Got ENable switch message for %s", 00319 switch_if->id()); 00320 value = 1.; 00321 remote_enabled = true; 00322 00323 } else if (SwitchInterface::DisableSwitchMessage *msg = 00324 switch_if->msgq_first_safe(msg)) 00325 { 00326 logger->log_debug(name(), "Got DISable switch message for %s", 00327 switch_if->id()); 00328 remote_enabled = false; 00329 value = 0.; 00330 } 00331 00332 switch_if->msgq_pop(); 00333 } 00334 00335 if (remote_enabled) value = 1.; 00336 } 00337 00338 00339 void 00340 NaoQiButtonThread::pattern_button_logic(float value, float time_diff_sec, 00341 bool &enabled, float &history, 00342 unsigned int &activations, 00343 unsigned int &short_act, 00344 unsigned int &long_act, 00345 int sound_short, int sound_long) 00346 { 00347 if (value < 0.5) { // button released or not pressed 00348 if (history > 0.025 /* sec */) { // released 00349 ++activations; 00350 if (history > 0.5) { 00351 ++long_act; 00352 if (__auplayer && (sound_long != -1)) __auplayer->play(sound_long); 00353 } else { 00354 ++short_act; 00355 if (__auplayer && (sound_short != -1)) __auplayer->play(sound_short); 00356 } 00357 } else if ( history < -2.0 /* sec */ ) { 00358 // reset after two seconds 00359 short_act = long_act = 0; 00360 } 00361 00362 enabled = false; 00363 if ( history < 0. ) { 00364 history -= time_diff_sec; 00365 } else { 00366 history = - time_diff_sec; 00367 } 00368 00369 } else { // at least one is enabled 00370 enabled = true; 00371 if ( history > 0. ) { 00372 history += time_diff_sec; 00373 } else { 00374 history = time_diff_sec; 00375 } 00376 } 00377 00378 // stop after two minutes, nobody cares after that 00379 if (history < -120.) { 00380 history = -120.; 00381 } else if (history > 120.) { 00382 history = 120.; 00383 } 00384 } 00385 00386 00387 void 00388 NaoQiButtonThread::bumpers_logic(float value, float time_diff_sec, 00389 bool &enabled, float &history, 00390 unsigned int &activations, int sound_id) 00391 { 00392 if (value < 0.5) { // button released or none pressed 00393 enabled = false; 00394 if ( history < 0. ) { 00395 history -= time_diff_sec; 00396 } else { 00397 history = - time_diff_sec; 00398 } 00399 00400 } else { // at least one is enabled 00401 if (history <= 0. /* sec */) { // pressed 00402 if (__auplayer && (sound_id != -1)) __auplayer->play(sound_id); 00403 ++activations; 00404 } 00405 00406 enabled = true; 00407 if ( history > 0. ) { 00408 history += time_diff_sec; 00409 } else { 00410 history = time_diff_sec; 00411 } 00412 } 00413 00414 // stop after two minutes, nobody cares after that 00415 if (history < -120.) { 00416 history = -120.; 00417 } else if (history > 120.) { 00418 history = 120.; 00419 } 00420 }