23 #include "motion_thread.h"
25 #include "motion_kick_task.h"
26 #include "motion_standup_task.h"
27 #include "motion_utils.h"
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>
48 :
Thread(
"NaoQiMotionThread",
Thread::OPMODE_WAITFORWAKEUP),
65 AL::ALPtr<AL::ALLauncherProxy> launcher(
new AL::ALLauncherProxy(
naoqi_broker));
66 bool is_almotion_available = launcher->isModulePresent(
"ALMotion");
68 if (!is_almotion_available) {
69 throw Exception(
"NaoQi ALMotion is not available");
71 }
catch (AL::ALError &e) {
72 throw Exception(
"Checking ALMotion aliveness failed: %s", e.toString().c_str());
101 NaoQiMotionThread::stop_motion()
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_);
109 motion_task_id_ = -1;
110 }
else if (motion_task_) {
112 motion_task_->exitTask();
113 motion_task_.reset();
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);
127 bool walking = almotion_->walkIsActive();
128 bool tasking = motion_task_id_ != -1 && almotion_->isRunning(motion_task_id_);
129 bool custom_task =
false;
132 if (motion_task_->getState() == AL::ALTask::RUNNING) {
134 }
else if (motion_task_->getState() == AL::ALTask::ENDED) {
135 motion_task_.reset();
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];
148 NaoQiMotionThread::process_messages()
163 if (msg_walk_velocity)
164 msg_walk_velocity->
unref();
165 msg_walk_velocity = msg;
166 msg_walk_velocity->
ref();
175 msg_move_head->
unref();
177 msg_move_head->
ref();
221 }
else if (msg_action) {
223 motion::timed_move_joints(almotion_,
254 motion::timed_move_joints(almotion_,
286 motion_task_->exitTask();
297 thread_pool_->enqueue(motion_task_);
304 motion_task_->exitTask();
307 thread_pool_->enqueue(motion_task_);
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],",
319 msg_walk_velocity->
speed());
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) {
331 msg_walk_velocity->
unref();
335 AL::ALValue names = AL::ALValue::array(
"HeadYaw",
"HeadPitch");
336 AL::ALValue angles = AL::ALValue::array(msg_move_head->
yaw(), msg_move_head->
pitch());
338 almotion_->setAngles(names, angles, msg_move_head->
speed());
339 msg_move_head->
unref();