Fawkes API  Fawkes Development Version
motion_thread.cpp
00001 
00002 /***************************************************************************
00003  *  motion_thread.cpp - Provide NaoQi motion to Fawkes
00004  *
00005  *  Created: Thu Jun 09 12:58:14 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "motion_thread.h"
00024 #include "motion_kick_task.h"
00025 #include "motion_standup_task.h"
00026 #include "motion_utils.h"
00027 
00028 #include <alcore/alerror.h>
00029 #include <alproxies/allauncherproxy.h>
00030 #include <alproxies/almotionproxy.h>
00031 #include <althread/althreadpool.h>
00032 
00033 #include <interfaces/HumanoidMotionInterface.h>
00034 #include <interfaces/NaoSensorInterface.h>
00035 
00036 using namespace fawkes;
00037 
00038 /** @class NaoQiMotionThread "motion_thread.h"
00039  * Thread to provide NaoQi motions to Fawkes.
00040  * This thread holds an ALMotion proxy and provides its capabilities via
00041  * the blackboard to other Fawkes threads.
00042  *
00043  * @author Tim Niemueller
00044  */
00045 
00046 /** Constructor. */
00047 NaoQiMotionThread::NaoQiMotionThread()
00048   : Thread("NaoQiMotionThread", Thread::OPMODE_WAITFORWAKEUP),
00049     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT)
00050 {
00051 }
00052 
00053 
00054 /** Destructor. */
00055 NaoQiMotionThread::~NaoQiMotionThread()
00056 {
00057 }
00058 
00059 
00060 void
00061 NaoQiMotionThread::init()
00062 {
00063   __motion_task_id = -1;
00064 
00065   // Is ALMotion available?
00066   try {
00067     AL::ALPtr<AL::ALLauncherProxy> launcher(new AL::ALLauncherProxy(naoqi_broker));
00068     bool is_almotion_available = launcher->isModulePresent("ALMotion");
00069 
00070     if (! is_almotion_available) {
00071       throw Exception("NaoQi ALMotion is not available");
00072     }
00073   } catch (AL::ALError& e) {
00074     throw Exception("Checking ALMotion aliveness failed: %s",
00075                     e.toString().c_str());
00076   }
00077 
00078   __almotion = naoqi_broker->getMotionProxy();
00079   __thread_pool = naoqi_broker->getThreadPool();
00080 
00081   __hummot_if =
00082     blackboard->open_for_writing<HumanoidMotionInterface>("NaoQi Motion");
00083   __sensor_if =
00084     blackboard->open_for_reading<NaoSensorInterface>("Nao Sensors");
00085 }
00086 
00087 
00088 void
00089 NaoQiMotionThread::finalize()
00090 {
00091   stop_motion();
00092 
00093   blackboard->close(__hummot_if);
00094   blackboard->close(__sensor_if);
00095   __hummot_if = NULL;
00096   __sensor_if = NULL;
00097 
00098   __almotion.reset();
00099 }
00100 
00101 
00102 /** Stop any currently running motion.
00103  * Walk tasks are stopped gracefully, i.e. by setting the velocity to zero and
00104  * allowing the robot to come to a safe stopping position. Other motion
00105  * tasks are simply killed.
00106  */
00107 void
00108 NaoQiMotionThread::stop_motion()
00109 {
00110   if (__almotion->walkIsActive()) {
00111     __almotion->setWalkTargetVelocity(0., 0., 0., 0.);
00112   } else if (__motion_task_id != -1) {
00113     if (__almotion->isRunning(__motion_task_id)) {
00114       __almotion->killTask(__motion_task_id);
00115     }
00116     __motion_task_id = -1;
00117   } else if (__motion_task) {
00118     if (__motion_task) {
00119       __motion_task->exitTask();
00120       __motion_task.reset();
00121     }
00122   }
00123 
00124 
00125   AL::ALValue names  = AL::ALValue::array("HeadYaw", "HeadPitch");
00126   std::vector<float> head_angles = __almotion->getAngles(names, false);
00127   __almotion->setAngles(names, head_angles, 1.0);
00128 }
00129 
00130 
00131 void
00132 NaoQiMotionThread::loop()
00133 {
00134   process_messages();
00135 
00136   bool walking = __almotion->walkIsActive();
00137   bool tasking = __motion_task_id != -1 && __almotion->isRunning(__motion_task_id);
00138   bool custom_task  = false;
00139 
00140   if (__motion_task) {
00141     if (__motion_task->getState() == AL::ALTask::RUNNING) {
00142       custom_task = true;
00143     } else if (__motion_task->getState() == AL::ALTask::ENDED) {
00144       __motion_task.reset();
00145     }
00146   }
00147 
00148   __hummot_if->set_moving(walking || tasking || custom_task);
00149   AL::ALValue varms_enabled = __almotion->getWalkArmsEnable();
00150   bool arms_enabled = varms_enabled[0] || varms_enabled[1];
00151   __hummot_if->set_arms_enabled(arms_enabled);
00152   __hummot_if->write();
00153 }
00154 
00155 
00156 /** Process incoming BB messages. */
00157 void
00158 NaoQiMotionThread::process_messages()
00159 {
00160   bool stop = false;
00161   HumanoidMotionInterface::WalkVelocityMessage*  msg_walk_velocity = NULL;
00162   HumanoidMotionInterface::MoveHeadMessage*      msg_move_head = NULL;
00163   Message* msg_action = NULL;
00164 
00165   // process bb messages
00166   while ( ! __hummot_if->msgq_empty() ) {
00167     if (__hummot_if->msgq_first_is<HumanoidMotionInterface::StopMessage>())
00168     {
00169       stop = true;
00170     }
00171     else if (HumanoidMotionInterface::WalkStraightMessage *msg =
00172                __hummot_if->msgq_first_safe(msg))
00173     {
00174 
00175     }
00176     else if (HumanoidMotionInterface::WalkVelocityMessage *msg =
00177              __hummot_if->msgq_first_safe(msg))
00178     {
00179       if (msg_walk_velocity)  msg_walk_velocity->unref();
00180       msg_walk_velocity = msg;
00181       msg_walk_velocity->ref();
00182       if (msg_action)  msg_action->unref();
00183       msg_action = NULL;
00184       stop = false;
00185     }
00186 
00187     else if (HumanoidMotionInterface::MoveHeadMessage *msg =
00188              __hummot_if->msgq_first_safe(msg))
00189     {
00190       if (msg_move_head)  msg_move_head->unref();
00191       msg_move_head = msg;
00192       msg_move_head->ref();
00193       if (msg_action)  msg_action->unref();
00194       msg_action = NULL;
00195       stop = false;
00196     }
00197 
00198     else if (HumanoidMotionInterface::GetUpMessage *msg =
00199              __hummot_if->msgq_first_safe(msg))
00200     {
00201       if (msg_action)  msg_action->unref();
00202       msg_action = msg;
00203       msg_action->ref();
00204       stop = false;
00205     }
00206     else if (HumanoidMotionInterface::ParkMessage *msg =
00207              __hummot_if->msgq_first_safe(msg))
00208     {
00209       if (msg_action)  msg_action->unref();
00210       msg_action = msg;
00211       msg_action->ref();
00212       stop = false;
00213     }
00214 
00215     else if (HumanoidMotionInterface::KickMessage *msg =
00216              __hummot_if->msgq_first_safe(msg))
00217     {
00218       if (msg_action)  msg_action->unref();
00219       msg_action = msg;
00220       msg_action->ref();
00221       stop = false;
00222     }
00223 
00224     else if (HumanoidMotionInterface::StandupMessage *msg =
00225              __hummot_if->msgq_first_safe(msg))
00226     {
00227       if (msg_action)  msg_action->unref();
00228       msg_action = msg;
00229       msg_action->ref();
00230       stop = false;
00231     }
00232 
00233     __hummot_if->msgq_pop();
00234   }
00235 
00236   // process last message
00237   if (stop) {
00238     logger->log_debug(name(), "Stopping motion");
00239     stop_motion();
00240   }
00241   else if (msg_action) {
00242     if (msg_action->is_of_type<HumanoidMotionInterface::GetUpMessage>()) {
00243       motion::timed_move_joints(__almotion,
00244                                 /* head */ 0., 0.,
00245                                 /* l shoulder */ 2.1, 0.35,
00246                                 /* l elbow */ -1.40, -1.40,
00247                                 /* l wrist/hand */ 0., 0.,
00248                                 /* l hip */ 0., 0., -0.52,
00249                                 /* l knee */ 1.05,
00250                                 /* l ankle */ -0.52, 0.,
00251                                 /* r shoulder */ 2.1, -0.35,
00252                                 /* r elbow */ 1.40, 1.40,
00253                                 /* r wrist/hand */ 0., 0.,
00254                                 /* r hip */ 0., 0., -0.52,
00255                                 /* r knee */ 1.05,
00256                                 /* r ankle */ -0.52, 0.,
00257                                 /* time */ 3.0);
00258 
00259       __hummot_if->set_msgid(msg_action->id());
00260     }
00261     else if (msg_action->is_of_type<HumanoidMotionInterface::ParkMessage>()) {
00262       motion::timed_move_joints(__almotion,
00263                                 /* head */ 0., 0.,
00264                                 /* l shoulder */ 1.58, 0.15,
00265                                 /* l elbow */ -1.20, -1.1,
00266                                 /* l wrist/hand */ 0., 0.,
00267                                 /* l hip */ -0.08, 0., -0.85,
00268                                 /* l knee */ 2.2,
00269                                 /* l ankle */ -1.23, 0.,
00270                                 /* r shoulder */ 1.55, -0.15,
00271                                 /* r elbow */ 1.2, 1.1,
00272                                 /* r wrist/hand */ 0., 0.,
00273                                 /* r hip */ -0.08, 0., -0.85,
00274                                 /* r knee */ 2.2,
00275                                 /* r ankle */ -1.23, 0.,
00276                                 /* time */ 3.0);
00277 
00278       __hummot_if->set_msgid(msg_action->id());
00279     }
00280     else if (msg_action->is_of_type<HumanoidMotionInterface::StandupMessage>()) {
00281       if (__motion_task) {
00282         __motion_task->exitTask();
00283       }
00284 
00285       HumanoidMotionInterface::StandupMessage* msg = dynamic_cast<HumanoidMotionInterface::StandupMessage*>(msg_action);
00286       __sensor_if->read();
00287       __motion_task.reset(new NaoQiMotionStandupTask(__almotion, msg->from_pos(),
00288                                                      __sensor_if->accel_x(),
00289                                                      __sensor_if->accel_y(),
00290                                                      __sensor_if->accel_z()));
00291       __thread_pool->enqueue(__motion_task);
00292 
00293       __hummot_if->set_msgid(msg->id());
00294     }
00295     else if (msg_action->is_of_type<HumanoidMotionInterface::KickMessage>()) {
00296       HumanoidMotionInterface::KickMessage* msg = dynamic_cast<HumanoidMotionInterface::KickMessage*>(msg_action);
00297       if (__motion_task) {
00298         __motion_task->exitTask();
00299       }
00300       __motion_task.reset(new NaoQiMotionKickTask(__almotion, msg->leg()));
00301       __thread_pool->enqueue(__motion_task);
00302 
00303       __hummot_if->set_msgid(msg->id());
00304     }
00305 
00306     msg_action->unref();
00307   }
00308   else {
00309     if (msg_walk_velocity) {
00310       if ( (msg_walk_velocity->speed() < 0.) || (msg_walk_velocity->speed() > 1.)) {
00311         logger->log_warn(name(), "Walk velocity speed %f out of range [0.0..1.0],",
00312                          "ignoring command", msg_walk_velocity->speed());
00313       } else {
00314         try {
00315           __almotion->setWalkTargetVelocity(msg_walk_velocity->x(), msg_walk_velocity->y(), msg_walk_velocity->theta(),
00316                                             msg_walk_velocity->speed());
00317         } catch (AL::ALError &e) {
00318           logger->log_warn(name(), "WalkVelocity command failed: %s", e.what());
00319         }
00320       }
00321       __hummot_if->set_msgid(msg_walk_velocity->id());
00322       msg_walk_velocity->unref();
00323     }
00324 
00325     if (msg_move_head) {
00326       AL::ALValue names  = AL::ALValue::array("HeadYaw", "HeadPitch");
00327       AL::ALValue angles = AL::ALValue::array(msg_move_head->yaw(), msg_move_head->pitch());
00328 
00329       __almotion->setAngles(names, angles, msg_move_head->speed());
00330       msg_move_head->unref();
00331     }
00332   }
00333 
00334 
00335 
00336 }