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