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