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
36using 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
58void
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
82void
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 */
100void
101NaoQiMotionThread::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
122void
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. */
147void
148NaoQiMotionThread::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.)) {
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:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
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.
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:44
bool is_of_type()
Check if message has desired type.
Definition: message.h:161
unsigned int id() const
Get message ID.
Definition: message.cpp:181
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.