Fawkes API
Fawkes Development Version
|
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 }