Fawkes API  Fawkes Development Version
dcm_thread.cpp
1 
2 /***************************************************************************
3  * dcm_thread.cpp - Provide NaoQi DCM to Fawkes
4  *
5  * Created: Tue May 31 15:00:54 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "dcm_thread.h"
24 
25 #include "motion_utils.h"
26 
27 #include <alcore/alerror.h>
28 #include <almemoryfastaccess/almemoryfastaccess.h>
29 #include <alproxies/allauncherproxy.h>
30 #include <alproxies/almemoryproxy.h>
31 #include <alproxies/almotionproxy.h>
32 #include <alproxies/dcmproxy.h>
33 #include <interfaces/NaoJointStiffnessInterface.h>
34 #include <interfaces/NaoSensorInterface.h>
35 
36 #include <boost/bind/bind.hpp>
37 
38 using namespace fawkes;
39 
40 enum SensorType {
41  HEAD_PITCH = 0,
42  HEAD_YAW,
43  L_SHOULDER_PITCH,
44  L_SHOULDER_ROLL,
45  L_ELBOW_YAW,
46  L_ELBOW_ROLL,
47  L_WRIST_YAW,
48  L_HAND,
49  L_HIP_YAW_PITCH,
50  L_HIP_ROLL,
51  L_HIP_PITCH,
52  L_KNEE_PITCH,
53  L_ANKLE_PITCH,
54  L_ANKLE_ROLL,
55  R_SHOULDER_PITCH,
56  R_SHOULDER_ROLL,
57  R_ELBOW_YAW,
58  R_ELBOW_ROLL,
59  R_WRIST_YAW,
60  R_HAND,
61  R_HIP_YAW_PITCH,
62  R_HIP_ROLL,
63  R_HIP_PITCH,
64  R_KNEE_PITCH,
65  R_ANKLE_PITCH,
66  R_ANKLE_ROLL,
67  STIFF_HEAD_PITCH,
68  STIFF_HEAD_YAW,
69  STIFF_L_SHOULDER_PITCH,
70  STIFF_L_SHOULDER_ROLL,
71  STIFF_L_ELBOW_YAW,
72  STIFF_L_ELBOW_ROLL,
73  STIFF_L_WRIST_YAW,
74  STIFF_L_HAND,
75  STIFF_L_HIP_YAW_PITCH,
76  STIFF_L_HIP_ROLL,
77  STIFF_L_HIP_PITCH,
78  STIFF_L_KNEE_PITCH,
79  STIFF_L_ANKLE_PITCH,
80  STIFF_L_ANKLE_ROLL,
81  STIFF_R_SHOULDER_PITCH,
82  STIFF_R_SHOULDER_ROLL,
83  STIFF_R_ELBOW_YAW,
84  STIFF_R_ELBOW_ROLL,
85  STIFF_R_WRIST_YAW,
86  STIFF_R_HAND,
87  STIFF_R_HIP_YAW_PITCH,
88  STIFF_R_HIP_ROLL,
89  STIFF_R_HIP_PITCH,
90  STIFF_R_KNEE_PITCH,
91  STIFF_R_ANKLE_PITCH,
92  STIFF_R_ANKLE_ROLL,
93  ACC_X,
94  ACC_Y,
95  ACC_Z,
96  GYR_X,
97  GYR_Y,
98  GYR_REF,
99  ANGLE_X,
100  ANGLE_Y,
101  L_FSR_FL,
102  L_FSR_FR,
103  L_FSR_RL,
104  L_FSR_RR,
105  R_FSR_FL,
106  R_FSR_FR,
107  R_FSR_RL,
108  R_FSR_RR,
109  L_COP_X,
110  L_COP_Y,
111  L_TOTAL_WEIGHT,
112  R_COP_X,
113  R_COP_Y,
114  R_TOTAL_WEIGHT,
115  ULTRASONIC_DIRECTION,
116  ULTRASONIC_DISTANCE,
117  ULTRASONIC_DISTANCE_LEFT_0,
118  ULTRASONIC_DISTANCE_LEFT_1,
119  ULTRASONIC_DISTANCE_LEFT_2,
120  ULTRASONIC_DISTANCE_LEFT_3,
121  ULTRASONIC_DISTANCE_RIGHT_0,
122  ULTRASONIC_DISTANCE_RIGHT_1,
123  ULTRASONIC_DISTANCE_RIGHT_2,
124  ULTRASONIC_DISTANCE_RIGHT_3,
125  L_FOOT_BUMPER_L,
126  L_FOOT_BUMPER_R,
127  R_FOOT_BUMPER_L,
128  R_FOOT_BUMPER_R,
129  HEAD_TOUCH_FRONT,
130  HEAD_TOUCH_MIDDLE,
131  HEAD_TOUCH_REAR,
132  CHEST_BUTTON,
133  BATTERY_CHARGE,
134  SensorTypeN
135 };
136 
137 enum StiffnessJoint {
138  STIFFJ_HEAD_PITCH = 0,
139  STIFFJ_HEAD_YAW,
140  STIFFJ_L_SHOULDER_PITCH,
141  STIFFJ_L_SHOULDER_ROLL,
142  STIFFJ_L_ELBOW_YAW,
143  STIFFJ_L_ELBOW_ROLL,
144  STIFFJ_L_HIP_YAW_PITCH,
145  STIFFJ_L_HIP_ROLL,
146  STIFFJ_L_HIP_PITCH,
147  STIFFJ_L_KNEE_PITCH,
148  STIFFJ_L_ANKLE_PITCH,
149  STIFFJ_L_ANKLE_ROLL,
150  STIFFJ_R_SHOULDER_PITCH,
151  STIFFJ_R_SHOULDER_ROLL,
152  STIFFJ_R_ELBOW_YAW,
153  STIFFJ_R_ELBOW_ROLL,
154  STIFFJ_R_HIP_YAW_PITCH,
155  STIFFJ_R_HIP_ROLL,
156  STIFFJ_R_HIP_PITCH,
157  STIFFJ_R_KNEE_PITCH,
158  STIFFJ_R_ANKLE_PITCH,
159  STIFFJ_R_ANKLE_ROLL,
160  STIFFJ_L_WRIST_YAW,
161  STIFFJ_L_HAND,
162  STIFFJ_R_WRIST_YAW,
163  STIFFJ_R_HAND,
164  StiffnessJointN
165 };
166 
167 #define ACCELEROMETER_G_FACTOR 56.
168 
169 // Clipping as suggested by Aldebaran at RoboCup 2008, Suzhou
170 // Changed head yaw to +/- 1.2 according to Nao v3 red book
171 #define HEAD_YAW_MIN -1.2
172 #define HEAD_YAW_MAX 1.2
173 // New values after RoboCup Workshop 2009 at Aldebaran, Paris
174 #define L_SHOULDER_PITCH_MIN -1.7
175 #define L_SHOULDER_PITCH_MAX 1.7
176 #define R_SHOULDER_PITCH_MIN -1.7
177 #define R_SHOULDER_PITCH_MAX 1.7
178 
179 #define CLIP_VALUE(V, value) clip_value(value, V##_MIN, V##_MAX)
180 
181 /** Clip value to given constraints.
182  * @param value value to clop
183  * @param min minimum value
184  * @param max maximum value
185  * @return clipped value
186  */
187 static inline float
188 clip_value(float value, float min, float max)
189 {
190  if (value < min)
191  value = min;
192  if (value > max)
193  value = max;
194  return value;
195 }
196 
197 /** Thread to write data at full DCM frequency.
198  * This thread is woken up by the DCM callback and publishes new data
199  * if there is a reader for any of the high frequency interfaces. It
200  * is also responsible for processing incoming commands.
201  */
203 {
204 public:
205  /** Constructor.
206  * @param parent parent NaoQiDCMThread to call.
207  */
208  explicit HighFreqThread(NaoQiDCMThread *parent)
209  : Thread("NaoQiDCMThread::HighFreqThread", Thread::OPMODE_WAITFORWAKEUP), parent_(parent)
210  {
211  set_coalesce_wakeups(true);
212  }
213 
214  virtual void
216  {
217  parent_->read_values();
218  if ((parent_->joint_pos_highfreq_if_->num_readers() > 0)
219  || (parent_->joint_stiffness_highfreq_if_->num_readers() > 0)
220  || (parent_->sensor_if_->num_readers() > 0)) {
221  parent_->update_interfaces(parent_->joint_pos_highfreq_if_,
222  parent_->joint_stiffness_highfreq_if_,
223  parent_->sensor_highfreq_if_);
224  }
225 
226  parent_->process_messages();
227  }
228 
229 private:
230  NaoQiDCMThread *parent_;
231 };
232 
233 /** @class NaoQiDCMThread "dcm_thread.h"
234  * Thread to provide DCM to Fawkes.
235  * This thread opens a DCM proxy and updates information about hardware
236  * in the blackboard and provides basic setting functionality to move
237  * specific servos.
238  *
239  * The DCM thread writes to two sets of interfaces, at a high and a lower
240  * frequency. The high frequency data is written if there is a reader at
241  * each DCM callback, which results in a frequency of about 100Hz. This
242  * should only be used if necessary, for example for custom motion pattern
243  * generating plugins. Otherwise, and especially if in a thread hooked
244  * into the main loop, use the lower frequency interfaces. These are
245  * written during the sensor hook.
246  *
247  * @author Tim Niemueller
248  */
249 
250 /** Constructor. */
252 : Thread("NaoQiDCMThread", Thread::OPMODE_WAITFORWAKEUP),
253  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
254 {
255 }
256 
257 /** Destructor. */
259 {
260 }
261 
262 void
264 {
265  // Is the DCM running ?
266  try {
267  AL::ALPtr<AL::ALLauncherProxy> launcher(new AL::ALLauncherProxy(naoqi_broker));
268  bool is_dcm_available = launcher->isModulePresent("DCM");
269  bool is_almotion_available = launcher->isModulePresent("ALMotion");
270 
271  if (!is_dcm_available) {
272  throw Exception("DCMThread: NaoQi DCM is not available");
273  }
274  if (!is_almotion_available) {
275  throw Exception("DCMThread: ALMotion is not available");
276  }
277  } catch (AL::ALError &e) {
278  throw Exception("Checking module availability failed: %s", e.toString().c_str());
279  }
280 
281  dcm_ = naoqi_broker->getDcmProxy();
282  almotion_ = naoqi_broker->getMotionProxy();
283 
284  try {
285  AL::ALPtr<AL::ALMemoryProxy> almemory = naoqi_broker->getMemoryProxy();
286  std::string version = almemory->getData("RobotConfig/Body/BaseVersion", 0);
287  unsigned int num_joints = almotion_->getJointNames("Body").size();
288  if (num_joints == 26) {
289  robot_type_ = NaoJointPositionInterface::ROBOTYPE_ACADEMIC;
290  } else {
291  robot_type_ = NaoJointPositionInterface::ROBOTYPE_ROBOCUP;
292  }
293  robot_version_[2] = robot_version_[3] = 0;
294  if (version[0] == 'V')
295  version = version.substr(1);
296  std::string::size_type pos;
297  if ((pos = version.find_first_of(".")) != std::string::npos) {
298  std::string version_major = version.substr(0, pos);
299  std::string version_minor = version.substr(pos + 1);
300  robot_version_[0] = atoi(version_major.c_str());
301  robot_version_[1] = atoi(version_minor.c_str());
302  }
303  usboard_version_ = almemory->getData("Device/DeviceList/USBoard/ProgVersion", 0);
304  } catch (AL::ALError &e) {
305  throw Exception("Retrieving robot info failed: %s", e.toString().c_str());
306  }
307 
308  memfa_.reset(new AL::ALMemoryFastAccess());
309 
310  std::string prefix = "Device/SubDeviceList/";
311 
312  // Initialize fast memory access
313  std::vector<std::string> keys;
314  keys.resize(SensorTypeN);
315  values_.resize(SensorTypeN);
316 
317  keys[HEAD_PITCH] = prefix + "HeadPitch/Position/Sensor/Value";
318  keys[HEAD_YAW] = prefix + "HeadYaw/Position/Sensor/Value";
319  keys[L_SHOULDER_PITCH] = prefix + "LShoulderPitch/Position/Sensor/Value";
320  keys[L_SHOULDER_ROLL] = prefix + "LShoulderRoll/Position/Sensor/Value";
321  keys[L_ELBOW_YAW] = prefix + "LElbowYaw/Position/Sensor/Value";
322  keys[L_ELBOW_ROLL] = prefix + "LElbowRoll/Position/Sensor/Value";
323  keys[L_WRIST_YAW] = prefix + "LWristYaw/Position/Sensor/Value";
324  keys[L_HAND] = prefix + "LHand/Position/Sensor/Value";
325  keys[L_HIP_YAW_PITCH] = prefix + "LHipYawPitch/Position/Sensor/Value";
326  keys[L_HIP_ROLL] = prefix + "LHipRoll/Position/Sensor/Value";
327  keys[L_HIP_PITCH] = prefix + "LHipPitch/Position/Sensor/Value";
328  keys[L_KNEE_PITCH] = prefix + "LKneePitch/Position/Sensor/Value";
329  keys[L_ANKLE_PITCH] = prefix + "LAnklePitch/Position/Sensor/Value";
330  keys[L_ANKLE_ROLL] = prefix + "LAnkleRoll/Position/Sensor/Value";
331 
332  keys[R_SHOULDER_PITCH] = prefix + "RShoulderPitch/Position/Sensor/Value";
333  keys[R_SHOULDER_ROLL] = prefix + "RShoulderRoll/Position/Sensor/Value";
334  keys[R_ELBOW_YAW] = prefix + "RElbowYaw/Position/Sensor/Value";
335  keys[R_ELBOW_ROLL] = prefix + "RElbowRoll/Position/Sensor/Value";
336  keys[R_WRIST_YAW] = prefix + "RWristYaw/Position/Sensor/Value";
337  keys[R_HAND] = prefix + "RHand/Position/Sensor/Value";
338  keys[R_HIP_YAW_PITCH] = prefix + "RHipYawPitch/Position/Sensor/Value";
339  keys[R_HIP_ROLL] = prefix + "RHipRoll/Position/Sensor/Value";
340  keys[R_HIP_PITCH] = prefix + "RHipPitch/Position/Sensor/Value";
341  keys[R_KNEE_PITCH] = prefix + "RKneePitch/Position/Sensor/Value";
342  keys[R_ANKLE_PITCH] = prefix + "RAnklePitch/Position/Sensor/Value";
343  keys[R_ANKLE_ROLL] = prefix + "RAnkleRoll/Position/Sensor/Value";
344 
345  keys[STIFF_HEAD_PITCH] = prefix + "HeadPitch/Hardness/Actuator/Value";
346  keys[STIFF_HEAD_YAW] = prefix + "HeadYaw/Hardness/Actuator/Value";
347  keys[STIFF_L_SHOULDER_PITCH] = prefix + "LShoulderPitch/Hardness/Actuator/Value";
348  keys[STIFF_L_SHOULDER_ROLL] = prefix + "LShoulderRoll/Hardness/Actuator/Value";
349  keys[STIFF_L_ELBOW_YAW] = prefix + "LElbowYaw/Hardness/Actuator/Value";
350  keys[STIFF_L_ELBOW_ROLL] = prefix + "LElbowRoll/Hardness/Actuator/Value";
351  keys[STIFF_L_WRIST_YAW] = prefix + "LWristYaw/Hardness/Actuator/Value";
352  keys[STIFF_L_HAND] = prefix + "LHand/Hardness/Actuator/Value";
353  keys[STIFF_L_HIP_YAW_PITCH] = prefix + "LHipYawPitch/Hardness/Actuator/Value";
354  keys[STIFF_L_HIP_ROLL] = prefix + "LHipRoll/Hardness/Actuator/Value";
355  keys[STIFF_L_HIP_PITCH] = prefix + "LHipPitch/Hardness/Actuator/Value";
356  keys[STIFF_L_KNEE_PITCH] = prefix + "LKneePitch/Hardness/Actuator/Value";
357  keys[STIFF_L_ANKLE_PITCH] = prefix + "LAnklePitch/Hardness/Actuator/Value";
358  keys[STIFF_L_ANKLE_ROLL] = prefix + "LAnkleRoll/Hardness/Actuator/Value";
359 
360  keys[STIFF_R_SHOULDER_PITCH] = prefix + "RShoulderPitch/Hardness/Actuator/Value";
361  keys[STIFF_R_SHOULDER_ROLL] = prefix + "RShoulderRoll/Hardness/Actuator/Value";
362  keys[STIFF_R_ELBOW_YAW] = prefix + "RElbowYaw/Hardness/Actuator/Value";
363  keys[STIFF_R_ELBOW_ROLL] = prefix + "RElbowRoll/Hardness/Actuator/Value";
364  keys[STIFF_R_WRIST_YAW] = prefix + "RWristYaw/Hardness/Actuator/Value";
365  keys[STIFF_R_HAND] = prefix + "RHand/Hardness/Actuator/Value";
366  keys[STIFF_R_HIP_YAW_PITCH] = prefix + "RHipYawPitch/Hardness/Actuator/Value";
367  keys[STIFF_R_HIP_ROLL] = prefix + "RHipRoll/Hardness/Actuator/Value";
368  keys[STIFF_R_HIP_PITCH] = prefix + "RHipPitch/Hardness/Actuator/Value";
369  keys[STIFF_R_KNEE_PITCH] = prefix + "RKneePitch/Hardness/Actuator/Value";
370  keys[STIFF_R_ANKLE_PITCH] = prefix + "RAnklePitch/Hardness/Actuator/Value";
371  keys[STIFF_R_ANKLE_ROLL] = prefix + "RAnkleRoll/Hardness/Actuator/Value";
372 
373  // Inertial sensors
374  keys[ACC_X] = prefix + "InertialSensor/AccX/Sensor/Value";
375  keys[ACC_Y] = prefix + "InertialSensor/AccY/Sensor/Value";
376  keys[ACC_Z] = prefix + "InertialSensor/AccZ/Sensor/Value";
377  keys[GYR_X] = prefix + "InertialSensor/GyrX/Sensor/Value";
378  keys[GYR_Y] = prefix + "InertialSensor/GyrY/Sensor/Value";
379  keys[GYR_REF] = prefix + "InertialSensor/GyrRef/Sensor/Value";
380  keys[ANGLE_X] = prefix + "InertialSensor/AngleX/Sensor/Value";
381  keys[ANGLE_Y] = prefix + "InertialSensor/AngleY/Sensor/Value";
382 
383  // FSR sensors
384  keys[L_FSR_FL] = prefix + "LFoot/FSR/FrontLeft/Sensor/Value";
385  keys[L_FSR_FR] = prefix + "LFoot/FSR/FrontRight/Sensor/Value";
386  keys[L_FSR_RL] = prefix + "LFoot/FSR/RearLeft/Sensor/Value";
387  keys[L_FSR_RR] = prefix + "LFoot/FSR/RearRight/Sensor/Value";
388  keys[R_FSR_FL] = prefix + "RFoot/FSR/FrontLeft/Sensor/Value";
389  keys[R_FSR_FR] = prefix + "RFoot/FSR/FrontRight/Sensor/Value";
390  keys[R_FSR_RL] = prefix + "RFoot/FSR/RearLeft/Sensor/Value";
391  keys[R_FSR_RR] = prefix + "RFoot/FSR/RearRight/Sensor/Value";
392  keys[L_COP_X] = prefix + "LFoot/FSR/CenterOfPressure/X/Sensor/Value";
393  keys[L_COP_Y] = prefix + "LFoot/FSR/CenterOfPressure/Y/Sensor/Value";
394  keys[L_TOTAL_WEIGHT] = prefix + "LFoot/FSR/TotalWeight/Sensor/Value";
395  keys[R_COP_X] = prefix + "RFoot/FSR/CenterOfPressure/X/Sensor/Value";
396  keys[R_COP_Y] = prefix + "RFoot/FSR/CenterOfPressure/Y/Sensor/Value";
397  keys[R_TOTAL_WEIGHT] = prefix + "RFoot/FSR/TotalWeight/Sensor/Value";
398 
399  // Ultrasonic
400  keys[ULTRASONIC_DIRECTION] = prefix + "US/Actuator/Value";
401  keys[ULTRASONIC_DISTANCE] = prefix + "US/Sensor/Value";
402 
403  keys[ULTRASONIC_DISTANCE_LEFT_0] = prefix + "US/Left/Sensor/Value";
404  keys[ULTRASONIC_DISTANCE_LEFT_1] = prefix + "US/Left/Sensor/Value1";
405  keys[ULTRASONIC_DISTANCE_LEFT_2] = prefix + "US/Left/Sensor/Value2";
406  keys[ULTRASONIC_DISTANCE_LEFT_3] = prefix + "US/Left/Sensor/Value3";
407 
408  keys[ULTRASONIC_DISTANCE_RIGHT_0] = prefix + "US/Right/Sensor/Value";
409  keys[ULTRASONIC_DISTANCE_RIGHT_1] = prefix + "US/Right/Sensor/Value1";
410  keys[ULTRASONIC_DISTANCE_RIGHT_2] = prefix + "US/Right/Sensor/Value2";
411  keys[ULTRASONIC_DISTANCE_RIGHT_3] = prefix + "US/Right/Sensor/Value3";
412 
413  // Bumpers and Buttons
414  keys[L_FOOT_BUMPER_L] = prefix + "LFoot/Bumper/Left/Sensor/Value";
415  keys[L_FOOT_BUMPER_R] = prefix + "LFoot/Bumper/Right/Sensor/Value";
416  keys[R_FOOT_BUMPER_L] = prefix + "RFoot/Bumper/Left/Sensor/Value";
417  keys[R_FOOT_BUMPER_R] = prefix + "RFoot/Bumper/Right/Sensor/Value";
418 
419  keys[HEAD_TOUCH_FRONT] = prefix + "Head/Touch/Front/Sensor/Value";
420  keys[HEAD_TOUCH_MIDDLE] = prefix + "Head/Touch/Middle/Sensor/Value";
421  keys[HEAD_TOUCH_REAR] = prefix + "Head/Touch/Rear/Sensor/Value";
422 
423  keys[CHEST_BUTTON] = prefix + "ChestBoard/Button/Sensor/Value";
424 
425  // Battery
426  keys[BATTERY_CHARGE] = prefix + "Battery/Charge/Sensor/Value";
427 
428  try {
429  memfa_->ConnectToVariables(naoqi_broker, keys, false);
430  } catch (AL::ALError &e) {
431  throw Exception("Failed to setup fast memory access: %s", e.toString().c_str());
432  }
433 
434  // Setup alias for setting stiffness, reuse fastmem keys vector
435  if (robot_type_ == NaoJointPositionInterface::ROBOTYPE_ROBOCUP) {
436  alljoint_names_.arraySetSize(22);
437  } else {
438  alljoint_names_.arraySetSize(26);
439  alljoint_names_[STIFFJ_L_WRIST_YAW] = "LWristYaw";
440  alljoint_names_[STIFFJ_L_HAND] = "LHand";
441  alljoint_names_[STIFFJ_R_WRIST_YAW] = "RWristYaw";
442  alljoint_names_[STIFFJ_R_HAND] = "RHand";
443  }
444  alljoint_names_[STIFFJ_HEAD_PITCH] = "HeadPitch";
445  alljoint_names_[STIFFJ_HEAD_YAW] = "HeadYaw";
446  alljoint_names_[STIFFJ_L_SHOULDER_PITCH] = "LShoulderPitch";
447  alljoint_names_[STIFFJ_L_SHOULDER_ROLL] = "LShoulderRoll";
448  alljoint_names_[STIFFJ_L_ELBOW_YAW] = "LElbowYaw";
449  alljoint_names_[STIFFJ_L_ELBOW_ROLL] = "LElbowRoll";
450  alljoint_names_[STIFFJ_L_HIP_YAW_PITCH] = "LHipYawPitch";
451  alljoint_names_[STIFFJ_L_HIP_ROLL] = "LHipRoll";
452  alljoint_names_[STIFFJ_L_HIP_PITCH] = "LHipPitch";
453  alljoint_names_[STIFFJ_L_KNEE_PITCH] = "LKneePitch";
454  alljoint_names_[STIFFJ_L_ANKLE_PITCH] = "LAnklePitch";
455  alljoint_names_[STIFFJ_L_ANKLE_ROLL] = "LAnkleRoll";
456 
457  alljoint_names_[STIFFJ_R_SHOULDER_PITCH] = "RShoulderPitch";
458  alljoint_names_[STIFFJ_R_SHOULDER_ROLL] = "RShoulderRoll";
459  alljoint_names_[STIFFJ_R_ELBOW_YAW] = "RElbowYaw";
460  alljoint_names_[STIFFJ_R_ELBOW_ROLL] = "RElbowRoll";
461  alljoint_names_[STIFFJ_R_HIP_YAW_PITCH] = "RHipYawPitch";
462  alljoint_names_[STIFFJ_R_HIP_ROLL] = "RHipRoll";
463  alljoint_names_[STIFFJ_R_HIP_PITCH] = "RHipPitch";
464  alljoint_names_[STIFFJ_R_KNEE_PITCH] = "RKneePitch";
465  alljoint_names_[STIFFJ_R_ANKLE_PITCH] = "RAnklePitch";
466  alljoint_names_[STIFFJ_R_ANKLE_ROLL] = "RAnkleRoll";
467 
468  try {
469  AL::ALValue setJointStiffnessAlias;
470  // Alias for all joint stiffness
471  setJointStiffnessAlias.clear();
472  setJointStiffnessAlias.arraySetSize(2);
473  setJointStiffnessAlias[0] = std::string("setJointStiffness");
474  setJointStiffnessAlias[1].arraySetSize(26);
475 
476  // stiffness list
477  int offset = STIFF_HEAD_PITCH - HEAD_PITCH;
478  for (int i = HEAD_PITCH; i <= R_ANKLE_ROLL; ++i) {
479  setJointStiffnessAlias[1][i] = keys[i + offset];
480  }
481 
482  dcm_->createAlias(setJointStiffnessAlias);
483  } catch (AL::ALError &e) {
484  memfa_.reset();
485  throw Exception("Failed to create SetJointStiffness alias: %s", e.toString().c_str());
486  }
487 
488  joint_pos_if_ = blackboard->open_for_writing<NaoJointPositionInterface>("Nao Joint Positions");
489  joint_stiffness_if_ =
490  blackboard->open_for_writing<NaoJointStiffnessInterface>("Nao Joint Stiffness");
491  sensor_if_ = blackboard->open_for_writing<NaoSensorInterface>("Nao Sensors");
492 
493  joint_pos_highfreq_if_ =
494  blackboard->open_for_writing<NaoJointPositionInterface>("Nao Joint Positions HF");
495  joint_stiffness_highfreq_if_ =
496  blackboard->open_for_writing<NaoJointStiffnessInterface>("Nao Joint Stiffness HF");
497  sensor_highfreq_if_ = blackboard->open_for_writing<NaoSensorInterface>("Nao Sensors HF");
498 
499  // Get all values from ALMemory using fastaccess
500  dcm_time_ = dcm_->getTime(0);
501  memfa_->GetValues(values_);
502 
503  joint_pos_if_->set_robot_type(robot_type_);
504  joint_pos_if_->set_robot_version(robot_version_);
505  joint_pos_highfreq_if_->set_robot_type(robot_type_);
506  joint_pos_highfreq_if_->set_robot_version(robot_version_);
507 
508  sensor_if_->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
509  sensor_highfreq_if_->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
510 
511  // Write once the current data on startup
512  update_interfaces(joint_pos_if_, joint_stiffness_if_, sensor_if_);
513  update_interfaces(joint_pos_highfreq_if_, joint_stiffness_highfreq_if_, sensor_highfreq_if_);
514 
515  highfreq_thread_ = new NaoQiDCMThread::HighFreqThread(this);
516  highfreq_thread_->start();
517 
518  dcm_sigconn_ = dcm_->getGenericProxy()->getModule()->atPostProcess(
519  boost::bind(&NaoQiDCMThread::dcm_callback, this));
520 
521  /*
522  AL::ALValue cmd;
523  cmd.arraySetSize(3);
524  cmd[0] = std::string("setJointStiffness");
525  cmd[1] = std::string("Merge");
526  cmd[2].arraySetSize(1);
527  cmd[2][0].arraySetSize(2);
528  cmd[2][0][0] = 1.0;
529  cmd[2][0][1] = dcm_time_ + 500;
530  dcm_->set(cmd);
531 
532  send_command("HeadPitch/Position/Sensor/Value", 0.4, 1000);
533  */
534 }
535 
536 void
537 NaoQiDCMThread::dcm_callback()
538 {
539  highfreq_thread_->wakeup();
540 }
541 
542 void
544 {
545  dcm_sigconn_.disconnect();
546 
547  highfreq_thread_->cancel();
548  highfreq_thread_->join();
549  delete highfreq_thread_;
550 
551  blackboard->close(joint_pos_if_);
552  blackboard->close(joint_stiffness_if_);
553  blackboard->close(sensor_if_);
554  blackboard->close(joint_pos_highfreq_if_);
555  blackboard->close(joint_stiffness_highfreq_if_);
556  blackboard->close(sensor_highfreq_if_);
557  joint_pos_if_ = NULL;
558  joint_stiffness_if_ = NULL;
559  sensor_if_ = NULL;
560  joint_pos_highfreq_if_ = NULL;
561  joint_stiffness_highfreq_if_ = NULL;
562  sensor_highfreq_if_ = NULL;
563 
564  memfa_.reset();
565  dcm_.reset();
566 }
567 
568 void
569 NaoQiDCMThread::read_values()
570 {
571  values_.lock();
572  dcm_time_ = dcm_->getTime(0);
573  memfa_->GetValues(values_);
574  values_.unlock();
575 }
576 
577 void
579 {
580  update_interfaces(joint_pos_if_, joint_stiffness_if_, sensor_if_);
581 }
582 
583 void
584 NaoQiDCMThread::update_interfaces(NaoJointPositionInterface * joint_pos_if,
585  NaoJointStiffnessInterface *joint_stiffness_if,
586  NaoSensorInterface * sensor_if)
587 {
588  // Joint Position
589  // Head
590  joint_pos_if->set_head_yaw(values_[HEAD_YAW]);
591  joint_pos_if->set_head_pitch(values_[HEAD_PITCH]);
592  // Arms
593  joint_pos_if->set_l_shoulder_pitch(values_[L_SHOULDER_PITCH]);
594  joint_pos_if->set_l_shoulder_roll(values_[L_SHOULDER_ROLL]);
595  joint_pos_if->set_l_elbow_yaw(values_[L_ELBOW_YAW]);
596  joint_pos_if->set_l_elbow_roll(values_[L_ELBOW_ROLL]);
597  joint_pos_if->set_l_wrist_yaw(values_[L_WRIST_YAW]);
598  joint_pos_if->set_l_hand(values_[L_HAND]);
599  joint_pos_if->set_r_shoulder_pitch(values_[R_SHOULDER_PITCH]);
600  joint_pos_if->set_r_shoulder_roll(values_[R_SHOULDER_ROLL]);
601  joint_pos_if->set_r_elbow_yaw(values_[R_ELBOW_YAW]);
602  joint_pos_if->set_r_elbow_roll(values_[R_ELBOW_ROLL]);
603  joint_pos_if->set_r_wrist_yaw(values_[R_WRIST_YAW]);
604  joint_pos_if->set_r_hand(values_[R_HAND]);
605  // Hip
606  joint_pos_if->set_l_hip_yaw_pitch(values_[L_HIP_YAW_PITCH]);
607  joint_pos_if->set_l_hip_pitch(values_[L_HIP_PITCH]);
608  joint_pos_if->set_l_hip_roll(values_[L_HIP_ROLL]);
609  joint_pos_if->set_r_hip_yaw_pitch(values_[R_HIP_YAW_PITCH]);
610  joint_pos_if->set_r_hip_pitch(values_[R_HIP_PITCH]);
611  joint_pos_if->set_r_hip_roll(values_[R_HIP_ROLL]);
612  // Knees
613  joint_pos_if->set_l_knee_pitch(values_[L_KNEE_PITCH]);
614  joint_pos_if->set_r_knee_pitch(values_[R_KNEE_PITCH]);
615  // Feet
616  joint_pos_if->set_l_ankle_pitch(values_[L_ANKLE_PITCH]);
617  joint_pos_if->set_l_ankle_roll(values_[L_ANKLE_ROLL]);
618  joint_pos_if->set_r_ankle_pitch(values_[R_ANKLE_PITCH]);
619  joint_pos_if->set_r_ankle_roll(values_[R_ANKLE_ROLL]);
620 
621  joint_pos_if->set_time(dcm_time_);
622 
623  // Joint Stiffness
624  // Head
625  joint_stiffness_if->set_head_yaw(values_[STIFF_HEAD_YAW]);
626  joint_stiffness_if->set_head_pitch(values_[STIFF_HEAD_PITCH]);
627  // Arms
628  joint_stiffness_if->set_l_shoulder_pitch(values_[STIFF_L_SHOULDER_PITCH]);
629  joint_stiffness_if->set_l_shoulder_roll(values_[STIFF_L_SHOULDER_ROLL]);
630  joint_stiffness_if->set_l_elbow_yaw(values_[STIFF_L_ELBOW_YAW]);
631  joint_stiffness_if->set_l_elbow_roll(values_[STIFF_L_ELBOW_ROLL]);
632  joint_stiffness_if->set_l_wrist_yaw(values_[STIFF_L_WRIST_YAW]);
633  joint_stiffness_if->set_l_hand(values_[STIFF_L_HAND]);
634  joint_stiffness_if->set_r_shoulder_pitch(values_[STIFF_R_SHOULDER_PITCH]);
635  joint_stiffness_if->set_r_shoulder_roll(values_[STIFF_R_SHOULDER_ROLL]);
636  joint_stiffness_if->set_r_elbow_yaw(values_[STIFF_R_ELBOW_YAW]);
637  joint_stiffness_if->set_r_elbow_roll(values_[STIFF_R_ELBOW_ROLL]);
638  joint_stiffness_if->set_r_wrist_yaw(values_[STIFF_R_WRIST_YAW]);
639  joint_stiffness_if->set_r_hand(values_[STIFF_R_HAND]);
640  // Hip
641  joint_stiffness_if->set_l_hip_yaw_pitch(values_[STIFF_L_HIP_YAW_PITCH]);
642  joint_stiffness_if->set_l_hip_pitch(values_[STIFF_L_HIP_PITCH]);
643  joint_stiffness_if->set_l_hip_roll(values_[STIFF_L_HIP_ROLL]);
644  // RHipYawPitch stiffness is always 0, copy from RYawPitch
645  joint_stiffness_if->set_r_hip_yaw_pitch(values_[STIFF_L_HIP_YAW_PITCH]);
646  joint_stiffness_if->set_r_hip_pitch(values_[STIFF_R_HIP_PITCH]);
647  joint_stiffness_if->set_r_hip_roll(values_[STIFF_R_HIP_ROLL]);
648  // Knees
649  joint_stiffness_if->set_l_knee_pitch(values_[STIFF_L_KNEE_PITCH]);
650  joint_stiffness_if->set_r_knee_pitch(values_[STIFF_R_KNEE_PITCH]);
651  // Feet
652  joint_stiffness_if->set_l_ankle_pitch(values_[STIFF_L_ANKLE_PITCH]);
653  joint_stiffness_if->set_l_ankle_roll(values_[STIFF_L_ANKLE_ROLL]);
654  joint_stiffness_if->set_r_ankle_pitch(values_[STIFF_R_ANKLE_PITCH]);
655  joint_stiffness_if->set_r_ankle_roll(values_[STIFF_R_ANKLE_ROLL]);
656 
657  float min_stiffness = 1.;
658  for (int i = STIFF_HEAD_YAW; i <= STIFF_R_ANKLE_ROLL; ++i) {
659  // ignore wrist and hand on RoboCup version
660  if ((robot_type_ == NaoJointPositionInterface::ROBOTYPE_ROBOCUP)
661  && ((i == STIFF_L_WRIST_YAW) || (i == STIFF_L_HAND) || (i == STIFF_R_WRIST_YAW)
662  || (i == STIFF_R_HAND)))
663  continue;
664 
665  // ignore RHipYawPitch stiffness, it's always 0
666  if (i == STIFF_R_HIP_YAW_PITCH)
667  continue;
668 
669  if (values_[i] < min_stiffness)
670  min_stiffness = values_[i];
671  }
672  joint_stiffness_if->set_minimum(min_stiffness);
673 
674  // Sensors
675  // FSRs
676  sensor_if->set_l_fsr_fl(values_[L_FSR_FL]);
677  sensor_if->set_l_fsr_fr(values_[L_FSR_FR]);
678  sensor_if->set_l_fsr_rl(values_[L_FSR_RL]);
679  sensor_if->set_l_fsr_rr(values_[L_FSR_RR]);
680  sensor_if->set_r_fsr_fl(values_[R_FSR_FL]);
681  sensor_if->set_r_fsr_fr(values_[R_FSR_FR]);
682  sensor_if->set_r_fsr_rl(values_[R_FSR_RL]);
683  sensor_if->set_r_fsr_rr(values_[R_FSR_RR]);
684 
685  sensor_if->set_l_cop_x(values_[L_COP_X]);
686  sensor_if->set_l_cop_y(values_[L_COP_Y]);
687  sensor_if->set_l_total_weight(values_[L_TOTAL_WEIGHT]);
688 
689  sensor_if->set_r_cop_x(values_[R_COP_X]);
690  sensor_if->set_r_cop_y(values_[R_COP_Y]);
691  sensor_if->set_r_total_weight(values_[R_TOTAL_WEIGHT]);
692 
693  // Buttons and bumpers
694  sensor_if->set_chest_button((values_[CHEST_BUTTON] >= 0.5) ? 1 : 0);
695  sensor_if->set_head_touch_front((values_[HEAD_TOUCH_FRONT] >= 0.5) ? 1 : 0);
696  sensor_if->set_head_touch_middle((values_[HEAD_TOUCH_MIDDLE] >= 0.5) ? 1 : 0);
697  sensor_if->set_head_touch_rear((values_[HEAD_TOUCH_REAR] >= 0.5) ? 1 : 0);
698 
699  sensor_if->set_l_foot_bumper_l((values_[L_FOOT_BUMPER_L] >= 0.5) ? 1 : 0);
700  sensor_if->set_l_foot_bumper_r((values_[L_FOOT_BUMPER_R] >= 0.5) ? 1 : 0);
701  sensor_if->set_r_foot_bumper_l((values_[R_FOOT_BUMPER_L] >= 0.5) ? 1 : 0);
702  sensor_if->set_r_foot_bumper_r((values_[R_FOOT_BUMPER_R] >= 0.5) ? 1 : 0);
703 
704  // Inertial measurement unit
705  sensor_if->set_accel_x(values_[ACC_X] / ACCELEROMETER_G_FACTOR);
706  sensor_if->set_accel_y(values_[ACC_Y] / ACCELEROMETER_G_FACTOR);
707  sensor_if->set_accel_z(values_[ACC_Z] / ACCELEROMETER_G_FACTOR);
708 
709  sensor_if->set_gyro_x(values_[GYR_X]);
710  sensor_if->set_gyro_y(values_[GYR_Y]);
711  sensor_if->set_gyro_ref(values_[GYR_REF]);
712 
713  sensor_if->set_angle_x(values_[ANGLE_X]);
714  sensor_if->set_angle_y(values_[ANGLE_Y]);
715 
716  // Ultrasonic sound
718  switch (us_dir) {
719  case NaoSensorInterface::USD_LEFT_LEFT:
720  case NaoSensorInterface::USD_RIGHT_LEFT: {
721  float us_left[4] = {values_[ULTRASONIC_DISTANCE], 0, 0, 0};
722  sensor_if->set_ultrasonic_distance_left(us_left);
723 
724  float us_right[4] = {0, 0, 0, 0};
725  sensor_if->set_ultrasonic_distance_right(us_right);
726  } break;
727  case NaoSensorInterface::USD_LEFT_RIGHT:
728  case NaoSensorInterface::USD_RIGHT_RIGHT: {
729  float us_left[4] = {0, 0, 0, 0};
730  sensor_if->set_ultrasonic_distance_left(us_left);
731 
732  float us_right[4] = {values_[ULTRASONIC_DISTANCE], 0, 0, 0};
733  sensor_if->set_ultrasonic_distance_right(us_right);
734  } break;
735 
736  default: {
737  float us_left[4] = {values_[ULTRASONIC_DISTANCE_LEFT_0],
738  values_[ULTRASONIC_DISTANCE_LEFT_1],
739  values_[ULTRASONIC_DISTANCE_LEFT_2],
740  values_[ULTRASONIC_DISTANCE_LEFT_3]};
741  sensor_if->set_ultrasonic_distance_left(us_left);
742 
743  float us_right[4] = {values_[ULTRASONIC_DISTANCE_RIGHT_0],
744  values_[ULTRASONIC_DISTANCE_RIGHT_1],
745  values_[ULTRASONIC_DISTANCE_RIGHT_2],
746  values_[ULTRASONIC_DISTANCE_RIGHT_3]};
747  sensor_if->set_ultrasonic_distance_right(us_right);
748  } break;
749  }
750 
751  // Battery
752  sensor_if->set_battery_charge(values_[BATTERY_CHARGE]);
753 
754  // Write to blackboard
755  joint_pos_if->write();
756  joint_stiffness_if->write();
757  sensor_if->write();
758 }
759 
760 void
761 NaoQiDCMThread::process_messages()
762 {
763  // *** Joint position messages
764  while (!joint_pos_if_->msgq_empty()) {
765  if (NaoJointPositionInterface::SetServoMessage *msg = joint_pos_if_->msgq_first_safe(msg)) {
766  send_commands(msg->servo(), "Position", msg->value(), msg->time());
767  }
768 
770  joint_pos_if_->msgq_first_safe(msg)) {
771  }
772 
774  joint_pos_if_->msgq_first_safe(msg)) {
775  std::vector<std::string> servos = parse_servo_bitfield(msg->servo());
776  std::vector<float> values(servos.size(), msg->value());
777  almotion_->setAngles(servos, values, msg->speed());
778  }
779 
781  joint_pos_if_->msgq_first_safe(msg)) {
782  motion::move_joints(almotion_,
783  /* head */ msg->head_yaw(),
784  msg->head_pitch(),
785  /* l shoulder */ msg->l_shoulder_pitch(),
786  msg->l_shoulder_roll(),
787  /* l elbow */ msg->l_elbow_yaw(),
788  msg->l_elbow_roll(),
789  /* l wrist/hand */ msg->l_wrist_yaw(),
790  msg->l_hand(),
791  /* l hip */ msg->l_hip_yaw_pitch(),
792  msg->l_hip_roll(),
793  msg->l_hip_pitch(),
794  /* l knee */ msg->l_knee_pitch(),
795  /* l ankle */ msg->l_ankle_pitch(),
796  msg->l_ankle_roll(),
797  /* r shoulder */ msg->r_shoulder_pitch(),
798  msg->r_shoulder_roll(),
799  /* r elbow */ msg->r_elbow_yaw(),
800  msg->r_elbow_roll(),
801  /* r wrist/hand */ msg->r_wrist_yaw(),
802  msg->r_hand(),
803  /* r hip */ msg->r_hip_yaw_pitch(),
804  msg->r_hip_roll(),
805  msg->r_hip_pitch(),
806  /* r knee */ msg->r_knee_pitch(),
807  /* r ankle */ msg->r_ankle_pitch(),
808  msg->r_ankle_roll(),
809  /* speed */ msg->speed());
810  }
811 
812  joint_pos_if_->msgq_pop();
813  }
814 
815  // *** Joint stiffness messages
816  while (!joint_stiffness_if_->msgq_empty()) {
818  joint_stiffness_if_->msgq_first_safe(msg)) {
819  /* DCM version, disfunctional due to ALMotion deficiencies
820  send_commands(msg->servo(), "Hardness", msg->value(),
821  "Merge", (int)roundf(1000. * msg->time_sec()));
822  */
823 
824  std::vector<std::string> servos = parse_servo_bitfield(msg->servo());
825  std::vector<float> values(servos.size(), msg->value());
826 
827  almotion_->post.stiffnessInterpolation(servos, values, msg->time_sec());
828 
830  joint_stiffness_if_->msgq_first_safe(msg)) {
831  /* Cannot be used atm because ALMotion will not update its internal
832  * belief of stiffness values causing any further motion via DCM
833  * or ALMotion to fail
834  // use setJointStiffness alias setup in init()
835  AL::ALValue cmd;
836  cmd.arraySetSize(3);
837  cmd[0] = std::string("setJointStiffness");
838  cmd[1] = std::string("Merge");
839  cmd[2].arraySetSize(1);
840  cmd[2][0].arraySetSize(2);
841  cmd[2][0][0] = msg->value();
842  cmd[2][0][1] = dcm_time_ + (int)roundf(1000. * msg->time_sec());
843  try {
844  dcm_->set(cmd);
845  } catch (const AL::ALError &e) {
846  logger->log_warn(name(), "Failed to call setJointStiffness: %s",
847  e.toString().c_str());
848  }
849  */
850 
851  almotion_->post.stiffnessInterpolation("Body", msg->value(), msg->time_sec());
852  }
853 
855  joint_stiffness_if_->msgq_first_safe(msg)) {
856  std::vector<float> values(alljoint_names_.getSize());
857  values[STIFFJ_HEAD_PITCH] = msg->head_pitch();
858  values[STIFFJ_HEAD_YAW] = msg->head_yaw();
859  values[STIFFJ_L_SHOULDER_PITCH] = msg->l_shoulder_pitch();
860  values[STIFFJ_L_SHOULDER_ROLL] = msg->l_shoulder_roll();
861  values[STIFFJ_L_ELBOW_YAW] = msg->l_elbow_yaw();
862  values[STIFFJ_L_ELBOW_ROLL] = msg->l_elbow_roll();
863  values[STIFFJ_L_HIP_YAW_PITCH] = msg->l_hip_yaw_pitch();
864  values[STIFFJ_L_HIP_ROLL] = msg->l_hip_roll();
865  values[STIFFJ_L_HIP_PITCH] = msg->l_hip_pitch();
866  values[STIFFJ_L_KNEE_PITCH] = msg->l_knee_pitch();
867  values[STIFFJ_L_ANKLE_PITCH] = msg->l_ankle_pitch();
868  values[STIFFJ_L_ANKLE_ROLL] = msg->l_ankle_roll();
869 
870  values[STIFFJ_R_SHOULDER_PITCH] = msg->r_shoulder_pitch();
871  values[STIFFJ_R_SHOULDER_ROLL] = msg->r_shoulder_roll();
872  values[STIFFJ_R_ELBOW_YAW] = msg->r_elbow_yaw();
873  values[STIFFJ_R_ELBOW_ROLL] = msg->r_elbow_roll();
874  values[STIFFJ_R_HIP_YAW_PITCH] = msg->r_hip_yaw_pitch();
875  values[STIFFJ_R_HIP_ROLL] = msg->r_hip_roll();
876  values[STIFFJ_R_HIP_PITCH] = msg->r_hip_pitch();
877  values[STIFFJ_R_KNEE_PITCH] = msg->r_knee_pitch();
878  values[STIFFJ_R_ANKLE_PITCH] = msg->r_ankle_pitch();
879  values[STIFFJ_R_ANKLE_ROLL] = msg->r_ankle_roll();
880 
881  if (robot_type_ != NaoJointPositionInterface::ROBOTYPE_ROBOCUP) {
882  values[STIFFJ_L_WRIST_YAW] = msg->l_wrist_yaw();
883  values[STIFFJ_L_HAND] = msg->l_hand();
884  values[STIFFJ_R_WRIST_YAW] = msg->r_wrist_yaw();
885  values[STIFFJ_R_HAND] = msg->r_hand();
886  }
887 
888  almotion_->post.stiffnessInterpolation(alljoint_names_, values, msg->time_sec());
889  }
890 
891  joint_stiffness_if_->msgq_pop();
892  }
893 
894  // *** Sensor messages
895  while (!sensor_if_->msgq_empty()) {
897  int value = ultrasonic_value(msg->ultrasonic_direction());
898  send_command("US/Actuator/Value", value, "ClearAll", 0);
899 
900  sensor_if_->set_ultrasonic_direction(msg->ultrasonic_direction());
901  sensor_highfreq_if_->set_ultrasonic_direction(msg->ultrasonic_direction());
902 
903  } else if (NaoSensorInterface::StartUltrasonicMessage *msg = sensor_if_->msgq_first_safe(msg)) {
904  int value = ultrasonic_value(msg->ultrasonic_direction());
905  value += 64;
906  send_command("US/Actuator/Value", value, "ClearAll", 0);
907 
908  sensor_if_->set_ultrasonic_direction(msg->ultrasonic_direction());
909  sensor_highfreq_if_->set_ultrasonic_direction(msg->ultrasonic_direction());
910 
911  } else if (NaoSensorInterface::StopUltrasonicMessage *msg = sensor_if_->msgq_first_safe(msg)) {
912  send_command("US/Actuator/Value", 0, "ClearAll", 0);
913 
914  sensor_if_->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
915  sensor_highfreq_if_->set_ultrasonic_direction(NaoSensorInterface::USD_NONE);
916  }
917 
918  sensor_if_->msgq_pop();
919  }
920 }
921 
922 void
923 NaoQiDCMThread::send_commands(unsigned int servos,
924  const std::string &what,
925  float value,
926  int time_offset)
927 {
928  /*
929  almotion_->wbEnable(false);
930  almotion_->wbEnableEffectorControl("LArm", false);
931  almotion_->wbEnableEffectorControl("RArm", false);
932  almotion_->setWalkArmsEnable(false, false);
933  almotion_->killAll();
934  */
935 
936  std::vector<std::string> servonames = parse_servo_bitfield(servos);
937 
938  std::vector<std::string>::iterator s;
939  for (s = servonames.begin(); s != servonames.end(); ++s) {
940  float v = value;
941  if (*s == "HeadYaw") {
942  v = CLIP_VALUE(HEAD_YAW, value);
943  } else if (*s == "LShoulderPitch") {
944  v = CLIP_VALUE(L_SHOULDER_PITCH, value);
945  } else if (*s == "RShoulderPitch") {
946  v = CLIP_VALUE(R_SHOULDER_PITCH, value);
947  }
948  send_command(*s + "/" + what + "/Actuator/Value", v, "Merge", time_offset);
949  }
950 }
951 
952 void
953 NaoQiDCMThread::send_command(const std::string &name,
954  float value,
955  const std::string &kind,
956  int time_offset)
957 {
958  AL::ALValue cmd;
959 
960  /*
961  printf("Command for %s to %f in %i ms (%i vs. %i)\n",
962  name.c_str(), value, time_offset, dcm_->getTime(time_offset),
963  dcm_time_ + time_offset);
964  */
965 
966  cmd.arraySetSize(3);
967  cmd[0] = name;
968  cmd[1] = kind;
969  cmd[2].arraySetSize(1);
970  cmd[2][0].arraySetSize(2);
971  cmd[2][0][0] = value;
972  cmd[2][0][1] = dcm_time_ + time_offset;
973 
974  dcm_->set(cmd);
975 }
976 
977 std::vector<std::string>
978 NaoQiDCMThread::parse_servo_bitfield(unsigned int servos)
979 {
980  std::vector<std::string> servonames;
981 
982  if (servos & NaoJointPositionInterface::SERVO_head_yaw)
983  servonames.push_back("HeadYaw");
984 
985  if (servos & NaoJointPositionInterface::SERVO_head_pitch)
986  servonames.push_back("HeadPitch");
987 
988  if (servos & NaoJointPositionInterface::SERVO_l_shoulder_pitch)
989  servonames.push_back("LShoulderPitch");
990 
991  if (servos & NaoJointPositionInterface::SERVO_l_shoulder_roll)
992  servonames.push_back("LShoulderRoll");
993 
994  if (servos & NaoJointPositionInterface::SERVO_l_elbow_yaw)
995  servonames.push_back("LElbowYaw");
996 
997  if (servos & NaoJointPositionInterface::SERVO_l_elbow_roll)
998  servonames.push_back("LElbowRoll");
999 
1000  if (servos & NaoJointPositionInterface::SERVO_r_shoulder_pitch)
1001  servonames.push_back("RShoulderPitch");
1002 
1003  if (servos & NaoJointPositionInterface::SERVO_r_shoulder_roll)
1004  servonames.push_back("RShoulderRoll");
1005 
1006  if (servos & NaoJointPositionInterface::SERVO_r_elbow_yaw)
1007  servonames.push_back("RElbowYaw");
1008 
1009  if (servos & NaoJointPositionInterface::SERVO_r_elbow_roll)
1010  servonames.push_back("RElbowRoll");
1011 
1012  if (servos & NaoJointPositionInterface::SERVO_l_hip_yaw_pitch)
1013  servonames.push_back("LHipYawPitch");
1014 
1015  if (servos & NaoJointPositionInterface::SERVO_l_hip_pitch)
1016  servonames.push_back("LHipPitch");
1017 
1018  if (servos & NaoJointPositionInterface::SERVO_l_hip_roll)
1019  servonames.push_back("LHipRoll");
1020 
1021  if (servos & NaoJointPositionInterface::SERVO_r_hip_yaw_pitch)
1022  servonames.push_back("RHipYawPitch");
1023 
1024  if (servos & NaoJointPositionInterface::SERVO_r_hip_pitch)
1025  servonames.push_back("RHipPitch");
1026 
1027  if (servos & NaoJointPositionInterface::SERVO_r_hip_roll)
1028  servonames.push_back("RHipRoll");
1029 
1030  if (servos & NaoJointPositionInterface::SERVO_l_knee_pitch)
1031  servonames.push_back("LKneePitch");
1032 
1033  if (servos & NaoJointPositionInterface::SERVO_r_knee_pitch)
1034  servonames.push_back("RKneePitch");
1035 
1036  if (servos & NaoJointPositionInterface::SERVO_l_ankle_pitch)
1037  servonames.push_back("LAnklePitch");
1038 
1039  if (servos & NaoJointPositionInterface::SERVO_l_ankle_roll)
1040  servonames.push_back("LAnkleRoll");
1041 
1042  if (servos & NaoJointPositionInterface::SERVO_r_ankle_pitch)
1043  servonames.push_back("RAnklePitch");
1044 
1045  if (servos & NaoJointPositionInterface::SERVO_r_ankle_roll)
1046  servonames.push_back("RAnkleRoll");
1047 
1048  return servonames;
1049 }
1050 
1051 int
1052 NaoQiDCMThread::ultrasonic_value(fawkes::NaoSensorInterface::UltrasonicDirection direction)
1053 {
1054  int value = 0;
1055  switch (direction) {
1056  case NaoSensorInterface::USD_LEFT_LEFT: value = 0; break;
1057  case NaoSensorInterface::USD_LEFT_RIGHT: value = 1; break;
1058  case NaoSensorInterface::USD_RIGHT_LEFT: value = 2; break;
1059  case NaoSensorInterface::USD_RIGHT_RIGHT: value = 3; break;
1060  case NaoSensorInterface::USD_BOTH_BOTH: value = 12; break;
1061  default:
1062  logger->log_warn(name(),
1063  "Illegal ultrasonic direction, "
1064  "using left-right");
1065  }
1066  return value;
1067 }
Thread to write data at full DCM frequency.
Definition: dcm_thread.cpp:203
virtual void loop()
Code to execute in the thread.
Definition: dcm_thread.cpp:215
HighFreqThread(NaoQiDCMThread *parent)
Constructor.
Definition: dcm_thread.cpp:208
Thread to provide DCM to Fawkes.
Definition: dcm_thread.h:54
virtual void finalize()
Finalize the thread.
Definition: dcm_thread.cpp:543
virtual void init()
Initialize the thread.
Definition: dcm_thread.cpp:263
virtual ~NaoQiDCMThread()
Destructor.
Definition: dcm_thread.cpp:258
NaoQiDCMThread()
Constructor.
Definition: dcm_thread.cpp:251
virtual void loop()
Code to execute in the thread.
Definition: dcm_thread.cpp:578
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
virtual void lock() const
Lock vector.
Definition: lock_vector.h:93
virtual void unlock() const
Unlock vector.
Definition: lock_vector.h:111
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
MoveServoMessage Fawkes BlackBoard Interface Message.
MoveServosMessage Fawkes BlackBoard Interface Message.
SetServoMessage Fawkes BlackBoard Interface Message.
SetServosMessage Fawkes BlackBoard Interface Message.
NaoJointPositionInterface Fawkes BlackBoard Interface.
void set_l_wrist_yaw(const float new_l_wrist_yaw)
Set l_wrist_yaw value.
void set_r_ankle_pitch(const float new_r_ankle_pitch)
Set r_ankle_pitch value.
void set_r_hip_pitch(const float new_r_hip_pitch)
Set r_hip_pitch value.
void set_r_elbow_yaw(const float new_r_elbow_yaw)
Set r_elbow_yaw value.
void set_l_ankle_roll(const float new_l_ankle_roll)
Set l_ankle_roll value.
void set_r_shoulder_roll(const float new_r_shoulder_roll)
Set r_shoulder_roll value.
void set_l_shoulder_pitch(const float new_l_shoulder_pitch)
Set l_shoulder_pitch value.
void set_r_knee_pitch(const float new_r_knee_pitch)
Set r_knee_pitch value.
void set_r_hip_yaw_pitch(const float new_r_hip_yaw_pitch)
Set r_hip_yaw_pitch value.
void set_l_ankle_pitch(const float new_l_ankle_pitch)
Set l_ankle_pitch value.
void set_r_wrist_yaw(const float new_r_wrist_yaw)
Set r_wrist_yaw value.
void set_l_elbow_yaw(const float new_l_elbow_yaw)
Set l_elbow_yaw value.
void set_l_hip_yaw_pitch(const float new_l_hip_yaw_pitch)
Set l_hip_yaw_pitch value.
void set_r_elbow_roll(const float new_r_elbow_roll)
Set r_elbow_roll value.
void set_l_hand(const float new_l_hand)
Set l_hand value.
void set_time(const int32_t new_time)
Set time value.
void set_l_shoulder_roll(const float new_l_shoulder_roll)
Set l_shoulder_roll value.
void set_robot_version(unsigned int index, const uint8_t new_robot_version)
Set robot_version value at given index.
void set_r_ankle_roll(const float new_r_ankle_roll)
Set r_ankle_roll value.
void set_l_knee_pitch(const float new_l_knee_pitch)
Set l_knee_pitch value.
void set_r_shoulder_pitch(const float new_r_shoulder_pitch)
Set r_shoulder_pitch value.
void set_l_elbow_roll(const float new_l_elbow_roll)
Set l_elbow_roll value.
void set_r_hip_roll(const float new_r_hip_roll)
Set r_hip_roll value.
void set_robot_type(const RobotType new_robot_type)
Set robot_type value.
void set_l_hip_pitch(const float new_l_hip_pitch)
Set l_hip_pitch value.
void set_head_yaw(const float new_head_yaw)
Set head_yaw value.
void set_head_pitch(const float new_head_pitch)
Set head_pitch value.
void set_l_hip_roll(const float new_l_hip_roll)
Set l_hip_roll value.
void set_r_hand(const float new_r_hand)
Set r_hand value.
SetBodyStiffnessMessage Fawkes BlackBoard Interface Message.
SetStiffnessMessage Fawkes BlackBoard Interface Message.
SetStiffnessesMessage Fawkes BlackBoard Interface Message.
NaoJointStiffnessInterface Fawkes BlackBoard Interface.
void set_r_hip_roll(const float new_r_hip_roll)
Set r_hip_roll value.
void set_r_ankle_pitch(const float new_r_ankle_pitch)
Set r_ankle_pitch value.
void set_l_shoulder_pitch(const float new_l_shoulder_pitch)
Set l_shoulder_pitch value.
void set_l_ankle_roll(const float new_l_ankle_roll)
Set l_ankle_roll value.
void set_r_hip_pitch(const float new_r_hip_pitch)
Set r_hip_pitch value.
void set_l_elbow_yaw(const float new_l_elbow_yaw)
Set l_elbow_yaw value.
void set_r_hip_yaw_pitch(const float new_r_hip_yaw_pitch)
Set r_hip_yaw_pitch value.
void set_r_wrist_yaw(const float new_r_wrist_yaw)
Set r_wrist_yaw value.
void set_l_hip_roll(const float new_l_hip_roll)
Set l_hip_roll value.
void set_minimum(const float new_minimum)
Set minimum value.
void set_r_shoulder_pitch(const float new_r_shoulder_pitch)
Set r_shoulder_pitch value.
void set_head_pitch(const float new_head_pitch)
Set head_pitch value.
void set_l_knee_pitch(const float new_l_knee_pitch)
Set l_knee_pitch value.
void set_l_hip_yaw_pitch(const float new_l_hip_yaw_pitch)
Set l_hip_yaw_pitch value.
void set_r_hand(const float new_r_hand)
Set r_hand value.
void set_l_shoulder_roll(const float new_l_shoulder_roll)
Set l_shoulder_roll value.
void set_l_ankle_pitch(const float new_l_ankle_pitch)
Set l_ankle_pitch value.
void set_r_ankle_roll(const float new_r_ankle_roll)
Set r_ankle_roll value.
void set_l_elbow_roll(const float new_l_elbow_roll)
Set l_elbow_roll value.
void set_r_elbow_yaw(const float new_r_elbow_yaw)
Set r_elbow_yaw value.
void set_l_wrist_yaw(const float new_l_wrist_yaw)
Set l_wrist_yaw value.
void set_l_hip_pitch(const float new_l_hip_pitch)
Set l_hip_pitch value.
void set_l_hand(const float new_l_hand)
Set l_hand value.
void set_r_shoulder_roll(const float new_r_shoulder_roll)
Set r_shoulder_roll value.
void set_r_knee_pitch(const float new_r_knee_pitch)
Set r_knee_pitch value.
void set_r_elbow_roll(const float new_r_elbow_roll)
Set r_elbow_roll value.
void set_head_yaw(const float new_head_yaw)
Set head_yaw value.
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
Definition: naoqi.h:44
EmitUltrasonicWaveMessage Fawkes BlackBoard Interface Message.
StartUltrasonicMessage Fawkes BlackBoard Interface Message.
StopUltrasonicMessage Fawkes BlackBoard Interface Message.
NaoSensorInterface Fawkes BlackBoard Interface.
void set_r_total_weight(const float new_r_total_weight)
Set r_total_weight value.
void set_l_fsr_fr(const float new_l_fsr_fr)
Set l_fsr_fr value.
void set_gyro_ref(const float new_gyro_ref)
Set gyro_ref value.
void set_accel_z(const float new_accel_z)
Set accel_z value.
void set_angle_x(const float new_angle_x)
Set angle_x value.
void set_accel_x(const float new_accel_x)
Set accel_x value.
void set_head_touch_rear(const uint8_t new_head_touch_rear)
Set head_touch_rear value.
void set_l_fsr_rr(const float new_l_fsr_rr)
Set l_fsr_rr value.
void set_head_touch_front(const uint8_t new_head_touch_front)
Set head_touch_front value.
UltrasonicDirection
This determines the chosen sender/receiver.
void set_ultrasonic_direction(const UltrasonicDirection new_ultrasonic_direction)
Set ultrasonic_direction value.
void set_battery_charge(const float new_battery_charge)
Set battery_charge value.
void set_accel_y(const float new_accel_y)
Set accel_y value.
void set_r_foot_bumper_l(const uint8_t new_r_foot_bumper_l)
Set r_foot_bumper_l value.
void set_r_cop_x(const float new_r_cop_x)
Set r_cop_x value.
void set_r_fsr_fl(const float new_r_fsr_fl)
Set r_fsr_fl value.
void set_r_fsr_fr(const float new_r_fsr_fr)
Set r_fsr_fr value.
void set_gyro_x(const float new_gyro_x)
Set gyro_x value.
void set_head_touch_middle(const uint8_t new_head_touch_middle)
Set head_touch_middle value.
void set_gyro_y(const float new_gyro_y)
Set gyro_y value.
void set_l_total_weight(const float new_l_total_weight)
Set l_total_weight value.
void set_r_foot_bumper_r(const uint8_t new_r_foot_bumper_r)
Set r_foot_bumper_r value.
void set_l_foot_bumper_r(const uint8_t new_l_foot_bumper_r)
Set l_foot_bumper_r value.
void set_l_fsr_fl(const float new_l_fsr_fl)
Set l_fsr_fl value.
void set_ultrasonic_distance_left(unsigned int index, const float new_ultrasonic_distance_left)
Set ultrasonic_distance_left value at given index.
void set_r_cop_y(const float new_r_cop_y)
Set r_cop_y value.
void set_r_fsr_rr(const float new_r_fsr_rr)
Set r_fsr_rr value.
void set_l_cop_x(const float new_l_cop_x)
Set l_cop_x value.
void set_l_cop_y(const float new_l_cop_y)
Set l_cop_y value.
void set_r_fsr_rl(const float new_r_fsr_rl)
Set r_fsr_rl value.
void set_chest_button(const uint8_t new_chest_button)
Set chest_button value.
UltrasonicDirection ultrasonic_direction() const
Get ultrasonic_direction value.
void set_ultrasonic_distance_right(unsigned int index, const float new_ultrasonic_distance_right)
Set ultrasonic_distance_right value at given index.
void set_angle_y(const float new_angle_y)
Set angle_y value.
void set_l_foot_bumper_l(const uint8_t new_l_foot_bumper_l)
Set l_foot_bumper_l value.
void set_l_fsr_rl(const float new_l_fsr_rl)
Set l_fsr_rl value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:499
void join()
Join the thread.
Definition: thread.cpp:597
void wakeup()
Wake up thread.
Definition: thread.cpp:995
void cancel()
Cancel a thread.
Definition: thread.cpp:646
Fawkes library namespace.