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> 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();
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.