Fawkes API  Fawkes Development Version
dcm_thread.cpp
00001 
00002 /***************************************************************************
00003  *  dcm_thread.cpp - Provide NaoQi DCM to Fawkes
00004  *
00005  *  Created: Tue May 31 15:00:54 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 "dcm_thread.h"
00024 #include "motion_utils.h"
00025 
00026 #include <alproxies/allauncherproxy.h>
00027 #include <alproxies/dcmproxy.h>
00028 #include <alproxies/almotionproxy.h>
00029 #include <alproxies/almemoryproxy.h>
00030 #include <alcore/alerror.h>
00031 #include <almemoryfastaccess/almemoryfastaccess.h>
00032 
00033 #include <boost/bind.hpp>
00034 
00035 #include <interfaces/NaoJointStiffnessInterface.h>
00036 #include <interfaces/NaoSensorInterface.h>
00037 
00038 using namespace fawkes;
00039 
00040 enum SensorType {  HEAD_PITCH = 0, HEAD_YAW,
00041                    L_SHOULDER_PITCH, L_SHOULDER_ROLL, L_ELBOW_YAW, L_ELBOW_ROLL,
00042                    L_WRIST_YAW, L_HAND,
00043                    L_HIP_YAW_PITCH, L_HIP_ROLL, L_HIP_PITCH, L_KNEE_PITCH,
00044                    L_ANKLE_PITCH, L_ANKLE_ROLL,
00045                    R_SHOULDER_PITCH, R_SHOULDER_ROLL, R_ELBOW_YAW, R_ELBOW_ROLL,
00046                    R_WRIST_YAW, R_HAND,
00047                    R_HIP_YAW_PITCH, R_HIP_ROLL, R_HIP_PITCH, R_KNEE_PITCH,
00048                    R_ANKLE_PITCH, R_ANKLE_ROLL,
00049                    STIFF_HEAD_PITCH, STIFF_HEAD_YAW,
00050                    STIFF_L_SHOULDER_PITCH, STIFF_L_SHOULDER_ROLL,
00051                    STIFF_L_ELBOW_YAW, STIFF_L_ELBOW_ROLL,
00052                    STIFF_L_WRIST_YAW, STIFF_L_HAND,
00053                    STIFF_L_HIP_YAW_PITCH, STIFF_L_HIP_ROLL, STIFF_L_HIP_PITCH,
00054                    STIFF_L_KNEE_PITCH, STIFF_L_ANKLE_PITCH, STIFF_L_ANKLE_ROLL,
00055                    STIFF_R_SHOULDER_PITCH, STIFF_R_SHOULDER_ROLL,
00056                    STIFF_R_ELBOW_YAW, STIFF_R_ELBOW_ROLL,
00057                    STIFF_R_WRIST_YAW, STIFF_R_HAND,
00058                    STIFF_R_HIP_YAW_PITCH, STIFF_R_HIP_ROLL, STIFF_R_HIP_PITCH,
00059                    STIFF_R_KNEE_PITCH, STIFF_R_ANKLE_PITCH, STIFF_R_ANKLE_ROLL,
00060                    ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_REF, ANGLE_X, ANGLE_Y,
00061                    L_FSR_FL, L_FSR_FR, L_FSR_RL, L_FSR_RR,
00062                    R_FSR_FL, R_FSR_FR, R_FSR_RL, R_FSR_RR,
00063                    L_COP_X, L_COP_Y, L_TOTAL_WEIGHT, R_COP_X, R_COP_Y,
00064                    R_TOTAL_WEIGHT,
00065                    ULTRASONIC_DIRECTION, ULTRASONIC_DISTANCE,
00066                    ULTRASONIC_DISTANCE_LEFT_0, ULTRASONIC_DISTANCE_LEFT_1,
00067                    ULTRASONIC_DISTANCE_LEFT_2, ULTRASONIC_DISTANCE_LEFT_3,
00068                    ULTRASONIC_DISTANCE_RIGHT_0, ULTRASONIC_DISTANCE_RIGHT_1,
00069                    ULTRASONIC_DISTANCE_RIGHT_2, ULTRASONIC_DISTANCE_RIGHT_3,
00070                    L_FOOT_BUMPER_L, L_FOOT_BUMPER_R,
00071                    R_FOOT_BUMPER_L, R_FOOT_BUMPER_R,
00072                    HEAD_TOUCH_FRONT, HEAD_TOUCH_MIDDLE, HEAD_TOUCH_REAR,
00073                    CHEST_BUTTON, BATTERY_CHARGE,
00074                    SensorTypeN};
00075 
00076 enum StiffnessJoint { STIFFJ_HEAD_PITCH = 0, STIFFJ_HEAD_YAW,
00077                       STIFFJ_L_SHOULDER_PITCH, STIFFJ_L_SHOULDER_ROLL,
00078                       STIFFJ_L_ELBOW_YAW, STIFFJ_L_ELBOW_ROLL,
00079                       STIFFJ_L_HIP_YAW_PITCH, STIFFJ_L_HIP_ROLL,
00080                       STIFFJ_L_HIP_PITCH, STIFFJ_L_KNEE_PITCH,
00081                       STIFFJ_L_ANKLE_PITCH, STIFFJ_L_ANKLE_ROLL,
00082                       STIFFJ_R_SHOULDER_PITCH, STIFFJ_R_SHOULDER_ROLL,
00083                       STIFFJ_R_ELBOW_YAW, STIFFJ_R_ELBOW_ROLL,
00084                       STIFFJ_R_HIP_YAW_PITCH, STIFFJ_R_HIP_ROLL,
00085                       STIFFJ_R_HIP_PITCH, STIFFJ_R_KNEE_PITCH,
00086                       STIFFJ_R_ANKLE_PITCH, STIFFJ_R_ANKLE_ROLL,
00087                       STIFFJ_L_WRIST_YAW, STIFFJ_L_HAND,
00088                       STIFFJ_R_WRIST_YAW, STIFFJ_R_HAND,
00089                       StiffnessJointN };
00090 
00091 #define ACCELEROMETER_G_FACTOR 56.
00092 
00093 // Clipping as suggested by Aldebaran at RoboCup 2008, Suzhou
00094 // Changed head yaw to +/- 1.2 according to Nao v3 red book
00095 #define HEAD_YAW_MIN          -1.2
00096 #define HEAD_YAW_MAX           1.2
00097 // New values after RoboCup Workshop 2009 at Aldebaran, Paris
00098 #define L_SHOULDER_PITCH_MIN  -1.7
00099 #define L_SHOULDER_PITCH_MAX   1.7
00100 #define R_SHOULDER_PITCH_MIN  -1.7
00101 #define R_SHOULDER_PITCH_MAX   1.7
00102 
00103 #define CLIP_VALUE(V, value) clip_value(value, V ## _MIN, V ## _MAX)
00104 
00105 /** Clip value to given constraints.
00106  * @param value value to clop
00107  * @param min minimum value
00108  * @param max maximum value
00109  * @return clipped value
00110  */
00111 static inline float
00112 clip_value(float value, float min, float max)
00113 {
00114   if (value < min) value = min;
00115   if (value > max) value = max;
00116   return value;
00117 }
00118 
00119 
00120 /** Thread to write data at full DCM frequency.
00121  * This thread is woken up by the DCM callback and publishes new data
00122  * if there is a reader for any of the high frequency interfaces. It
00123  * is also responsible for processing incoming commands.
00124  */
00125 class NaoQiDCMThread::HighFreqThread : public Thread
00126 {
00127  public:
00128   /** Constructor.
00129    * @param parent parent NaoQiDCMThread to call.
00130    */
00131   HighFreqThread(NaoQiDCMThread *parent)
00132     : Thread("NaoQiDCMThread::HighFreqThread", Thread::OPMODE_WAITFORWAKEUP),
00133       __parent(parent)
00134   {
00135     set_coalesce_wakeups(true);
00136   }
00137 
00138   virtual void loop()
00139   {
00140     __parent->read_values();
00141     if ( (__parent->__joint_pos_highfreq_if->num_readers() > 0) ||
00142          (__parent->__joint_stiffness_highfreq_if->num_readers() > 0) ||
00143          (__parent->__sensor_if->num_readers() > 0) )
00144     {
00145       __parent->update_interfaces(__parent->__joint_pos_highfreq_if,
00146                                   __parent->__joint_stiffness_highfreq_if,
00147                                   __parent->__sensor_highfreq_if);
00148     }
00149 
00150     __parent->process_messages();
00151   }
00152 
00153 
00154  private:
00155   NaoQiDCMThread *__parent;
00156 };
00157 
00158 
00159 
00160 /** @class NaoQiDCMThread "dcm_thread.h"
00161  * Thread to provide DCM to Fawkes.
00162  * This thread opens a DCM proxy and updates information about hardware
00163  * in the blackboard and provides basic setting functionality to move
00164  * specific servos.
00165  *
00166  * The DCM thread writes to two sets of interfaces, at a high and a lower
00167  * frequency. The high frequency data is written if there is a reader at
00168  * each DCM callback, which results in a frequency of about 100Hz. This
00169  * should only be used if necessary, for example for custom motion pattern
00170  * generating plugins. Otherwise, and especially if in a thread hooked
00171  * into the main loop, use the lower frequency interfaces. These are
00172  * written during the sensor hook.
00173  *
00174  * @author Tim Niemueller
00175  */
00176 
00177 /** Constructor. */
00178 NaoQiDCMThread::NaoQiDCMThread()
00179   : Thread("NaoQiDCMThread", Thread::OPMODE_WAITFORWAKEUP),
00180     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
00181 {
00182 }
00183 
00184 
00185 /** Destructor. */
00186 NaoQiDCMThread::~NaoQiDCMThread()
00187 {
00188 }
00189 
00190 
00191 void
00192 NaoQiDCMThread::init()
00193 {
00194   // Is the DCM running ?
00195   try {
00196     AL::ALPtr<AL::ALLauncherProxy> launcher(new AL::ALLauncherProxy(naoqi_broker));
00197     bool is_dcm_available = launcher->isModulePresent("DCM");
00198     bool is_almotion_available = launcher->isModulePresent("ALMotion");
00199 
00200     if (! is_dcm_available) {
00201       throw Exception("DCMThread: NaoQi DCM is not available");
00202     }
00203     if (! is_almotion_available) {
00204       throw Exception("DCMThread: ALMotion is not available");
00205     }
00206   } catch (AL::ALError& e) {
00207     throw Exception("Checking module availability failed: %s",
00208                     e.toString().c_str());
00209   }
00210 
00211   __dcm = naoqi_broker->getDcmProxy();
00212   __almotion = naoqi_broker->getMotionProxy();
00213 
00214   try {
00215     AL::ALPtr<AL::ALMemoryProxy> almemory = naoqi_broker->getMemoryProxy();
00216     std::string version = almemory->getData("RobotConfig/Body/BaseVersion", 0);
00217     unsigned int num_joints = __almotion->getJointNames("Body").size();
00218     if (num_joints == 26) {
00219       __robot_type = NaoJointPositionInterface::ROBOTYPE_ACADEMIC;
00220     } else {
00221       __robot_type = NaoJointPositionInterface::ROBOTYPE_ROBOCUP;
00222     }
00223     __robot_version[2] = __robot_version[3] = 0;
00224     if (version[0] == 'V')  version = version.substr(1);
00225     std::string::size_type pos;
00226     if ((pos = version.find_first_of(".")) != std::string::npos) {
00227       std::string version_major = version.substr(0, pos);
00228       std::string version_minor = version.substr(pos+1);
00229       __robot_version[0] = atoi(version_major.c_str());
00230       __robot_version[1] = atoi(version_minor.c_str());
00231     }
00232     __usboard_version =
00233       almemory->getData("Device/DeviceList/USBoard/ProgVersion", 0);
00234   } catch (AL::ALError &e) {
00235     throw Exception("Retrieving robot info failed: %s", e.toString().c_str());
00236   }
00237 
00238   __memfa.reset(new AL::ALMemoryFastAccess());
00239 
00240   std::string prefix = "Device/SubDeviceList/";
00241 
00242   // Initialize fast memory access
00243   std::vector<std::string> keys;
00244   keys.resize(SensorTypeN);
00245   __values.resize(SensorTypeN);
00246 
00247   keys[HEAD_PITCH]       = prefix + "HeadPitch/Position/Sensor/Value";
00248   keys[HEAD_YAW]         = prefix + "HeadYaw/Position/Sensor/Value";
00249   keys[L_SHOULDER_PITCH] = prefix + "LShoulderPitch/Position/Sensor/Value";
00250   keys[L_SHOULDER_ROLL]  = prefix + "LShoulderRoll/Position/Sensor/Value";
00251   keys[L_ELBOW_YAW]      = prefix + "LElbowYaw/Position/Sensor/Value";
00252   keys[L_ELBOW_ROLL]     = prefix + "LElbowRoll/Position/Sensor/Value";
00253   keys[L_WRIST_YAW]      = prefix + "LWristYaw/Position/Sensor/Value";
00254   keys[L_HAND]           = prefix + "LHand/Position/Sensor/Value";
00255   keys[L_HIP_YAW_PITCH]  = prefix + "LHipYawPitch/Position/Sensor/Value";
00256   keys[L_HIP_ROLL]       = prefix + "LHipRoll/Position/Sensor/Value";
00257   keys[L_HIP_PITCH]      = prefix + "LHipPitch/Position/Sensor/Value";
00258   keys[L_KNEE_PITCH]     = prefix + "LKneePitch/Position/Sensor/Value";
00259   keys[L_ANKLE_PITCH]    = prefix + "LAnklePitch/Position/Sensor/Value";
00260   keys[L_ANKLE_ROLL]     = prefix + "LAnkleRoll/Position/Sensor/Value";
00261 
00262   keys[R_SHOULDER_PITCH] = prefix + "RShoulderPitch/Position/Sensor/Value";
00263   keys[R_SHOULDER_ROLL]  = prefix + "RShoulderRoll/Position/Sensor/Value";
00264   keys[R_ELBOW_YAW]      = prefix + "RElbowYaw/Position/Sensor/Value";
00265   keys[R_ELBOW_ROLL]     = prefix + "RElbowRoll/Position/Sensor/Value";
00266   keys[R_WRIST_YAW]      = prefix + "RWristYaw/Position/Sensor/Value";
00267   keys[R_HAND]           = prefix + "RHand/Position/Sensor/Value";
00268   keys[R_HIP_YAW_PITCH]  = prefix + "RHipYawPitch/Position/Sensor/Value";
00269   keys[R_HIP_ROLL]       = prefix + "RHipRoll/Position/Sensor/Value";
00270   keys[R_HIP_PITCH]      = prefix + "RHipPitch/Position/Sensor/Value";
00271   keys[R_KNEE_PITCH]     = prefix + "RKneePitch/Position/Sensor/Value";
00272   keys[R_ANKLE_PITCH]    = prefix + "RAnklePitch/Position/Sensor/Value";
00273   keys[R_ANKLE_ROLL]     = prefix + "RAnkleRoll/Position/Sensor/Value";
00274 
00275   keys[STIFF_HEAD_PITCH]       = prefix + "HeadPitch/Hardness/Actuator/Value";
00276   keys[STIFF_HEAD_YAW]         = prefix + "HeadYaw/Hardness/Actuator/Value";
00277   keys[STIFF_L_SHOULDER_PITCH] = prefix + "LShoulderPitch/Hardness/Actuator/Value";
00278   keys[STIFF_L_SHOULDER_ROLL]  = prefix + "LShoulderRoll/Hardness/Actuator/Value";
00279   keys[STIFF_L_ELBOW_YAW]      = prefix + "LElbowYaw/Hardness/Actuator/Value";
00280   keys[STIFF_L_ELBOW_ROLL]     = prefix + "LElbowRoll/Hardness/Actuator/Value";
00281   keys[STIFF_L_WRIST_YAW]      = prefix + "LWristYaw/Hardness/Actuator/Value";
00282   keys[STIFF_L_HAND]           = prefix + "LHand/Hardness/Actuator/Value";
00283   keys[STIFF_L_HIP_YAW_PITCH]  = prefix + "LHipYawPitch/Hardness/Actuator/Value";
00284   keys[STIFF_L_HIP_ROLL]       = prefix + "LHipRoll/Hardness/Actuator/Value";
00285   keys[STIFF_L_HIP_PITCH]      = prefix + "LHipPitch/Hardness/Actuator/Value";
00286   keys[STIFF_L_KNEE_PITCH]     = prefix + "LKneePitch/Hardness/Actuator/Value";
00287   keys[STIFF_L_ANKLE_PITCH]    = prefix + "LAnklePitch/Hardness/Actuator/Value";
00288   keys[STIFF_L_ANKLE_ROLL]     = prefix + "LAnkleRoll/Hardness/Actuator/Value";
00289 
00290   keys[STIFF_R_SHOULDER_PITCH] = prefix + "RShoulderPitch/Hardness/Actuator/Value";
00291   keys[STIFF_R_SHOULDER_ROLL]  = prefix + "RShoulderRoll/Hardness/Actuator/Value";
00292   keys[STIFF_R_ELBOW_YAW]      = prefix + "RElbowYaw/Hardness/Actuator/Value";
00293   keys[STIFF_R_ELBOW_ROLL]     = prefix + "RElbowRoll/Hardness/Actuator/Value";
00294   keys[STIFF_R_WRIST_YAW]      = prefix + "RWristYaw/Hardness/Actuator/Value";
00295   keys[STIFF_R_HAND]           = prefix + "RHand/Hardness/Actuator/Value";
00296   keys[STIFF_R_HIP_YAW_PITCH]  = prefix + "RHipYawPitch/Hardness/Actuator/Value";
00297   keys[STIFF_R_HIP_ROLL]       = prefix + "RHipRoll/Hardness/Actuator/Value";
00298   keys[STIFF_R_HIP_PITCH]      = prefix + "RHipPitch/Hardness/Actuator/Value";
00299   keys[STIFF_R_KNEE_PITCH]     = prefix + "RKneePitch/Hardness/Actuator/Value";
00300   keys[STIFF_R_ANKLE_PITCH]    = prefix + "RAnklePitch/Hardness/Actuator/Value";
00301   keys[STIFF_R_ANKLE_ROLL]     = prefix + "RAnkleRoll/Hardness/Actuator/Value";
00302 
00303   // Inertial sensors
00304   keys[ACC_X]   = prefix + "InertialSensor/AccX/Sensor/Value";
00305   keys[ACC_Y]   = prefix + "InertialSensor/AccY/Sensor/Value";
00306   keys[ACC_Z]   = prefix + "InertialSensor/AccZ/Sensor/Value";
00307   keys[GYR_X]   = prefix + "InertialSensor/GyrX/Sensor/Value";
00308   keys[GYR_Y]   = prefix + "InertialSensor/GyrY/Sensor/Value";
00309   keys[GYR_REF] = prefix + "InertialSensor/GyrRef/Sensor/Value";
00310   keys[ANGLE_X] = prefix + "InertialSensor/AngleX/Sensor/Value";
00311   keys[ANGLE_Y] = prefix + "InertialSensor/AngleY/Sensor/Value";
00312 
00313   // FSR sensors
00314   keys[L_FSR_FL]     = prefix + "LFoot/FSR/FrontLeft/Sensor/Value";
00315   keys[L_FSR_FR]     = prefix + "LFoot/FSR/FrontRight/Sensor/Value";
00316   keys[L_FSR_RL]     = prefix + "LFoot/FSR/RearLeft/Sensor/Value";
00317   keys[L_FSR_RR]     = prefix + "LFoot/FSR/RearRight/Sensor/Value";
00318   keys[R_FSR_FL]     = prefix + "RFoot/FSR/FrontLeft/Sensor/Value";
00319   keys[R_FSR_FR]     = prefix + "RFoot/FSR/FrontRight/Sensor/Value";
00320   keys[R_FSR_RL]     = prefix + "RFoot/FSR/RearLeft/Sensor/Value";
00321   keys[R_FSR_RR]     = prefix + "RFoot/FSR/RearRight/Sensor/Value";
00322   keys[L_COP_X]        = prefix + "LFoot/FSR/CenterOfPressure/X/Sensor/Value";
00323   keys[L_COP_Y]        = prefix + "LFoot/FSR/CenterOfPressure/Y/Sensor/Value";
00324   keys[L_TOTAL_WEIGHT] = prefix + "LFoot/FSR/TotalWeight/Sensor/Value";
00325   keys[R_COP_X]        = prefix + "RFoot/FSR/CenterOfPressure/X/Sensor/Value";
00326   keys[R_COP_Y]        = prefix + "RFoot/FSR/CenterOfPressure/Y/Sensor/Value";
00327   keys[R_TOTAL_WEIGHT] = prefix + "RFoot/FSR/TotalWeight/Sensor/Value";
00328 
00329   // Ultrasonic
00330   keys[ULTRASONIC_DIRECTION]        = prefix + "US/Actuator/Value";
00331   keys[ULTRASONIC_DISTANCE]         = prefix + "US/Sensor/Value";
00332 
00333   keys[ULTRASONIC_DISTANCE_LEFT_0]  = prefix + "US/Left/Sensor/Value";
00334   keys[ULTRASONIC_DISTANCE_LEFT_1]  = prefix + "US/Left/Sensor/Value1";
00335   keys[ULTRASONIC_DISTANCE_LEFT_2]  = prefix + "US/Left/Sensor/Value2";
00336   keys[ULTRASONIC_DISTANCE_LEFT_3]  = prefix + "US/Left/Sensor/Value3";
00337 
00338   keys[ULTRASONIC_DISTANCE_RIGHT_0] = prefix + "US/Right/Sensor/Value";
00339   keys[ULTRASONIC_DISTANCE_RIGHT_1] = prefix + "US/Right/Sensor/Value1";
00340   keys[ULTRASONIC_DISTANCE_RIGHT_2] = prefix + "US/Right/Sensor/Value2";
00341   keys[ULTRASONIC_DISTANCE_RIGHT_3] = prefix + "US/Right/Sensor/Value3";
00342 
00343   // Bumpers and Buttons
00344   keys[L_FOOT_BUMPER_L]     = prefix + "LFoot/Bumper/Left/Sensor/Value";
00345   keys[L_FOOT_BUMPER_R]     = prefix + "LFoot/Bumper/Right/Sensor/Value";
00346   keys[R_FOOT_BUMPER_L]     = prefix + "RFoot/Bumper/Left/Sensor/Value";
00347   keys[R_FOOT_BUMPER_R]     = prefix + "RFoot/Bumper/Right/Sensor/Value";
00348 
00349   keys[HEAD_TOUCH_FRONT]     = prefix + "Head/Touch/Front/Sensor/Value";
00350   keys[HEAD_TOUCH_MIDDLE]    = prefix + "Head/Touch/Middle/Sensor/Value";
00351   keys[HEAD_TOUCH_REAR]      = prefix + "Head/Touch/Rear/Sensor/Value";
00352 
00353   keys[CHEST_BUTTON]     = prefix + "ChestBoard/Button/Sensor/Value";
00354 
00355   // Battery
00356   keys[BATTERY_CHARGE]     = prefix + "Battery/Charge/Sensor/Value";
00357 
00358   try {
00359     __memfa->ConnectToVariables(naoqi_broker, keys, false);
00360   } catch (AL::ALError &e) {
00361     throw Exception("Failed to setup fast memory access: %s",
00362                     e.toString().c_str());
00363   }
00364 
00365 
00366   // Setup alias for setting stiffness, reuse fastmem keys vector
00367   if (__robot_type == NaoJointPositionInterface::ROBOTYPE_ROBOCUP) {
00368     __alljoint_names.arraySetSize(22);
00369   } else {
00370     __alljoint_names.arraySetSize(26);
00371     __alljoint_names[STIFFJ_L_WRIST_YAW]      = "LWristYaw";
00372     __alljoint_names[STIFFJ_L_HAND]           = "LHand";
00373     __alljoint_names[STIFFJ_R_WRIST_YAW]      = "RWristYaw";
00374     __alljoint_names[STIFFJ_R_HAND]           = "RHand";
00375   }
00376   __alljoint_names[STIFFJ_HEAD_PITCH]       = "HeadPitch";
00377   __alljoint_names[STIFFJ_HEAD_YAW]         = "HeadYaw";
00378   __alljoint_names[STIFFJ_L_SHOULDER_PITCH] = "LShoulderPitch";
00379   __alljoint_names[STIFFJ_L_SHOULDER_ROLL]  = "LShoulderRoll";
00380   __alljoint_names[STIFFJ_L_ELBOW_YAW]      = "LElbowYaw";
00381   __alljoint_names[STIFFJ_L_ELBOW_ROLL]     = "LElbowRoll";
00382   __alljoint_names[STIFFJ_L_HIP_YAW_PITCH]  = "LHipYawPitch";
00383   __alljoint_names[STIFFJ_L_HIP_ROLL]       = "LHipRoll";
00384   __alljoint_names[STIFFJ_L_HIP_PITCH]      = "LHipPitch";
00385   __alljoint_names[STIFFJ_L_KNEE_PITCH]     = "LKneePitch";
00386   __alljoint_names[STIFFJ_L_ANKLE_PITCH]    = "LAnklePitch";
00387   __alljoint_names[STIFFJ_L_ANKLE_ROLL]     = "LAnkleRoll";
00388 
00389   __alljoint_names[STIFFJ_R_SHOULDER_PITCH] = "RShoulderPitch";
00390   __alljoint_names[STIFFJ_R_SHOULDER_ROLL]  = "RShoulderRoll";
00391   __alljoint_names[STIFFJ_R_ELBOW_YAW]      = "RElbowYaw";
00392   __alljoint_names[STIFFJ_R_ELBOW_ROLL]     = "RElbowRoll";
00393   __alljoint_names[STIFFJ_R_HIP_YAW_PITCH]  = "RHipYawPitch";
00394   __alljoint_names[STIFFJ_R_HIP_ROLL]       = "RHipRoll";
00395   __alljoint_names[STIFFJ_R_HIP_PITCH]      = "RHipPitch";
00396   __alljoint_names[STIFFJ_R_KNEE_PITCH]     = "RKneePitch";
00397   __alljoint_names[STIFFJ_R_ANKLE_PITCH]    = "RAnklePitch";
00398   __alljoint_names[STIFFJ_R_ANKLE_ROLL]     = "RAnkleRoll";
00399 
00400   try {
00401     AL::ALValue setJointStiffnessAlias;
00402     // Alias for all joint stiffness
00403     setJointStiffnessAlias.clear();
00404     setJointStiffnessAlias.arraySetSize(2);
00405     setJointStiffnessAlias[0] = std::string("setJointStiffness");
00406     setJointStiffnessAlias[1].arraySetSize(26);
00407 
00408     // stiffness list
00409     int offset = STIFF_HEAD_PITCH - HEAD_PITCH;
00410     for (int i = HEAD_PITCH; i <= R_ANKLE_ROLL; ++i) {
00411       setJointStiffnessAlias[1][i] = keys[i + offset];
00412     }
00413 
00414     __dcm->createAlias(setJointStiffnessAlias);
00415   } catch (AL::ALError &e) {
00416     __memfa.reset();
00417     throw Exception("Failed to create SetJointStiffness alias: %s",
00418                     e.toString().c_str());
00419   }
00420 
00421   __joint_pos_if =
00422     blackboard->open_for_writing<NaoJointPositionInterface>("Nao Joint Positions");
00423   __joint_stiffness_if =
00424     blackboard->open_for_writing<NaoJointStiffnessInterface>
00425                                 ("Nao Joint Stiffness");
00426   __sensor_if =
00427     blackboard->open_for_writing<NaoSensorInterface>("Nao Sensors");
00428 
00429   __joint_pos_highfreq_if =
00430     blackboard->open_for_writing<NaoJointPositionInterface>
00431                                 ("Nao Joint Positions HF");
00432   __joint_stiffness_highfreq_if =
00433     blackboard->open_for_writing<NaoJointStiffnessInterface>
00434                                 ("Nao Joint Stiffness HF");
00435   __sensor_highfreq_if =
00436     blackboard->open_for_writing<NaoSensorInterface>("Nao Sensors HF");
00437 
00438 
00439   // Get all values from ALMemory using fastaccess
00440   __dcm_time = __dcm->getTime(0);
00441   __memfa->GetValues(__values);
00442 
00443   __joint_pos_if->set_robot_type(__robot_type);
00444   __joint_pos_if->set_robot_version(__robot_version);
00445   __joint_pos_highfreq_if->set_robot_type(__robot_type);
00446   __joint_pos_highfreq_if->set_robot_version(__robot_version);
00447 
00448   __sensor_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
00449   __sensor_highfreq_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
00450 
00451   // Write once the current data on startup
00452   update_interfaces(__joint_pos_if, __joint_stiffness_if, __sensor_if);
00453   update_interfaces(__joint_pos_highfreq_if, __joint_stiffness_highfreq_if,
00454                     __sensor_highfreq_if);
00455 
00456   __highfreq_thread = new NaoQiDCMThread::HighFreqThread(this);
00457   __highfreq_thread->start();
00458 
00459   __dcm_sigconn =
00460     __dcm->getGenericProxy()->getModule()->
00461     atPostProcess(boost::bind(&NaoQiDCMThread::dcm_callback, this));
00462   
00463   /*
00464   AL::ALValue cmd;
00465   cmd.arraySetSize(3);
00466   cmd[0] = std::string("setJointStiffness");
00467   cmd[1] = std::string("Merge");
00468   cmd[2].arraySetSize(1);
00469   cmd[2][0].arraySetSize(2);
00470   cmd[2][0][0] = 1.0;
00471   cmd[2][0][1] = __dcm_time + 500;
00472   __dcm->set(cmd);
00473 
00474   send_command("HeadPitch/Position/Sensor/Value", 0.4, 1000);
00475   */
00476 }
00477 
00478 
00479 void
00480 NaoQiDCMThread::dcm_callback()
00481 {
00482   __highfreq_thread->wakeup();
00483 }
00484 
00485 void
00486 NaoQiDCMThread::finalize()
00487 {
00488   __dcm_sigconn.disconnect();
00489 
00490   __highfreq_thread->cancel();
00491   __highfreq_thread->join();
00492   delete __highfreq_thread;
00493 
00494   blackboard->close(__joint_pos_if);
00495   blackboard->close(__joint_stiffness_if);
00496   blackboard->close(__sensor_if);
00497   blackboard->close(__joint_pos_highfreq_if);
00498   blackboard->close(__joint_stiffness_highfreq_if);
00499   blackboard->close(__sensor_highfreq_if);
00500   __joint_pos_if = NULL;
00501   __joint_stiffness_if = NULL;
00502   __sensor_if = NULL;
00503   __joint_pos_highfreq_if = NULL;
00504   __joint_stiffness_highfreq_if = NULL;
00505   __sensor_highfreq_if = NULL;
00506 
00507   __memfa.reset();
00508   __dcm.reset();
00509 }
00510 
00511 
00512 void
00513 NaoQiDCMThread::read_values()
00514 {
00515   __values.lock();
00516   __dcm_time = __dcm->getTime(0);
00517   __memfa->GetValues(__values);
00518   __values.unlock();
00519 }
00520 
00521 
00522 void
00523 NaoQiDCMThread::loop()
00524 {
00525   update_interfaces(__joint_pos_if, __joint_stiffness_if, __sensor_if);
00526 }
00527 
00528 void
00529 NaoQiDCMThread::update_interfaces(NaoJointPositionInterface *joint_pos_if,
00530                                   NaoJointStiffnessInterface *joint_stiffness_if,
00531                                   NaoSensorInterface *sensor_if)
00532 {
00533   // Joint Position
00534   // Head
00535   joint_pos_if->set_head_yaw(__values[HEAD_YAW]);
00536   joint_pos_if->set_head_pitch(__values[HEAD_PITCH]);
00537   // Arms
00538   joint_pos_if->set_l_shoulder_pitch(__values[L_SHOULDER_PITCH]);
00539   joint_pos_if->set_l_shoulder_roll(__values[L_SHOULDER_ROLL]);
00540   joint_pos_if->set_l_elbow_yaw(__values[L_ELBOW_YAW]);
00541   joint_pos_if->set_l_elbow_roll(__values[L_ELBOW_ROLL]);
00542   joint_pos_if->set_l_wrist_yaw(__values[L_WRIST_YAW]);
00543   joint_pos_if->set_l_hand(__values[L_HAND]);
00544   joint_pos_if->set_r_shoulder_pitch(__values[R_SHOULDER_PITCH]);
00545   joint_pos_if->set_r_shoulder_roll(__values[R_SHOULDER_ROLL]);
00546   joint_pos_if->set_r_elbow_yaw(__values[R_ELBOW_YAW]);
00547   joint_pos_if->set_r_elbow_roll(__values[R_ELBOW_ROLL]);
00548   joint_pos_if->set_r_wrist_yaw(__values[R_WRIST_YAW]);
00549   joint_pos_if->set_r_hand(__values[R_HAND]);
00550   // Hip
00551   joint_pos_if->set_l_hip_yaw_pitch(__values[L_HIP_YAW_PITCH]);
00552   joint_pos_if->set_l_hip_pitch(__values[L_HIP_PITCH]);
00553   joint_pos_if->set_l_hip_roll(__values[L_HIP_ROLL]);
00554   joint_pos_if->set_r_hip_yaw_pitch(__values[R_HIP_YAW_PITCH]);
00555   joint_pos_if->set_r_hip_pitch(__values[R_HIP_PITCH]);
00556   joint_pos_if->set_r_hip_roll(__values[R_HIP_ROLL]);
00557   // Knees
00558   joint_pos_if->set_l_knee_pitch(__values[L_KNEE_PITCH]);
00559   joint_pos_if->set_r_knee_pitch(__values[R_KNEE_PITCH]);
00560   // Feet
00561   joint_pos_if->set_l_ankle_pitch(__values[L_ANKLE_PITCH]);
00562   joint_pos_if->set_l_ankle_roll(__values[L_ANKLE_ROLL]);
00563   joint_pos_if->set_r_ankle_pitch(__values[R_ANKLE_PITCH]);
00564   joint_pos_if->set_r_ankle_roll(__values[R_ANKLE_ROLL]);
00565 
00566   joint_pos_if->set_time(__dcm_time);
00567 
00568   // Joint Stiffness
00569   // Head
00570   joint_stiffness_if->set_head_yaw(__values[STIFF_HEAD_YAW]);
00571   joint_stiffness_if->set_head_pitch(__values[STIFF_HEAD_PITCH]);
00572   // Arms
00573   joint_stiffness_if->set_l_shoulder_pitch(__values[STIFF_L_SHOULDER_PITCH]);
00574   joint_stiffness_if->set_l_shoulder_roll(__values[STIFF_L_SHOULDER_ROLL]);
00575   joint_stiffness_if->set_l_elbow_yaw(__values[STIFF_L_ELBOW_YAW]);
00576   joint_stiffness_if->set_l_elbow_roll(__values[STIFF_L_ELBOW_ROLL]);
00577   joint_stiffness_if->set_l_wrist_yaw(__values[STIFF_L_WRIST_YAW]);
00578   joint_stiffness_if->set_l_hand(__values[STIFF_L_HAND]);
00579   joint_stiffness_if->set_r_shoulder_pitch(__values[STIFF_R_SHOULDER_PITCH]);
00580   joint_stiffness_if->set_r_shoulder_roll(__values[STIFF_R_SHOULDER_ROLL]);
00581   joint_stiffness_if->set_r_elbow_yaw(__values[STIFF_R_ELBOW_YAW]);
00582   joint_stiffness_if->set_r_elbow_roll(__values[STIFF_R_ELBOW_ROLL]);
00583   joint_stiffness_if->set_r_wrist_yaw(__values[STIFF_R_WRIST_YAW]);
00584   joint_stiffness_if->set_r_hand(__values[STIFF_R_HAND]);
00585   // Hip
00586   joint_stiffness_if->set_l_hip_yaw_pitch(__values[STIFF_L_HIP_YAW_PITCH]);
00587   joint_stiffness_if->set_l_hip_pitch(__values[STIFF_L_HIP_PITCH]);
00588   joint_stiffness_if->set_l_hip_roll(__values[STIFF_L_HIP_ROLL]);
00589   // RHipYawPitch stiffness is always 0, copy from RYawPitch
00590   joint_stiffness_if->set_r_hip_yaw_pitch(__values[STIFF_L_HIP_YAW_PITCH]);
00591   joint_stiffness_if->set_r_hip_pitch(__values[STIFF_R_HIP_PITCH]);
00592   joint_stiffness_if->set_r_hip_roll(__values[STIFF_R_HIP_ROLL]);
00593   // Knees
00594   joint_stiffness_if->set_l_knee_pitch(__values[STIFF_L_KNEE_PITCH]);
00595   joint_stiffness_if->set_r_knee_pitch(__values[STIFF_R_KNEE_PITCH]);
00596   // Feet
00597   joint_stiffness_if->set_l_ankle_pitch(__values[STIFF_L_ANKLE_PITCH]);
00598   joint_stiffness_if->set_l_ankle_roll(__values[STIFF_L_ANKLE_ROLL]);
00599   joint_stiffness_if->set_r_ankle_pitch(__values[STIFF_R_ANKLE_PITCH]);
00600   joint_stiffness_if->set_r_ankle_roll(__values[STIFF_R_ANKLE_ROLL]);
00601 
00602   float min_stiffness = 1.;
00603   for (int i = STIFF_HEAD_YAW; i <= STIFF_R_ANKLE_ROLL; ++i) {
00604     // ignore wrist and hand on RoboCup version
00605     if ( (__robot_type == NaoJointPositionInterface::ROBOTYPE_ROBOCUP) &&
00606          ( (i == STIFF_L_WRIST_YAW) || (i == STIFF_L_HAND) ||
00607            (i == STIFF_R_WRIST_YAW) || (i == STIFF_R_HAND) ) )
00608       continue;
00609 
00610     // ignore RHipYawPitch stiffness, it's always 0
00611     if (i == STIFF_R_HIP_YAW_PITCH) continue;
00612 
00613     if (__values[i] < min_stiffness)  min_stiffness = __values[i];
00614   }
00615   joint_stiffness_if->set_minimum(min_stiffness);
00616 
00617   // Sensors
00618   // FSRs
00619   sensor_if->set_l_fsr_fl(__values[L_FSR_FL]);
00620   sensor_if->set_l_fsr_fr(__values[L_FSR_FR]);
00621   sensor_if->set_l_fsr_rl(__values[L_FSR_RL]);
00622   sensor_if->set_l_fsr_rr(__values[L_FSR_RR]);
00623   sensor_if->set_r_fsr_fl(__values[R_FSR_FL]);
00624   sensor_if->set_r_fsr_fr(__values[R_FSR_FR]);
00625   sensor_if->set_r_fsr_rl(__values[R_FSR_RL]);
00626   sensor_if->set_r_fsr_rr(__values[R_FSR_RR]);
00627 
00628   sensor_if->set_l_cop_x(__values[L_COP_X]);
00629   sensor_if->set_l_cop_y(__values[L_COP_Y]);
00630   sensor_if->set_l_total_weight(__values[L_TOTAL_WEIGHT]);
00631 
00632   sensor_if->set_r_cop_x(__values[R_COP_X]);
00633   sensor_if->set_r_cop_y(__values[R_COP_Y]);
00634   sensor_if->set_r_total_weight(__values[R_TOTAL_WEIGHT]);
00635 
00636   // Buttons and bumpers
00637   sensor_if->set_chest_button((__values[CHEST_BUTTON] >= 0.5) ? 1 : 0);
00638   sensor_if->set_head_touch_front((__values[HEAD_TOUCH_FRONT] >= 0.5) ? 1 : 0);
00639   sensor_if->set_head_touch_middle((__values[HEAD_TOUCH_MIDDLE] >= 0.5) ? 1 : 0);
00640   sensor_if->set_head_touch_rear((__values[HEAD_TOUCH_REAR] >= 0.5) ? 1 : 0);
00641 
00642   sensor_if->set_l_foot_bumper_l((__values[L_FOOT_BUMPER_L] >= 0.5) ? 1 : 0);
00643   sensor_if->set_l_foot_bumper_r((__values[L_FOOT_BUMPER_R] >= 0.5) ? 1 : 0);
00644   sensor_if->set_r_foot_bumper_l((__values[R_FOOT_BUMPER_L] >= 0.5) ? 1 : 0);
00645   sensor_if->set_r_foot_bumper_r((__values[R_FOOT_BUMPER_R] >= 0.5) ? 1 : 0);
00646 
00647   // Inertial measurement unit
00648   sensor_if->set_accel_x(__values[ACC_X] / ACCELEROMETER_G_FACTOR);
00649   sensor_if->set_accel_y(__values[ACC_Y] / ACCELEROMETER_G_FACTOR);
00650   sensor_if->set_accel_z(__values[ACC_Z] / ACCELEROMETER_G_FACTOR);
00651 
00652   sensor_if->set_gyro_x(__values[GYR_X]);
00653   sensor_if->set_gyro_y(__values[GYR_Y]);
00654   sensor_if->set_gyro_ref(__values[GYR_REF]);
00655 
00656   sensor_if->set_angle_x(__values[ANGLE_X]);
00657   sensor_if->set_angle_y(__values[ANGLE_Y]);
00658 
00659   // Ultrasonic sound
00660   NaoSensorInterface::UltrasonicDirection us_dir =
00661     sensor_if->ultrasonic_direction();
00662   switch (us_dir) {
00663   case NaoSensorInterface::USD_LEFT_LEFT:
00664   case NaoSensorInterface::USD_RIGHT_LEFT:
00665     {
00666       float us_left[4] = {__values[ULTRASONIC_DISTANCE], 0, 0, 0};
00667       sensor_if->set_ultrasonic_distance_left(us_left);
00668 
00669       float us_right[4] = {0, 0, 0, 0};
00670       sensor_if->set_ultrasonic_distance_right(us_right);
00671     }
00672     break;
00673   case NaoSensorInterface::USD_LEFT_RIGHT:
00674   case NaoSensorInterface::USD_RIGHT_RIGHT:
00675     {
00676       float us_left[4] = {0, 0, 0, 0};
00677       sensor_if->set_ultrasonic_distance_left(us_left);
00678 
00679       float us_right[4] = {__values[ULTRASONIC_DISTANCE], 0, 0, 0};
00680       sensor_if->set_ultrasonic_distance_right(us_right);
00681     }
00682     break;
00683 
00684   default:
00685     {
00686       float us_left[4] = {__values[ULTRASONIC_DISTANCE_LEFT_0],
00687                           __values[ULTRASONIC_DISTANCE_LEFT_1],
00688                           __values[ULTRASONIC_DISTANCE_LEFT_2],
00689                           __values[ULTRASONIC_DISTANCE_LEFT_3]};
00690       sensor_if->set_ultrasonic_distance_left(us_left);
00691 
00692       float us_right[4] = {__values[ULTRASONIC_DISTANCE_RIGHT_0],
00693                            __values[ULTRASONIC_DISTANCE_RIGHT_1],
00694                            __values[ULTRASONIC_DISTANCE_RIGHT_2],
00695                            __values[ULTRASONIC_DISTANCE_RIGHT_3]};
00696       sensor_if->set_ultrasonic_distance_right(us_right);
00697     }
00698     break;
00699   }
00700 
00701   // Battery
00702   sensor_if->set_battery_charge(__values[BATTERY_CHARGE]);
00703 
00704   // Write to blackboard
00705   joint_pos_if->write();
00706   joint_stiffness_if->write();
00707   sensor_if->write();
00708 }
00709 
00710 
00711 void
00712 NaoQiDCMThread::process_messages()
00713 {
00714   // *** Joint position messages
00715   while (! __joint_pos_if->msgq_empty()) {
00716     if (NaoJointPositionInterface::SetServoMessage *msg =
00717         __joint_pos_if->msgq_first_safe(msg))
00718     {
00719       send_commands(msg->servo(), "Position", msg->value(), msg->time());
00720     }
00721 
00722     else if (NaoJointPositionInterface::SetServosMessage *msg =
00723              __joint_pos_if->msgq_first_safe(msg))
00724     {
00725     }
00726 
00727     else if (NaoJointPositionInterface::MoveServoMessage *msg =
00728              __joint_pos_if->msgq_first_safe(msg))
00729     {
00730       std::vector<std::string> servos = parse_servo_bitfield(msg->servo());
00731       std::vector<float> values(servos.size(), msg->value());
00732       __almotion->setAngles(servos, values, msg->speed());
00733     }
00734 
00735     else if (NaoJointPositionInterface::MoveServosMessage *msg =
00736              __joint_pos_if->msgq_first_safe(msg))
00737     {
00738       motion::move_joints(
00739         __almotion,
00740         /* head */ msg->head_yaw(), msg->head_pitch(),
00741         /* l shoulder */ msg->l_shoulder_pitch(), msg->l_shoulder_roll(),
00742         /* l elbow */ msg->l_elbow_yaw(), msg->l_elbow_roll(),
00743         /* l wrist/hand */ msg->l_wrist_yaw(), msg->l_hand(),
00744         /* l hip */ msg->l_hip_yaw_pitch(), msg->l_hip_roll(), msg->l_hip_pitch(),
00745         /* l knee */ msg->l_knee_pitch(),
00746         /* l ankle */ msg->l_ankle_pitch(), msg->l_ankle_roll(),
00747         /* r shoulder */ msg->r_shoulder_pitch(), msg->r_shoulder_roll(),
00748         /* r elbow */ msg->r_elbow_yaw(), msg->r_elbow_roll(),
00749         /* r wrist/hand */ msg->r_wrist_yaw(), msg->r_hand(),
00750         /* r hip */ msg->r_hip_yaw_pitch(), msg->r_hip_roll(), msg->r_hip_pitch(),
00751         /* r knee */ msg->r_knee_pitch(),
00752         /* r ankle */ msg->r_ankle_pitch(), msg->r_ankle_roll(),
00753         /* speed */ msg->speed());
00754     }
00755 
00756     __joint_pos_if->msgq_pop();
00757   }
00758 
00759   // *** Joint stiffness messages
00760   while (! __joint_stiffness_if->msgq_empty()) {
00761     if (NaoJointStiffnessInterface::SetStiffnessMessage *msg =
00762         __joint_stiffness_if->msgq_first_safe(msg))
00763     {
00764       /* DCM version, disfunctional due to ALMotion deficiencies
00765       send_commands(msg->servo(), "Hardness", msg->value(),
00766                     "Merge", (int)roundf(1000. * msg->time_sec()));
00767       */
00768 
00769       std::vector<std::string> servos = parse_servo_bitfield(msg->servo());
00770       std::vector<float> values(servos.size(), msg->value());
00771 
00772       __almotion->post.stiffnessInterpolation(servos, values,
00773                                               msg->time_sec());
00774 
00775     }
00776     else if (NaoJointStiffnessInterface::SetBodyStiffnessMessage *msg =
00777         __joint_stiffness_if->msgq_first_safe(msg))
00778     {
00779       /* Cannot be used atm because ALMotion will not update its internal
00780        * belief of stiffness values causing any further motion via DCM
00781        * or ALMotion to fail
00782       // use setJointStiffness alias setup in init()
00783       AL::ALValue cmd;
00784       cmd.arraySetSize(3);
00785       cmd[0] = std::string("setJointStiffness");
00786       cmd[1] = std::string("Merge");
00787       cmd[2].arraySetSize(1);
00788       cmd[2][0].arraySetSize(2);
00789       cmd[2][0][0] = msg->value();
00790       cmd[2][0][1] = __dcm_time + (int)roundf(1000. * msg->time_sec());
00791       try {
00792         __dcm->set(cmd);
00793       } catch (const AL::ALError &e) {
00794         logger->log_warn(name(), "Failed to call setJointStiffness: %s",
00795                          e.toString().c_str());
00796       }
00797       */
00798 
00799       __almotion->post.stiffnessInterpolation("Body", msg->value(),
00800                                               msg->time_sec());
00801     }
00802 
00803     else if (NaoJointStiffnessInterface::SetStiffnessesMessage *msg =
00804              __joint_stiffness_if->msgq_first_safe(msg))
00805     {
00806       std::vector<float> values(__alljoint_names.getSize());
00807       values[STIFFJ_HEAD_PITCH] = msg->head_pitch();
00808       values[STIFFJ_HEAD_YAW] = msg->head_yaw();
00809       values[STIFFJ_L_SHOULDER_PITCH] = msg->l_shoulder_pitch();
00810       values[STIFFJ_L_SHOULDER_ROLL] = msg->l_shoulder_roll();
00811       values[STIFFJ_L_ELBOW_YAW] = msg->l_elbow_yaw();
00812       values[STIFFJ_L_ELBOW_ROLL] = msg->l_elbow_roll();
00813       values[STIFFJ_L_HIP_YAW_PITCH] = msg->l_hip_yaw_pitch();
00814       values[STIFFJ_L_HIP_ROLL] = msg->l_hip_roll();
00815       values[STIFFJ_L_HIP_PITCH] = msg->l_hip_pitch();
00816       values[STIFFJ_L_KNEE_PITCH] = msg->l_knee_pitch();
00817       values[STIFFJ_L_ANKLE_PITCH] = msg->l_ankle_pitch();
00818       values[STIFFJ_L_ANKLE_ROLL] = msg->l_ankle_roll();
00819 
00820       values[STIFFJ_R_SHOULDER_PITCH] = msg->r_shoulder_pitch();
00821       values[STIFFJ_R_SHOULDER_ROLL] = msg->r_shoulder_roll();
00822       values[STIFFJ_R_ELBOW_YAW] = msg->r_elbow_yaw();
00823       values[STIFFJ_R_ELBOW_ROLL] = msg->r_elbow_roll();
00824       values[STIFFJ_R_HIP_YAW_PITCH] = msg->r_hip_yaw_pitch();
00825       values[STIFFJ_R_HIP_ROLL] = msg->r_hip_roll();
00826       values[STIFFJ_R_HIP_PITCH] = msg->r_hip_pitch();
00827       values[STIFFJ_R_KNEE_PITCH] = msg->r_knee_pitch();
00828       values[STIFFJ_R_ANKLE_PITCH] = msg->r_ankle_pitch();
00829       values[STIFFJ_R_ANKLE_ROLL] = msg->r_ankle_roll();
00830 
00831       if (__robot_type != NaoJointPositionInterface::ROBOTYPE_ROBOCUP) {
00832         values[STIFFJ_L_WRIST_YAW] = msg->l_wrist_yaw();
00833         values[STIFFJ_L_HAND] = msg->l_hand();
00834         values[STIFFJ_R_WRIST_YAW] = msg->r_wrist_yaw();
00835         values[STIFFJ_R_HAND] = msg->r_hand();
00836       }
00837 
00838       __almotion->post.stiffnessInterpolation(__alljoint_names, values,
00839                                               msg->time_sec());
00840     }
00841 
00842     __joint_stiffness_if->msgq_pop();
00843   }
00844 
00845   // *** Sensor messages
00846   while (! __sensor_if->msgq_empty()) {
00847     if (NaoSensorInterface::EmitUltrasonicWaveMessage *msg =
00848         __sensor_if->msgq_first_safe(msg))
00849     {
00850       int value = ultrasonic_value(msg->ultrasonic_direction());
00851       send_command("US/Actuator/Value", value, "ClearAll", 0);
00852 
00853       __sensor_if->set_ultrasonic_direction(msg->ultrasonic_direction());
00854       __sensor_highfreq_if->set_ultrasonic_direction(msg->ultrasonic_direction());
00855 
00856     } else if (NaoSensorInterface::StartUltrasonicMessage *msg =
00857         __sensor_if->msgq_first_safe(msg))
00858     {
00859       int value = ultrasonic_value(msg->ultrasonic_direction());
00860       value += 64;
00861       send_command("US/Actuator/Value", value, "ClearAll", 0);
00862 
00863       __sensor_if->set_ultrasonic_direction(msg->ultrasonic_direction());
00864       __sensor_highfreq_if->set_ultrasonic_direction(msg->ultrasonic_direction());
00865 
00866     } else if (NaoSensorInterface::StopUltrasonicMessage *msg =
00867         __sensor_if->msgq_first_safe(msg))
00868     {
00869       send_command("US/Actuator/Value", 0, "ClearAll", 0);
00870 
00871       __sensor_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
00872       __sensor_highfreq_if->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
00873     }
00874 
00875     __sensor_if->msgq_pop();
00876   }
00877 
00878 }
00879 
00880 
00881 void
00882 NaoQiDCMThread::send_commands(unsigned int servos, std::string what,
00883                               float value, int time_offset)
00884 {
00885   /*
00886   __almotion->wbEnable(false);
00887   __almotion->wbEnableEffectorControl("LArm", false);
00888   __almotion->wbEnableEffectorControl("RArm", false);
00889   __almotion->setWalkArmsEnable(false, false);
00890   __almotion->killAll();
00891   */
00892 
00893   std::vector<std::string> servonames = parse_servo_bitfield(servos);
00894 
00895   std::vector<std::string>::iterator s;
00896   for (s = servonames.begin(); s != servonames.end(); ++s) {
00897     float v = value;
00898     if (*s == "HeadYaw") {
00899       v = CLIP_VALUE(HEAD_YAW, value);
00900     } else if (*s == "LShoulderPitch") {
00901       v = CLIP_VALUE(L_SHOULDER_PITCH, value);
00902     } else if (*s == "RShoulderPitch") {
00903       v = CLIP_VALUE(R_SHOULDER_PITCH, value);
00904     }
00905     send_command(*s + "/" + what + "/Actuator/Value",
00906                  v, "Merge", time_offset);
00907   }
00908 }
00909 
00910 void
00911 NaoQiDCMThread::send_command(std::string name, float value,
00912                              std::string kind, int time_offset)
00913 {
00914   AL::ALValue cmd;
00915 
00916   /*
00917   printf("Command for %s to %f in %i ms (%i vs. %i)\n",
00918          name.c_str(), value, time_offset, __dcm->getTime(time_offset),
00919          __dcm_time + time_offset);
00920   */
00921 
00922   cmd.arraySetSize(3);
00923   cmd[0] = name;
00924   cmd[1] = kind;
00925   cmd[2].arraySetSize(1);
00926   cmd[2][0].arraySetSize(2);
00927   cmd[2][0][0] = value;
00928   cmd[2][0][1] = __dcm_time + time_offset;
00929 
00930   __dcm->set(cmd); 
00931 }
00932 
00933 
00934 std::vector<std::string>
00935 NaoQiDCMThread::parse_servo_bitfield(unsigned int servos)
00936 {
00937   std::vector<std::string> servonames;
00938 
00939   if ( servos & NaoJointPositionInterface::SERVO_head_yaw )
00940     servonames.push_back("HeadYaw");
00941 
00942   if ( servos & NaoJointPositionInterface::SERVO_head_pitch )
00943     servonames.push_back("HeadPitch");
00944 
00945   if ( servos & NaoJointPositionInterface::SERVO_l_shoulder_pitch )
00946     servonames.push_back("LShoulderPitch");
00947 
00948   if ( servos & NaoJointPositionInterface::SERVO_l_shoulder_roll )
00949     servonames.push_back("LShoulderRoll");
00950 
00951   if ( servos & NaoJointPositionInterface::SERVO_l_elbow_yaw )
00952     servonames.push_back("LElbowYaw");
00953 
00954   if ( servos & NaoJointPositionInterface::SERVO_l_elbow_roll )
00955     servonames.push_back("LElbowRoll");
00956 
00957   if ( servos & NaoJointPositionInterface::SERVO_r_shoulder_pitch )
00958     servonames.push_back("RShoulderPitch");
00959 
00960   if ( servos & NaoJointPositionInterface::SERVO_r_shoulder_roll )
00961     servonames.push_back("RShoulderRoll");
00962 
00963   if ( servos & NaoJointPositionInterface::SERVO_r_elbow_yaw )
00964     servonames.push_back("RElbowYaw");
00965 
00966   if ( servos & NaoJointPositionInterface::SERVO_r_elbow_roll )
00967     servonames.push_back("RElbowRoll");
00968 
00969   if ( servos & NaoJointPositionInterface::SERVO_l_hip_yaw_pitch )
00970     servonames.push_back("LHipYawPitch");
00971 
00972   if ( servos & NaoJointPositionInterface::SERVO_l_hip_pitch )
00973     servonames.push_back("LHipPitch");
00974 
00975   if ( servos & NaoJointPositionInterface::SERVO_l_hip_roll )
00976     servonames.push_back("LHipRoll");
00977 
00978   if ( servos & NaoJointPositionInterface::SERVO_r_hip_yaw_pitch )
00979     servonames.push_back("RHipYawPitch");
00980 
00981   if ( servos & NaoJointPositionInterface::SERVO_r_hip_pitch )
00982     servonames.push_back("RHipPitch");
00983 
00984   if ( servos & NaoJointPositionInterface::SERVO_r_hip_roll )
00985     servonames.push_back("RHipRoll");
00986 
00987   if ( servos & NaoJointPositionInterface::SERVO_l_knee_pitch )
00988     servonames.push_back("LKneePitch");
00989 
00990   if ( servos & NaoJointPositionInterface::SERVO_r_knee_pitch )
00991     servonames.push_back("RKneePitch");
00992 
00993   if ( servos & NaoJointPositionInterface::SERVO_l_ankle_pitch )
00994     servonames.push_back("LAnklePitch");
00995 
00996   if ( servos & NaoJointPositionInterface::SERVO_l_ankle_roll )
00997     servonames.push_back("LAnkleRoll");
00998 
00999   if ( servos & NaoJointPositionInterface::SERVO_r_ankle_pitch )
01000     servonames.push_back("RAnklePitch");
01001 
01002   if ( servos & NaoJointPositionInterface::SERVO_r_ankle_roll )
01003     servonames.push_back("RAnkleRoll");
01004 
01005   return servonames;
01006 }
01007 
01008 
01009 int
01010 NaoQiDCMThread::ultrasonic_value(fawkes::NaoSensorInterface::UltrasonicDirection direction)
01011 {
01012   int value = 0;
01013   switch (direction) {
01014   case NaoSensorInterface::USD_LEFT_LEFT:   value =  0; break;
01015   case NaoSensorInterface::USD_LEFT_RIGHT:  value =  1; break;
01016   case NaoSensorInterface::USD_RIGHT_LEFT:  value =  2; break;
01017   case NaoSensorInterface::USD_RIGHT_RIGHT: value =  3; break;
01018   case NaoSensorInterface::USD_BOTH_BOTH:   value = 12; break;
01019   default:
01020     logger->log_warn(name(), "Illegal ultrasonic direction, "
01021                      "using left-right");
01022   }
01023   return value;
01024 }