Fawkes API  Fawkes Development Version
motion_thread.cpp
1 
2 /***************************************************************************
3  * motion_thread.cpp - Provide NaoQi motion to Fawkes
4  *
5  * Created: Thu Jun 09 12:58:14 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 "motion_thread.h"
24 #include "motion_kick_task.h"
25 #include "motion_standup_task.h"
26 #include "motion_utils.h"
27 
28 #include <alcore/alerror.h>
29 #include <alproxies/allauncherproxy.h>
30 #include <alproxies/almotionproxy.h>
31 #include <althread/althreadpool.h>
32 
33 #include <interfaces/HumanoidMotionInterface.h>
34 #include <interfaces/NaoSensorInterface.h>
35 
36 using namespace fawkes;
37 
38 /** @class NaoQiMotionThread "motion_thread.h"
39  * Thread to provide NaoQi motions to Fawkes.
40  * This thread holds an ALMotion proxy and provides its capabilities via
41  * the blackboard to other Fawkes threads.
42  *
43  * @author Tim Niemueller
44  */
45 
46 /** Constructor. */
48  : Thread("NaoQiMotionThread", Thread::OPMODE_WAITFORWAKEUP),
50 {
51 }
52 
53 
54 /** Destructor. */
56 {
57 }
58 
59 
60 void
62 {
63  __motion_task_id = -1;
64 
65  // Is ALMotion available?
66  try {
67  AL::ALPtr<AL::ALLauncherProxy> launcher(new AL::ALLauncherProxy(naoqi_broker));
68  bool is_almotion_available = launcher->isModulePresent("ALMotion");
69 
70  if (! is_almotion_available) {
71  throw Exception("NaoQi ALMotion is not available");
72  }
73  } catch (AL::ALError& e) {
74  throw Exception("Checking ALMotion aliveness failed: %s",
75  e.toString().c_str());
76  }
77 
78  __almotion = naoqi_broker->getMotionProxy();
79  __thread_pool = naoqi_broker->getThreadPool();
80 
81  __hummot_if =
83  __sensor_if =
85 }
86 
87 
88 void
90 {
91  stop_motion();
92 
93  blackboard->close(__hummot_if);
94  blackboard->close(__sensor_if);
95  __hummot_if = NULL;
96  __sensor_if = NULL;
97 
98  __almotion.reset();
99 }
100 
101 
102 /** Stop any currently running motion.
103  * Walk tasks are stopped gracefully, i.e. by setting the velocity to zero and
104  * allowing the robot to come to a safe stopping position. Other motion
105  * tasks are simply killed.
106  */
107 void
108 NaoQiMotionThread::stop_motion()
109 {
110  if (__almotion->walkIsActive()) {
111  __almotion->setWalkTargetVelocity(0., 0., 0., 0.);
112  } else if (__motion_task_id != -1) {
113  if (__almotion->isRunning(__motion_task_id)) {
114  __almotion->killTask(__motion_task_id);
115  }
116  __motion_task_id = -1;
117  } else if (__motion_task) {
118  if (__motion_task) {
119  __motion_task->exitTask();
120  __motion_task.reset();
121  }
122  }
123 
124 
125  AL::ALValue names = AL::ALValue::array("HeadYaw", "HeadPitch");
126  std::vector<float> head_angles = __almotion->getAngles(names, false);
127  __almotion->setAngles(names, head_angles, 1.0);
128 }
129 
130 
131 void
133 {
134  process_messages();
135 
136  bool walking = __almotion->walkIsActive();
137  bool tasking = __motion_task_id != -1 && __almotion->isRunning(__motion_task_id);
138  bool custom_task = false;
139 
140  if (__motion_task) {
141  if (__motion_task->getState() == AL::ALTask::RUNNING) {
142  custom_task = true;
143  } else if (__motion_task->getState() == AL::ALTask::ENDED) {
144  __motion_task.reset();
145  }
146  }
147 
148  __hummot_if->set_moving(walking || tasking || custom_task);
149  AL::ALValue varms_enabled = __almotion->getWalkArmsEnable();
150  bool arms_enabled = varms_enabled[0] || varms_enabled[1];
151  __hummot_if->set_arms_enabled(arms_enabled);
152  __hummot_if->write();
153 }
154 
155 
156 /** Process incoming BB messages. */
157 void
158 NaoQiMotionThread::process_messages()
159 {
160  bool stop = false;
161  HumanoidMotionInterface::WalkVelocityMessage* msg_walk_velocity = NULL;
162  HumanoidMotionInterface::MoveHeadMessage* msg_move_head = NULL;
163  Message* msg_action = NULL;
164 
165  // process bb messages
166  while ( ! __hummot_if->msgq_empty() ) {
168  {
169  stop = true;
170  }
172  __hummot_if->msgq_first_safe(msg))
173  {
174 
175  }
177  __hummot_if->msgq_first_safe(msg))
178  {
179  if (msg_walk_velocity) msg_walk_velocity->unref();
180  msg_walk_velocity = msg;
181  msg_walk_velocity->ref();
182  if (msg_action) msg_action->unref();
183  msg_action = NULL;
184  stop = false;
185  }
186 
188  __hummot_if->msgq_first_safe(msg))
189  {
190  if (msg_move_head) msg_move_head->unref();
191  msg_move_head = msg;
192  msg_move_head->ref();
193  if (msg_action) msg_action->unref();
194  msg_action = NULL;
195  stop = false;
196  }
197 
199  __hummot_if->msgq_first_safe(msg))
200  {
201  if (msg_action) msg_action->unref();
202  msg_action = msg;
203  msg_action->ref();
204  stop = false;
205  }
207  __hummot_if->msgq_first_safe(msg))
208  {
209  if (msg_action) msg_action->unref();
210  msg_action = msg;
211  msg_action->ref();
212  stop = false;
213  }
214 
216  __hummot_if->msgq_first_safe(msg))
217  {
218  if (msg_action) msg_action->unref();
219  msg_action = msg;
220  msg_action->ref();
221  stop = false;
222  }
223 
225  __hummot_if->msgq_first_safe(msg))
226  {
227  if (msg_action) msg_action->unref();
228  msg_action = msg;
229  msg_action->ref();
230  stop = false;
231  }
232 
233  __hummot_if->msgq_pop();
234  }
235 
236  // process last message
237  if (stop) {
238  logger->log_debug(name(), "Stopping motion");
239  stop_motion();
240  }
241  else if (msg_action) {
243  motion::timed_move_joints(__almotion,
244  /* head */ 0., 0.,
245  /* l shoulder */ 2.1, 0.35,
246  /* l elbow */ -1.40, -1.40,
247  /* l wrist/hand */ 0., 0.,
248  /* l hip */ 0., 0., -0.52,
249  /* l knee */ 1.05,
250  /* l ankle */ -0.52, 0.,
251  /* r shoulder */ 2.1, -0.35,
252  /* r elbow */ 1.40, 1.40,
253  /* r wrist/hand */ 0., 0.,
254  /* r hip */ 0., 0., -0.52,
255  /* r knee */ 1.05,
256  /* r ankle */ -0.52, 0.,
257  /* time */ 3.0);
258 
259  __hummot_if->set_msgid(msg_action->id());
260  }
261  else if (msg_action->is_of_type<HumanoidMotionInterface::ParkMessage>()) {
262  motion::timed_move_joints(__almotion,
263  /* head */ 0., 0.,
264  /* l shoulder */ 1.58, 0.15,
265  /* l elbow */ -1.20, -1.1,
266  /* l wrist/hand */ 0., 0.,
267  /* l hip */ -0.08, 0., -0.85,
268  /* l knee */ 2.2,
269  /* l ankle */ -1.23, 0.,
270  /* r shoulder */ 1.55, -0.15,
271  /* r elbow */ 1.2, 1.1,
272  /* r wrist/hand */ 0., 0.,
273  /* r hip */ -0.08, 0., -0.85,
274  /* r knee */ 2.2,
275  /* r ankle */ -1.23, 0.,
276  /* time */ 3.0);
277 
278  __hummot_if->set_msgid(msg_action->id());
279  }
280  else if (msg_action->is_of_type<HumanoidMotionInterface::StandupMessage>()) {
281  if (__motion_task) {
282  __motion_task->exitTask();
283  }
284 
286  __sensor_if->read();
287  __motion_task.reset(new NaoQiMotionStandupTask(__almotion, msg->from_pos(),
288  __sensor_if->accel_x(),
289  __sensor_if->accel_y(),
290  __sensor_if->accel_z()));
291  __thread_pool->enqueue(__motion_task);
292 
293  __hummot_if->set_msgid(msg->id());
294  }
295  else if (msg_action->is_of_type<HumanoidMotionInterface::KickMessage>()) {
297  if (__motion_task) {
298  __motion_task->exitTask();
299  }
300  __motion_task.reset(new NaoQiMotionKickTask(__almotion, msg->leg()));
301  __thread_pool->enqueue(__motion_task);
302 
303  __hummot_if->set_msgid(msg->id());
304  }
305 
306  msg_action->unref();
307  }
308  else {
309  if (msg_walk_velocity) {
310  if ( (msg_walk_velocity->speed() < 0.) || (msg_walk_velocity->speed() > 1.)) {
311  logger->log_warn(name(), "Walk velocity speed %f out of range [0.0..1.0],",
312  "ignoring command", msg_walk_velocity->speed());
313  } else {
314  try {
315  __almotion->setWalkTargetVelocity(msg_walk_velocity->x(), msg_walk_velocity->y(), msg_walk_velocity->theta(),
316  msg_walk_velocity->speed());
317  } catch (AL::ALError &e) {
318  logger->log_warn(name(), "WalkVelocity command failed: %s", e.what());
319  }
320  }
321  __hummot_if->set_msgid(msg_walk_velocity->id());
322  msg_walk_velocity->unref();
323  }
324 
325  if (msg_move_head) {
326  AL::ALValue names = AL::ALValue::array("HeadYaw", "HeadPitch");
327  AL::ALValue angles = AL::ALValue::array(msg_move_head->yaw(), msg_move_head->pitch());
328 
329  __almotion->setAngles(names, angles, msg_move_head->speed());
330  msg_move_head->unref();
331  }
332  }
333 
334 
335 
336 }
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
unsigned int id() const
Get message ID.
Definition: message.cpp:197
StandupMessage Fawkes BlackBoard Interface Message.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
KickMessage Fawkes BlackBoard Interface Message.
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:99
virtual void finalize()
Finalize the thread.
Fawkes library namespace.
void set_arms_enabled(const bool new_arms_enabled)
Set arms_enabled value.
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
Definition: naoqi.h:45
virtual ~NaoQiMotionThread()
Destructor.
float accel_x() const
Get accel_x value.
WalkVelocityMessage Fawkes BlackBoard Interface Message.
Thread class encapsulation of pthreads.
Definition: thread.h:42
NaoSensorInterface Fawkes BlackBoard Interface.
NaoQiMotionThread()
Constructor.
void set_moving(const bool new_moving)
Set moving value.
GetUpMessage Fawkes BlackBoard Interface Message.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
void set_msgid(const uint32_t new_msgid)
Set msgid value.
HumanoidMotionInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
NaoQi kick task.
float accel_y() const
Get accel_y value.
float accel_z() const
Get accel_z value.
void ref()
Increment reference count.
Definition: refcount.cpp:70
NaoQi standup task.
StandupEnum from_pos() const
Get from_pos value.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void loop()
Code to execute in the thread.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
bool is_of_type()
Check if message has desired type.
Definition: message.h:138
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
ParkMessage Fawkes BlackBoard Interface Message.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:302
WalkStraightMessage Fawkes BlackBoard Interface Message.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
MoveHeadMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.