Fawkes API  Fawkes Development Version
button_thread.cpp
1 
2 /***************************************************************************
3  * button_thread.cpp - Provide Nao buttons to Fawkes
4  *
5  * Created: Mon Aug 15 11:02:49 2011
6  * Copyright 2006-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 "button_thread.h"
24 
25 #include <alcore/alerror.h>
26 #include <almemoryfastaccess/almemoryfastaccess.h>
27 #include <alproxies/alaudioplayerproxy.h>
28 #include <alproxies/allauncherproxy.h>
29 #include <alproxies/almemoryproxy.h>
30 #include <alproxies/alsentinelproxy.h>
31 #include <alproxies/dcmproxy.h>
32 #include <interfaces/NaoSensorInterface.h>
33 #include <interfaces/SwitchInterface.h>
34 #include <sys/stat.h>
35 
36 #include <boost/bind/bind.hpp>
37 #include <cerrno>
38 #include <cstring>
39 
40 using namespace fawkes;
41 
42 #define POWEROFF_PATH "/sbin/poweroff"
43 
44 /** @class NaoQiButtonThread "dcm_thread.h"
45  * Thread to provide buttons to Fawkes.
46  * This thread reads the sensors data from the DCM thread and
47  * processes the included button data. From that it derives short and
48  * long activations as well as a basic pattern (three times long
49  * press) to turn off the robot. It also plays audio samples based on
50  * the activations.
51  *
52  * @author Tim Niemueller
53  */
54 
55 /** Constructor. */
57 : Thread("NaoQiButtonThread", Thread::OPMODE_WAITFORWAKEUP),
58  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
59 {
60 }
61 
62 /** Destructor. */
64 {
65 }
66 
67 void
69 {
70  sound_longpling_ = sound_pling_ = -1;
71  sound_bumper_left_ = sound_bumper_right_ = -1;
72  last_shutdown_actcount = 0;
73 
74  cfg_chest_triple_long_click_shutdown_ = false;
75  try {
76  cfg_chest_triple_long_click_shutdown_ =
77  config->get_bool("/hardware/nao/chestbut_triple_long_click_shutdown");
78  } catch (Exception &e) {
79  } // ignored
80 
81  // Is the audio player loaded?
82  try {
83  AL::ALPtr<AL::ALLauncherProxy> launcher(new AL::ALLauncherProxy(naoqi_broker));
84  bool is_auplayer_available = launcher->isModulePresent("ALAudioPlayer");
85  bool is_alsentinel_available = launcher->isModulePresent("ALSentinel");
86 
87  if (!is_auplayer_available) {
88  logger->log_warn(name(), "ALAudioPlayer not available, disabling sounds");
89  } else {
90  auplayer_ = AL::ALPtr<AL::ALAudioPlayerProxy>(new AL::ALAudioPlayerProxy(naoqi_broker));
91  sound_longpling_ = auplayer_->loadFile(RESDIR "/sounds/longpling.wav");
92  sound_pling_ = auplayer_->loadFile(RESDIR "/sounds/pling.wav");
93  sound_bumper_left_ = auplayer_->loadFile(RESDIR "/sounds/metal_click_1_left.wav");
94  sound_bumper_right_ = auplayer_->loadFile(RESDIR "/sounds/metal_click_1_right.wav");
95  }
96 
97  if (is_alsentinel_available) {
98  logger->log_warn(name(), "ALSentinel loaded, disabling its button handling");
99  AL::ALPtr<AL::ALSentinelProxy> alsentinel(new AL::ALSentinelProxy(naoqi_broker));
100  alsentinel->enableDefaultActionSimpleClick(false);
101  alsentinel->enableDefaultActionDoubleClick(false);
102  alsentinel->enableDefaultActionTripleClick(false);
103  }
104  } catch (AL::ALError &e) {
105  throw Exception("Checking module availability failed: %s", e.toString().c_str());
106  }
107 
108  sensor_if_ = blackboard->open_for_reading<NaoSensorInterface>("Nao Sensors");
109 
110  chestbut_if_ = blackboard->open_for_writing<SwitchInterface>("Nao Button Chest");
111  lfoot_bumper_if_ = blackboard->open_for_writing<SwitchInterface>("Nao Button Foot Left");
112  rfoot_bumper_if_ = blackboard->open_for_writing<SwitchInterface>("Nao Button Foot Right");
113  head_front_if_ = blackboard->open_for_writing<SwitchInterface>("Nao Button Head Front");
114  head_middle_if_ = blackboard->open_for_writing<SwitchInterface>("Nao Button Head Middle");
115  head_rear_if_ = blackboard->open_for_writing<SwitchInterface>("Nao Button Head Rear");
116 
117  chestbut_if_->resize_buffers(1);
118  lfoot_bumper_if_->resize_buffers(1);
119  rfoot_bumper_if_->resize_buffers(1);
120  head_front_if_->resize_buffers(1);
121  head_middle_if_->resize_buffers(1);
122  head_rear_if_->resize_buffers(1);
123 
124  chestbut_remote_enabled_ = false;
125  lfoot_bumper_remote_enabled_ = rfoot_bumper_remote_enabled_ = false;
126  head_front_remote_enabled_ = head_middle_remote_enabled_ = head_rear_remote_enabled_ = false;
127 
128  now.set_clock(clock);
129  last.set_clock(clock);
130  now.stamp();
131  last.stamp();
132 }
133 
134 void
136 {
137  blackboard->close(chestbut_if_);
138  blackboard->close(lfoot_bumper_if_);
139  blackboard->close(rfoot_bumper_if_);
140  blackboard->close(head_front_if_);
141  blackboard->close(head_middle_if_);
142  blackboard->close(head_rear_if_);
143  blackboard->close(sensor_if_);
144  chestbut_if_ = NULL;
145  lfoot_bumper_if_ = NULL;
146  rfoot_bumper_if_ = NULL;
147  head_front_if_ = NULL;
148  head_middle_if_ = NULL;
149  head_rear_if_ = NULL;
150  sensor_if_ = NULL;
151 
152  if (auplayer_) {
153  auplayer_->unloadFile(sound_longpling_);
154  auplayer_->unloadFile(sound_pling_);
155  auplayer_->unloadFile(sound_bumper_left_);
156  auplayer_->unloadFile(sound_bumper_right_);
157  auplayer_.reset();
158  }
159 }
160 
161 void
163 {
164  now.stamp();
165  float time_diff_sec = now - &last;
166  last = now;
167 
168  sensor_if_->read();
169 
170  process_pattern_button(chestbut_if_,
171  sensor_if_->chest_button(),
173  chestbut_remote_enabled_,
174  sound_pling_,
175  sound_longpling_);
176  process_pattern_button(head_front_if_,
177  sensor_if_->head_touch_front(),
179  head_front_remote_enabled_);
180  process_pattern_button(head_middle_if_,
181  sensor_if_->head_touch_middle(),
183  head_middle_remote_enabled_);
184  process_pattern_button(head_rear_if_,
185  sensor_if_->head_touch_rear(),
187  head_rear_remote_enabled_);
188 
189  process_bumpers(lfoot_bumper_if_,
190  sensor_if_->l_foot_bumper_l(),
191  sensor_if_->l_foot_bumper_r(),
193  lfoot_bumper_remote_enabled_,
194  sound_bumper_left_);
195 
196  process_bumpers(rfoot_bumper_if_,
197  sensor_if_->r_foot_bumper_l(),
198  sensor_if_->r_foot_bumper_r(),
200  rfoot_bumper_remote_enabled_,
201  sound_bumper_right_);
202 
203  if (cfg_chest_triple_long_click_shutdown_ && chestbut_if_->long_activations() == 3
204  && chestbut_if_->activation_count() != last_shutdown_actcount) {
205  logger->log_debug(name(), "Shutting down");
206  last_shutdown_actcount = chestbut_if_->activation_count();
207  if (auplayer_)
208  auplayer_->playFile(RESDIR "/sounds/naoshutdown.wav");
209 
210  struct stat s;
211  if (stat(POWEROFF_PATH, &s) == -1) {
212  logger->log_error(name(), "Cannot stat '%s': %s", POWEROFF_PATH, strerror(errno));
213  } else {
214  if (s.st_mode & S_ISUID) {
215  int rv = system(POWEROFF_PATH);
216  if (rv == -1 || (WEXITSTATUS(rv) != 0)) {
217  logger->log_error(name(), "Failed to execute shutdown command");
218  }
219  } else {
220  logger->log_error(name(), "SetUID bit on '%s' not set, cannot shutdown.", POWEROFF_PATH);
221  }
222  }
223  }
224 }
225 
226 void
227 NaoQiButtonThread::set_interface(SwitchInterface *switch_if,
228  bool enabled,
229  float value,
230  float history,
231  unsigned int activations,
232  unsigned int short_act,
233  unsigned int long_act)
234 {
235  switch_if->copy_shared_to_buffer(0);
236 
237  switch_if->set_enabled(enabled);
238  switch_if->set_value(value);
239  switch_if->set_history(history);
240  switch_if->set_activation_count(activations);
241  switch_if->set_short_activations(short_act);
242  switch_if->set_long_activations(long_act);
243 
244  if (switch_if->compare_buffers(0) != 0)
245  switch_if->write();
246 }
247 
248 void
249 NaoQiButtonThread::process_pattern_button(SwitchInterface *switch_if,
250  float sensor_value,
251  float time_diff_sec,
252  bool & remote_enabled,
253  int sound_short,
254  int sound_long)
255 {
256  float value = 0;
257  process_messages(switch_if, remote_enabled, value);
258  value = std::max(value, sensor_value);
259 
260  bool enabled = false;
261  float history = switch_if->history();
262  unsigned int activations = switch_if->activation_count();
263  unsigned int short_act = switch_if->short_activations();
264  unsigned int long_act = switch_if->long_activations();
265 
266  pattern_button_logic(value,
267  time_diff_sec,
268  enabled,
269  history,
270  activations,
271  short_act,
272  long_act,
273  sound_short,
274  sound_long);
275 
276  set_interface(switch_if, enabled, value, history, activations, short_act, long_act);
277 }
278 
279 void
280 NaoQiButtonThread::process_bumpers(SwitchInterface *switch_if,
281  float left_value,
282  float right_value,
283  float time_diff_sec,
284  bool & remote_enabled,
285  int sound_id)
286 {
287  float value = 0;
288  process_messages(switch_if, remote_enabled, value);
289  value = std::max(std::max(value, left_value), right_value);
290 
291  bool enabled = false;
292  float history = switch_if->history();
293  unsigned int activations = switch_if->activation_count();
294  unsigned int short_act = switch_if->short_activations();
295  unsigned int long_act = switch_if->long_activations();
296 
297  bumpers_logic(value, time_diff_sec, enabled, history, activations, sound_id);
298 
299  set_interface(switch_if, enabled, value, history, activations, short_act, long_act);
300 }
301 
302 void
303 NaoQiButtonThread::process_messages(SwitchInterface *switch_if, bool &remote_enabled, float &value)
304 {
305  while (!switch_if->msgq_empty()) {
306  if (SwitchInterface::SetMessage *msg = switch_if->msgq_first_safe(msg)) {
307  if (msg->is_enabled()) {
308  value = std::min(0.5f, msg->value());
309  } else {
310  value = std::max(0.49f, msg->value());
311  }
312 
313  } else if (SwitchInterface::EnableSwitchMessage *msg = switch_if->msgq_first_safe(msg)) {
314  logger->log_debug(name(), "Got ENable switch message for %s", switch_if->id());
315  value = 1.;
316  remote_enabled = true;
317 
318  } else if (SwitchInterface::DisableSwitchMessage *msg = switch_if->msgq_first_safe(msg)) {
319  logger->log_debug(name(), "Got DISable switch message for %s", switch_if->id());
320  remote_enabled = false;
321  value = 0.;
322  }
323 
324  switch_if->msgq_pop();
325  }
326 
327  if (remote_enabled)
328  value = 1.;
329 }
330 
331 void
332 NaoQiButtonThread::pattern_button_logic(float value,
333  float time_diff_sec,
334  bool & enabled,
335  float & history,
336  unsigned int &activations,
337  unsigned int &short_act,
338  unsigned int &long_act,
339  int sound_short,
340  int sound_long)
341 {
342  if (value < 0.5) { // button released or not pressed
343  if (history > 0.025 /* sec */) { // released
344  ++activations;
345  if (history > 0.5) {
346  ++long_act;
347  if (auplayer_ && (sound_long != -1))
348  auplayer_->play(sound_long);
349  } else {
350  ++short_act;
351  if (auplayer_ && (sound_short != -1))
352  auplayer_->play(sound_short);
353  }
354  } else if (history < -2.0 /* sec */) {
355  // reset after two seconds
356  short_act = long_act = 0;
357  }
358 
359  enabled = false;
360  if (history < 0.) {
361  history -= time_diff_sec;
362  } else {
363  history = -time_diff_sec;
364  }
365 
366  } else { // at least one is enabled
367  enabled = true;
368  if (history > 0.) {
369  history += time_diff_sec;
370  } else {
371  history = time_diff_sec;
372  }
373  }
374 
375  // stop after two minutes, nobody cares after that
376  if (history < -120.) {
377  history = -120.;
378  } else if (history > 120.) {
379  history = 120.;
380  }
381 }
382 
383 void
384 NaoQiButtonThread::bumpers_logic(float value,
385  float time_diff_sec,
386  bool & enabled,
387  float & history,
388  unsigned int &activations,
389  int sound_id)
390 {
391  if (value < 0.5) { // button released or none pressed
392  enabled = false;
393  if (history < 0.) {
394  history -= time_diff_sec;
395  } else {
396  history = -time_diff_sec;
397  }
398 
399  } else { // at least one is enabled
400  if (history <= 0. /* sec */) { // pressed
401  if (auplayer_ && (sound_id != -1))
402  auplayer_->play(sound_id);
403  ++activations;
404  }
405 
406  enabled = true;
407  if (history > 0.) {
408  history += time_diff_sec;
409  } else {
410  history = time_diff_sec;
411  }
412  }
413 
414  // stop after two minutes, nobody cares after that
415  if (history < -120.) {
416  history = -120.;
417  } else if (history > 120.) {
418  history = 120.;
419  }
420 }
NaoQiButtonThread()
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual ~NaoQiButtonThread()
Destructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void copy_shared_to_buffer(unsigned int buffer)
Copy data from private memory to buffer.
Definition: interface.cpp:1296
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void resize_buffers(unsigned int num_buffers)
Resize buffer array.
Definition: interface.cpp:1261
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
int compare_buffers(unsigned int buffer)
Compare buffer to private memory.
Definition: interface.cpp:1360
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
Definition: naoqi.h:44
NaoSensorInterface Fawkes BlackBoard Interface.
uint8_t head_touch_rear() const
Get head_touch_rear value.
uint8_t l_foot_bumper_l() const
Get l_foot_bumper_l value.
uint8_t chest_button() const
Get chest_button value.
uint8_t r_foot_bumper_l() const
Get r_foot_bumper_l value.
uint8_t l_foot_bumper_r() const
Get l_foot_bumper_r value.
uint8_t head_touch_middle() const
Get head_touch_middle value.
uint8_t head_touch_front() const
Get head_touch_front value.
uint8_t r_foot_bumper_r() const
Get r_foot_bumper_r value.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
SetMessage Fawkes BlackBoard Interface Message.
SwitchInterface Fawkes BlackBoard Interface.
float history() const
Get history value.
uint32_t short_activations() const
Get short_activations value.
void set_long_activations(const uint32_t new_long_activations)
Set long_activations value.
void set_short_activations(const uint32_t new_short_activations)
Set short_activations value.
uint32_t long_activations() const
Get long_activations value.
void set_enabled(const bool new_enabled)
Set enabled value.
void set_activation_count(const uint32_t new_activation_count)
Set activation_count value.
void set_value(const float new_value)
Set value value.
void set_history(const float new_history)
Set history value.
uint32_t activation_count() const
Get activation_count value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_clock(Clock *clock)
Set clock for this instance.
Definition: time.cpp:308
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
Fawkes library namespace.
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
Definition: time.h:41