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
40using 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
67void
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
134void
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
161void
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
226void
227NaoQiButtonThread::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
248void
249NaoQiButtonThread::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
279void
280NaoQiButtonThread::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
302void
303NaoQiButtonThread::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
331void
332NaoQiButtonThread::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
383void
384NaoQiButtonThread::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