Fawkes API  Fawkes Development Version
button_thread.cpp
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 }