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
38using namespace fawkes;
39
40enum 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
137enum 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 */
187static inline float
188clip_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{
204public:
205 /** Constructor.
206 * @param parent parent NaoQiDCMThread to call.
207 */
209 : Thread("NaoQiDCMThread::HighFreqThread", Thread::OPMODE_WAITFORWAKEUP), parent_(parent)
210 {
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
229private:
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
262void
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_ =
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
536void
537NaoQiDCMThread::dcm_callback()
538{
539 highfreq_thread_->wakeup();
540}
541
542void
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
568void
569NaoQiDCMThread::read_values()
570{
571 values_.lock();
572 dcm_time_ = dcm_->getTime(0);
573 memfa_->GetValues(values_);
574 values_.unlock();
575}
576
577void
579{
580 update_interfaces(joint_pos_if_, joint_stiffness_if_, sensor_if_);
581}
582
583void
584NaoQiDCMThread::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
760void
761NaoQiDCMThread::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
922void
923NaoQiDCMThread::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
952void
953NaoQiDCMThread::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
977std::vector<std::string>
978NaoQiDCMThread::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
1051int
1052NaoQiDCMThread::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
unsigned int num_readers() const
Get the number of readers.
Definition: interface.cpp:876
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
@ OPMODE_WAITFORWAKEUP
operate in wait-for-wakeup mode
Definition: thread.h:58
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:729
Fawkes library namespace.