23 #include "motion_thread.h"
24 #include "motion_kick_task.h"
25 #include "motion_standup_task.h"
26 #include "motion_utils.h"
28 #include <alcore/alerror.h>
29 #include <alproxies/allauncherproxy.h>
30 #include <alproxies/almotionproxy.h>
31 #include <althread/althreadpool.h>
33 #include <interfaces/HumanoidMotionInterface.h>
34 #include <interfaces/NaoSensorInterface.h>
36 using namespace fawkes;
48 :
Thread(
"NaoQiMotionThread",
Thread::OPMODE_WAITFORWAKEUP),
63 __motion_task_id = -1;
67 AL::ALPtr<AL::ALLauncherProxy> launcher(
new AL::ALLauncherProxy(
naoqi_broker));
68 bool is_almotion_available = launcher->isModulePresent(
"ALMotion");
70 if (! is_almotion_available) {
71 throw Exception(
"NaoQi ALMotion is not available");
73 }
catch (AL::ALError& e) {
74 throw Exception(
"Checking ALMotion aliveness failed: %s",
75 e.toString().c_str());
108 NaoQiMotionThread::stop_motion()
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);
116 __motion_task_id = -1;
117 }
else if (__motion_task) {
119 __motion_task->exitTask();
120 __motion_task.reset();
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);
136 bool walking = __almotion->walkIsActive();
137 bool tasking = __motion_task_id != -1 && __almotion->isRunning(__motion_task_id);
138 bool custom_task =
false;
141 if (__motion_task->getState() == AL::ALTask::RUNNING) {
143 }
else if (__motion_task->getState() == AL::ALTask::ENDED) {
144 __motion_task.reset();
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];
152 __hummot_if->
write();
158 NaoQiMotionThread::process_messages()
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();
190 if (msg_move_head) msg_move_head->
unref();
192 msg_move_head->
ref();
193 if (msg_action) msg_action->
unref();
201 if (msg_action) msg_action->
unref();
209 if (msg_action) msg_action->
unref();
218 if (msg_action) msg_action->
unref();
227 if (msg_action) msg_action->
unref();
241 else if (msg_action) {
243 motion::timed_move_joints(__almotion,
262 motion::timed_move_joints(__almotion,
282 __motion_task->exitTask();
291 __thread_pool->enqueue(__motion_task);
298 __motion_task->exitTask();
301 __thread_pool->enqueue(__motion_task);
309 if (msg_walk_velocity) {
310 if ( (msg_walk_velocity->
speed() < 0.) || (msg_walk_velocity->
speed() > 1.)) {
312 "ignoring command", msg_walk_velocity->
speed());
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) {
322 msg_walk_velocity->
unref();
326 AL::ALValue names = AL::ALValue::array(
"HeadYaw",
"HeadPitch");
327 AL::ALValue angles = AL::ALValue::array(msg_move_head->
yaw(), msg_move_head->
pitch());
329 __almotion->setAngles(names, angles, msg_move_head->
speed());
330 msg_move_head->
unref();