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();
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.
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.
GetUpMessage Fawkes BlackBoard Interface Message.
KickMessage Fawkes BlackBoard Interface Message.
LegEnum leg() const
Get leg value.
MoveHeadMessage Fawkes BlackBoard Interface Message.
float speed() const
Get speed value.
float pitch() const
Get pitch value.
float yaw() const
Get yaw value.
ParkMessage Fawkes BlackBoard Interface Message.
StandupMessage Fawkes BlackBoard Interface Message.
StandupEnum from_pos() const
Get from_pos value.
StopMessage Fawkes BlackBoard Interface Message.
WalkStraightMessage Fawkes BlackBoard Interface Message.
WalkVelocityMessage Fawkes BlackBoard Interface Message.
float x() const
Get x value.
float speed() const
Get speed value.
float y() const
Get y value.
float theta() const
Get theta value.
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.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void msgq_pop()
Erase first message from queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
void read()
Read from BlackBoard into local copy.
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.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
bool is_of_type()
Check if message has desired type.
unsigned int id() const
Get message ID.
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
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.
void ref()
Increment reference count.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.