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 }
fawkes::NaoSensorInterface::accel_y
float accel_y() const
Get accel_y value.
Definition: NaoSensorInterface.cpp:165
fawkes::RefCount::unref
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:101
fawkes::HumanoidMotionInterface::KickMessage
Definition: HumanoidMotionInterface.h:270
fawkes::Interface::msgq_first_is
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
NaoQiMotionThread::NaoQiMotionThread
NaoQiMotionThread()
Constructor.
Definition: motion_thread.cpp:47
NaoQiMotionKickTask
Definition: motion_kick_task.h:31
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1184
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1031
fawkes::HumanoidMotionInterface::WalkStraightMessage
Definition: HumanoidMotionInterface.h:111
fawkes::HumanoidMotionInterface::WalkVelocityMessage::theta
float theta() const
Get theta value.
Definition: HumanoidMotionInterface.cpp:810
fawkes::HumanoidMotionInterface::StandupMessage
Definition: HumanoidMotionInterface.h:345
fawkes::Interface::msgq_first_safe
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:303
fawkes::Message
Definition: message.h:41
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
fawkes::NaoSensorInterface
Definition: NaoSensorInterface.h:39
fawkes::HumanoidMotionInterface::WalkVelocityMessage::x
float x() const
Get x value.
Definition: HumanoidMotionInterface.cpp:742
fawkes::HumanoidMotionInterface::set_msgid
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Definition: HumanoidMotionInterface.cpp:212
fawkes::HumanoidMotionInterface::MoveHeadMessage
Definition: HumanoidMotionInterface.h:372
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
NaoQiMotionThread::init
virtual void init()
Initialize the thread.
Definition: motion_thread.cpp:59
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
NaoQiMotionThread::loop
virtual void loop()
Code to execute in the thread.
Definition: motion_thread.cpp:123
NaoQiMotionThread::finalize
virtual void finalize()
Finalize the thread.
Definition: motion_thread.cpp:83
fawkes::HumanoidMotionInterface::MoveHeadMessage::yaw
float yaw() const
Get yaw value.
Definition: HumanoidMotionInterface.cpp:1390
fawkes::HumanoidMotionInterface::WalkVelocityMessage::speed
float speed() const
Get speed value.
Definition: HumanoidMotionInterface.cpp:844
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::RefCount::ref
void ref()
Increment reference count.
Definition: refcount.cpp:73
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::NaoQiAspect::naoqi_broker
AL::ALPtr< AL::ALBroker > naoqi_broker
Definition: naoqi.h:50
NaoQiMotionStandupTask
Definition: motion_standup_task.h:31
fawkes::HumanoidMotionInterface
Definition: HumanoidMotionInterface.h:39
fawkes
fawkes::HumanoidMotionInterface::MoveHeadMessage::pitch
float pitch() const
Get pitch value.
Definition: HumanoidMotionInterface.cpp:1420
fawkes::HumanoidMotionInterface::StandupMessage::from_pos
StandupEnum from_pos() const
Get from_pos value.
Definition: HumanoidMotionInterface.cpp:1282
fawkes::HumanoidMotionInterface::set_moving
void set_moving(const bool new_moving)
Set moving value.
Definition: HumanoidMotionInterface.cpp:140
fawkes::HumanoidMotionInterface::GetUpMessage
Definition: HumanoidMotionInterface.h:323
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::HumanoidMotionInterface::set_arms_enabled
void set_arms_enabled(const bool new_arms_enabled)
Set arms_enabled value.
Definition: HumanoidMotionInterface.cpp:175
fawkes::HumanoidMotionInterface::WalkVelocityMessage::y
float y() const
Get y value.
Definition: HumanoidMotionInterface.cpp:776
fawkes::NaoSensorInterface::accel_x
float accel_x() const
Get accel_x value.
Definition: NaoSensorInterface.cpp:134
fawkes::HumanoidMotionInterface::KickMessage::leg
LegEnum leg() const
Get leg value.
Definition: HumanoidMotionInterface.cpp:1050
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::HumanoidMotionInterface::ParkMessage
Definition: HumanoidMotionInterface.h:301
fawkes::Message::id
unsigned int id() const
Get message ID.
Definition: message.cpp:186
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Message::is_of_type
bool is_of_type()
Check if message has desired type.
Definition: message.h:140
fawkes::HumanoidMotionInterface::StopMessage
Definition: HumanoidMotionInterface.h:89
fawkes::HumanoidMotionInterface::MoveHeadMessage::speed
float speed() const
Get speed value.
Definition: HumanoidMotionInterface.cpp:1450
fawkes::HumanoidMotionInterface::WalkVelocityMessage
Definition: HumanoidMotionInterface.h:196
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:499
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::NaoSensorInterface::accel_z
float accel_z() const
Get accel_z value.
Definition: NaoSensorInterface.cpp:196
NaoQiMotionThread::~NaoQiMotionThread
virtual ~NaoQiMotionThread()
Destructor.
Definition: motion_thread.cpp:54
fawkes::Exception
Definition: exception.h:41