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();
290 dynamic_cast<HumanoidMotionInterface::StandupMessage *>(msg_action);
297 thread_pool_->enqueue(motion_task_);
302 dynamic_cast<HumanoidMotionInterface::KickMessage *>(msg_action);
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();
float speed() const
Get speed value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
StandupMessage Fawkes BlackBoard Interface Message.
bool msgq_empty()
Check if queue is empty.
KickMessage Fawkes BlackBoard Interface Message.
void unref()
Decrement reference count and conditionally delete this instance.
virtual void finalize()
Finalize the thread.
Fawkes library namespace.
void set_arms_enabled(const bool new_arms_enabled)
Set arms_enabled value.
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
float yaw() const
Get yaw value.
LegEnum leg() const
Get leg value.
virtual ~NaoQiMotionThread()
Destructor.
float accel_x() const
Get accel_x value.
float theta() const
Get theta value.
WalkVelocityMessage Fawkes BlackBoard Interface Message.
Thread class encapsulation of pthreads.
NaoSensorInterface Fawkes BlackBoard Interface.
NaoQiMotionThread()
Constructor.
void set_moving(const bool new_moving)
Set moving value.
GetUpMessage Fawkes BlackBoard Interface Message.
void write()
Write from local copy into BlackBoard memory.
Logger * logger
This is the Logger member used to access the logger.
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
HumanoidMotionInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
float y() const
Get y value.
void read()
Read from BlackBoard into local copy.
float accel_y() const
Get accel_y value.
float accel_z() const
Get accel_z value.
void ref()
Increment reference count.
StandupEnum from_pos() const
Get from_pos value.
const char * name() const
Get name of thread.
virtual void loop()
Code to execute in the thread.
bool msgq_first_is()
Check if first message has desired type.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
bool is_of_type()
Check if message has desired type.
float x() const
Get x value.
float pitch() const
Get pitch value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
ParkMessage Fawkes BlackBoard Interface Message.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
WalkStraightMessage Fawkes BlackBoard Interface Message.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
MoveHeadMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
float speed() const
Get speed value.
virtual void close(Interface *interface)=0
Close interface.