PageRenderTime 49ms CodeModel.GetById 2ms app.highlight 42ms RepoModel.GetById 1ms app.codeStats 0ms

/src/man/sensors/SensorsModule.cpp

https://github.com/wdawson/nbites
C++ | 510 lines | 410 code | 60 blank | 40 comment | 11 complexity | 2b544e27d13828e4ae1faeb4a7bf6595 MD5 | raw file
  1#include "SensorsModule.h"
  2#include "Common.h"
  3#include <stdio.h>
  4
  5namespace man {
  6namespace sensors {
  7
  8SensorsModule::SensorsModule(boost::shared_ptr<AL::ALBroker> broker)
  9    : portals::Module(),
 10      jointsOutput_(base()),
 11      currentsOutput_(base()),
 12      temperatureOutput_(base()),
 13      chestboardButtonOutput_(base()),
 14      footbumperOutput_(base()),
 15      inertialsOutput_(base()),
 16      sonarsOutput_(base()),
 17      fsrOutput_(base()),
 18      batteryOutput_(base()),
 19      stiffStatusOutput_(base()),
 20      broker_(broker),
 21      fastMemoryAccess_(new AL::ALMemoryFastAccess()),
 22      sensorValues_(NUM_SENSOR_VALUES),
 23      sensorKeys_(NUM_SENSOR_VALUES)
 24{
 25    // Initialize the Aldebaran fast access memory interface
 26    // to quickly read sensor values from memory.
 27    initializeSensorFastAccess();
 28    initializeSonarValues();
 29}
 30
 31SensorsModule::~SensorsModule()
 32{
 33}
 34
 35void SensorsModule::initializeSensorFastAccess()
 36{
 37    int i = 0;
 38
 39    // Joint Angles
 40    for(; i < END_JOINTS; ++i)
 41    {
 42        sensorKeys_[i] = std::string("Device/SubDeviceList/") +
 43            SensorNames[i] + std::string("/Position/Sensor/Value");
 44    }
 45    i++;
 46
 47    // Joint Currents
 48    for(; i < END_CURRENTS; ++i)
 49    {
 50        // Subtract 27 from index in SensorsNames[] to get correct value.
 51        sensorKeys_[i] = std::string("Device/SubDeviceList/") +
 52            SensorNames[i-27] + std::string("/ElectricCurrent/Sensor/Value");
 53    }
 54    i++;
 55
 56    // Temperatures
 57    for(; i < END_TEMPERATURES; ++i)
 58    {
 59        // Subtract 2*27 from index in SensorsNames[] to get correct value.
 60        sensorKeys_[i] = std::string("Device/SubDeviceList/") +
 61            SensorNames[i-2*27] + std::string("/Temperature/Sensor/Value");
 62    }
 63    i++;
 64
 65    // FSR (Left foot)
 66    for(; i < END_FSRS_LEFT; ++i)
 67    {
 68        sensorKeys_[i] = std::string("Device/SubDeviceList/LFoot/FSR/") +
 69            SensorNames[i] + std::string("/Sensor/Value");
 70    }
 71    i++;
 72
 73    // FSR (Right foot)
 74    for(; i < END_FSRS_RIGHT; ++i)
 75    {
 76        sensorKeys_[i] = std::string("Device/SubDeviceList/RFoot/FSR/") +
 77            SensorNames[i] + std::string("/Sensor/Value");
 78    }
 79    i++;
 80
 81    // Inertial Sensors
 82    for(; i < END_INTERTIALS; ++i)
 83    {
 84        sensorKeys_[i] = std::string("Device/SubDeviceList/InertialSensor/") +
 85            SensorNames[i] + std::string("/Sensor/Value");
 86    }
 87    i++;
 88
 89    // There are 2 battery values.
 90    sensorKeys_[i] = std::string("Device/SubDeviceList/Battery/Charge/Sensor/Value");
 91    i++;
 92    /* IMPORTANT for some reason, battery charge cannot be read correctly unless
 93     * battery current is read also, who knows why, bad aldebaran code?
 94     * NOT ACTUALLY OUTPORTALED OR USED AT ALL, current is needed for bug fix */
 95    sensorKeys_[i] = std::string("Device/SubDeviceList/Battery/Current/Sensor/Value");
 96    i++;
 97    // There are 2 important sonars.
 98    sensorKeys_[i] = std::string("Device/SubDeviceList/US/Left/Sensor/Value");
 99    i++;
100    sensorKeys_[i] = std::string("Device/SubDeviceList/US/Right/Sensor/Value");
101    i++;
102    // There are 4 foot bumpers.
103    sensorKeys_[i] = std::string("Device/SubDeviceList/LFoot/Bumper/Left/Sensor/Value");
104    i++;
105    sensorKeys_[i] = std::string("Device/SubDeviceList/LFoot/Bumper/Right/Sensor/Value");
106    i++;
107    sensorKeys_[i] = std::string("Device/SubDeviceList/RFoot/Bumper/Left/Sensor/Value");
108    i++;
109    sensorKeys_[i] = std::string("Device/SubDeviceList/RFoot/Bumper/Right/Sensor/Value");
110    i++;
111    // There is a single chest button.
112    sensorKeys_[i] = std::string("Device/SubDeviceList/ChestBoard/Button/Sensor/Value");
113    i++;
114    // All joints have stiffnesses associated with them.
115    sensorKeys_[i] = std::string("Device/SubDeviceList/HeadPitch/Hardness/Actuator/Value");
116    i++;
117    sensorKeys_[i] = std::string("Device/SubDeviceList/HeadYaw/Hardness/Actuator/Value");
118    i++;
119    sensorKeys_[i] = std::string("Device/SubDeviceList/LAnklePitch/Hardness/Actuator/Value");
120    i++;
121    sensorKeys_[i] = std::string("Device/SubDeviceList/LAnkleRoll/Hardness/Actuator/Value");
122    i++;
123    sensorKeys_[i] = std::string("Device/SubDeviceList/LElbowRoll/Hardness/Actuator/Value");
124    i++;
125    sensorKeys_[i] = std::string("Device/SubDeviceList/LElbowYaw/Hardness/Actuator/Value");
126    i++;
127    sensorKeys_[i] = std::string("Device/SubDeviceList/LHand/Hardness/Actuator/Value");
128    i++;
129    sensorKeys_[i] = std::string("Device/SubDeviceList/LHipPitch/Hardness/Actuator/Value");
130    i++;
131    sensorKeys_[i] = std::string("Device/SubDeviceList/LHipRoll/Hardness/Actuator/Value");
132    i++;
133    sensorKeys_[i] = std::string("Device/SubDeviceList/LHipYawPitch/Hardness/Actuator/Value");
134    i++;
135    sensorKeys_[i] = std::string("Device/SubDeviceList/LKneePitch/Hardness/Actuator/Value");
136    i++;
137    sensorKeys_[i] = std::string("Device/SubDeviceList/LShoulderPitch/Hardness/Actuator/Value");
138    i++;
139    sensorKeys_[i] = std::string("Device/SubDeviceList/LShoulderRoll/Hardness/Actuator/Value");
140    i++;
141    sensorKeys_[i] = std::string("Device/SubDeviceList/LWristYaw/Hardness/Actuator/Value");
142    i++;
143    sensorKeys_[i] = std::string("Device/SubDeviceList/RAnklePitch/Hardness/Actuator/Value");
144    i++;
145    sensorKeys_[i] = std::string("Device/SubDeviceList/RAnkleRoll/Hardness/Actuator/Value");
146    i++;
147    sensorKeys_[i] = std::string("Device/SubDeviceList/RElbowRoll/Hardness/Actuator/Value");
148    i++;
149    sensorKeys_[i] = std::string("Device/SubDeviceList/RElbowYaw/Hardness/Actuator/Value");
150    i++;
151    sensorKeys_[i] = std::string("Device/SubDeviceList/RHand/Hardness/Actuator/Value");
152    i++;
153    sensorKeys_[i] = std::string("Device/SubDeviceList/RHipPitch/Hardness/Actuator/Value");
154    i++;
155    sensorKeys_[i] = std::string("Device/SubDeviceList/RHipYawPitch/Hardness/Actuator/Value");
156    i++;
157    sensorKeys_[i] = std::string("Device/SubDeviceList/RHipRoll/Hardness/Actuator/Value");
158    i++;
159    sensorKeys_[i] = std::string("Device/SubDeviceList/RKneePitch/Hardness/Actuator/Value");
160    i++;
161    sensorKeys_[i] = std::string("Device/SubDeviceList/RShoulderPitch/Hardness/Actuator/Value");
162    i++;
163    sensorKeys_[i]= std::string("Device/SubDeviceList/RShoulderRoll/Hardness/Actuator/Value");
164    i++;
165    sensorKeys_[i] = std::string("Device/SubDeviceList/RWristYaw/Hardness/Actuator/Value");
166
167    fastMemoryAccess_->ConnectToVariables(broker_, sensorKeys_);
168
169    std::cout << "SensorsModule : Sensor keys initialized." << std::endl;
170    // for(std::vector<std::string>::iterator iter = sensorKeys_.begin();
171    //  iter != sensorKeys_.end();
172    //  ++iter)
173    // {
174    //  std::cout << *iter << std::endl;
175    // }
176}
177
178void SensorsModule::initializeSonarValues()
179{
180    // Get a proxy to the DCM.
181    boost::shared_ptr<AL::DCMProxy> dcmProxy = broker_->getDcmProxy();
182    if(dcmProxy != 0)
183    {
184        try
185        {
186            // For DCM::set() see:
187            // http://www.aldebaran-robotics.com/documentation/naoqi/sensors/dcm-api.html#DCMProxy::set__AL::ALValueCR
188            AL::ALValue dcmSonarCommand;
189
190            dcmSonarCommand.arraySetSize(3);
191            dcmSonarCommand[0] = std::string("Device/SubDeviceList/US/Actuator/Value"); // Device name.
192            dcmSonarCommand[1] = std::string("ClearAll"); // Delete all timed commands before adding this one.
193
194            dcmSonarCommand[2].arraySetSize(1); // A list of (1) timed-commands.
195            dcmSonarCommand[2][0].arraySetSize(2);
196            dcmSonarCommand[2][0][0] = 68.0; // The command itself.
197            dcmSonarCommand[2][0][1] = dcmProxy->getTime(0); // The DCM time for the command to be applied.
198
199            // Send the timed command to the sonars.
200            dcmProxy->set(dcmSonarCommand);
201        }
202        catch(AL::ALError& e)
203        {
204            std::cout << "SensorsModule : Failed to initialize sonars, "
205                      << e.toString() << std::endl;
206        }
207    }
208}
209
210void SensorsModule::updateSensorValues()
211{
212    //std::cout << "SensorsModule : Retrieving sensor values from NAOqi." << std::endl;
213    // Update stored sensor values.
214    fastMemoryAccess_->GetValues(sensorValues_);
215
216    updateJointsMessage();
217    updateCurrentsMessage();
218    updateTemperatureMessage();
219    updateChestboardButtonMessage();
220    updateFootbumperMessage();
221    updateInertialsMessage();
222    updateSonarsMessage();
223    updateFSRMessage();
224    updateBatteryMessage();
225    updateStiffMessage();
226
227    // std::cout << "SensorsModule : Sensor values " << std::endl;
228    // for(int i = 0; i < NUM_SENSOR_VALUES; ++i)
229    // {
230    //  std::cout << SensorNames[i] << " = " << sensorValues_[i] << std::endl;
231    // }
232}
233
234void SensorsModule::updateJointsMessage()
235{
236    portals::Message<messages::JointAngles> jointsMessage(0);
237
238    jointsMessage.get()->set_head_yaw(sensorValues_[HeadYaw]);
239    jointsMessage.get()->set_head_pitch(sensorValues_[HeadPitch]);
240    jointsMessage.get()->set_l_shoulder_pitch(sensorValues_[LShoulderPitch]);
241    jointsMessage.get()->set_l_shoulder_roll(sensorValues_[LShoulderRoll]);
242    jointsMessage.get()->set_l_elbow_yaw(sensorValues_[LElbowYaw]);
243    jointsMessage.get()->set_l_elbow_roll(sensorValues_[LElbowRoll]);
244    jointsMessage.get()->set_l_wrist_yaw(sensorValues_[LWristYaw]);
245    jointsMessage.get()->set_l_hand(sensorValues_[LHand]);
246    jointsMessage.get()->set_r_shoulder_pitch(sensorValues_[RShoulderPitch]);
247    jointsMessage.get()->set_r_shoulder_roll(sensorValues_[RShoulderRoll]);
248    jointsMessage.get()->set_r_elbow_yaw(sensorValues_[RElbowYaw]);
249    jointsMessage.get()->set_r_elbow_roll(sensorValues_[RElbowRoll]);
250    jointsMessage.get()->set_r_wrist_yaw(sensorValues_[RWristYaw]);
251    jointsMessage.get()->set_r_hand(sensorValues_[RHand]);
252    jointsMessage.get()->set_l_hip_yaw_pitch(sensorValues_[LHipYawPitch]);
253    jointsMessage.get()->set_r_hip_yaw_pitch(sensorValues_[RHipYawPitch]);
254    jointsMessage.get()->set_l_hip_roll(sensorValues_[LHipRoll]);
255    jointsMessage.get()->set_l_hip_pitch(sensorValues_[LHipPitch]);
256    jointsMessage.get()->set_l_knee_pitch(sensorValues_[LKneePitch]);
257    jointsMessage.get()->set_l_ankle_pitch(sensorValues_[LAnklePitch]);
258    jointsMessage.get()->set_l_ankle_roll(sensorValues_[LAnkleRoll]);
259    jointsMessage.get()->set_r_hip_roll(sensorValues_[RHipRoll]);
260    jointsMessage.get()->set_r_hip_pitch(sensorValues_[RHipPitch]);
261    jointsMessage.get()->set_r_knee_pitch(sensorValues_[RKneePitch]);
262    jointsMessage.get()->set_r_ankle_pitch(sensorValues_[RAnklePitch]);
263    jointsMessage.get()->set_r_ankle_roll(sensorValues_[RAnkleRoll]);
264
265    jointsOutput_.setMessage(jointsMessage);
266}
267
268void SensorsModule::updateCurrentsMessage()
269{
270    portals::Message<messages::JointAngles> jointsMessage(0);
271
272    jointsMessage.get()->set_head_yaw(sensorValues_[HeadYawCurrent]);
273    jointsMessage.get()->set_head_pitch(sensorValues_[HeadPitchCurrent]);
274    jointsMessage.get()->set_l_shoulder_pitch(sensorValues_[LShoulderPitchCurrent]);
275    jointsMessage.get()->set_l_shoulder_roll(sensorValues_[LShoulderRollCurrent]);
276    jointsMessage.get()->set_l_elbow_yaw(sensorValues_[LElbowYawCurrent]);
277    jointsMessage.get()->set_l_elbow_roll(sensorValues_[LElbowRollCurrent]);
278    jointsMessage.get()->set_l_wrist_yaw(sensorValues_[LWristYawCurrent]);
279    jointsMessage.get()->set_l_hand(sensorValues_[LHandCurrent]);
280    jointsMessage.get()->set_r_shoulder_pitch(sensorValues_[RShoulderPitchCurrent]);
281    jointsMessage.get()->set_r_shoulder_roll(sensorValues_[RShoulderRollCurrent]);
282    jointsMessage.get()->set_r_elbow_yaw(sensorValues_[RElbowYawCurrent]);
283    jointsMessage.get()->set_r_elbow_roll(sensorValues_[RElbowRollCurrent]);
284    jointsMessage.get()->set_r_wrist_yaw(sensorValues_[RWristYawCurrent]);
285    jointsMessage.get()->set_r_hand(sensorValues_[RHandCurrent]);
286    jointsMessage.get()->set_l_hip_yaw_pitch(sensorValues_[LHipYawPitchCurrent]);
287    jointsMessage.get()->set_r_hip_yaw_pitch(sensorValues_[RHipYawPitchCurrent]);
288    jointsMessage.get()->set_l_hip_roll(sensorValues_[LHipRollCurrent]);
289    jointsMessage.get()->set_l_hip_pitch(sensorValues_[LHipPitchCurrent]);
290    jointsMessage.get()->set_l_knee_pitch(sensorValues_[LKneePitchCurrent]);
291    jointsMessage.get()->set_l_ankle_pitch(sensorValues_[LAnklePitchCurrent]);
292    jointsMessage.get()->set_l_ankle_roll(sensorValues_[LAnkleRollCurrent]);
293    jointsMessage.get()->set_r_hip_roll(sensorValues_[RHipRollCurrent]);
294    jointsMessage.get()->set_r_hip_pitch(sensorValues_[RHipPitchCurrent]);
295    jointsMessage.get()->set_r_knee_pitch(sensorValues_[RKneePitchCurrent]);
296    jointsMessage.get()->set_r_ankle_pitch(sensorValues_[RAnklePitchCurrent]);
297    jointsMessage.get()->set_r_ankle_roll(sensorValues_[RAnkleRollCurrent]);
298
299    currentsOutput_.setMessage(jointsMessage);
300}
301
302void SensorsModule::updateTemperatureMessage()
303{
304    portals::Message<messages::JointAngles> temperaturesMessage(0);
305    temperaturesMessage.get()->set_head_yaw(sensorValues_[HeadYawTemp]);
306    temperaturesMessage.get()->set_head_pitch(sensorValues_[HeadPitchTemp]);
307    temperaturesMessage.get()->set_l_shoulder_pitch(sensorValues_[LShoulderPitchTemp]);
308    temperaturesMessage.get()->set_l_shoulder_roll(sensorValues_[LShoulderRollTemp]);
309    temperaturesMessage.get()->set_l_elbow_yaw(sensorValues_[LElbowYawTemp]);
310    temperaturesMessage.get()->set_l_elbow_roll(sensorValues_[LElbowRollTemp]);
311    temperaturesMessage.get()->set_l_wrist_yaw(sensorValues_[LWristYawTemp]);
312    temperaturesMessage.get()->set_l_hand(sensorValues_[LHandTemp]);
313    temperaturesMessage.get()->set_r_shoulder_pitch(sensorValues_[RShoulderPitchTemp]);
314    temperaturesMessage.get()->set_r_shoulder_roll(sensorValues_[RShoulderRollTemp]);
315    temperaturesMessage.get()->set_r_elbow_yaw(sensorValues_[RElbowYawTemp]);
316    temperaturesMessage.get()->set_r_elbow_roll(sensorValues_[RElbowRollTemp]);
317    temperaturesMessage.get()->set_r_wrist_yaw(sensorValues_[RWristYawTemp]);
318    temperaturesMessage.get()->set_r_hand(sensorValues_[RHandTemp]);
319    temperaturesMessage.get()->set_l_hip_yaw_pitch(sensorValues_[LHipYawPitchTemp]);
320    temperaturesMessage.get()->set_r_hip_yaw_pitch(sensorValues_[RHipYawPitchTemp]);
321    temperaturesMessage.get()->set_l_hip_roll(sensorValues_[LHipRollTemp]);
322    temperaturesMessage.get()->set_l_hip_pitch(sensorValues_[LHipPitchTemp]);
323    temperaturesMessage.get()->set_l_knee_pitch(sensorValues_[LKneePitchTemp]);
324    temperaturesMessage.get()->set_l_ankle_pitch(sensorValues_[LAnklePitchTemp]);
325    temperaturesMessage.get()->set_l_ankle_roll(sensorValues_[LAnkleRollTemp]);
326    temperaturesMessage.get()->set_r_hip_roll(sensorValues_[RHipRollTemp]);
327    temperaturesMessage.get()->set_r_hip_pitch(sensorValues_[RHipPitchTemp]);
328    temperaturesMessage.get()->set_r_knee_pitch(sensorValues_[RKneePitchTemp]);
329    temperaturesMessage.get()->set_r_ankle_pitch(sensorValues_[RAnklePitchTemp]);
330    temperaturesMessage.get()->set_r_ankle_roll(sensorValues_[RAnkleRollTemp]);
331
332    temperatureOutput_.setMessage(temperaturesMessage);
333}
334
335void SensorsModule::updateChestboardButtonMessage()
336{
337    portals::Message<messages::ButtonState> chestboardMessage(0);
338
339    chestboardMessage.get()->set_pressed(
340        sensorValues_[ChestboardButton] > 0.5f ? true : false
341        );
342
343    chestboardButtonOutput_.setMessage(chestboardMessage);
344}
345
346void SensorsModule::updateFootbumperMessage()
347{
348    portals::Message<messages::FootBumperState> footbumperMessage(0);
349
350    footbumperMessage.get()->mutable_l_foot_bumper_left() ->set_pressed(
351        sensorValues_[LFootBumperLeft]  > 0.5f ? true : false
352        );
353    footbumperMessage.get()->mutable_l_foot_bumper_right()->set_pressed(
354        sensorValues_[LFootBumperRight] > 0.5f ? true : false
355        );
356
357    footbumperMessage.get()->mutable_r_foot_bumper_left() ->set_pressed(
358        sensorValues_[RFootBumperLeft]  > 0.5f ? true : false
359        );
360    footbumperMessage.get()->mutable_r_foot_bumper_right()->set_pressed(
361        sensorValues_[RFootBumperRight] > 0.5f ? true : false
362        );
363
364    footbumperOutput_.setMessage(footbumperMessage);
365}
366
367void SensorsModule::updateInertialsMessage()
368{
369    portals::Message<messages::InertialState> inertialsMessage(0);
370
371    inertialsMessage.get()->set_acc_x(sensorValues_[AccX]);
372    inertialsMessage.get()->set_acc_y(sensorValues_[AccY]);
373    inertialsMessage.get()->set_acc_z(sensorValues_[AccZ]);
374
375    inertialsMessage.get()->set_gyr_x(sensorValues_[GyrX]);
376    inertialsMessage.get()->set_gyr_y(sensorValues_[GyrY]);
377
378    inertialsMessage.get()->set_angle_x(sensorValues_[AngleX]);
379    inertialsMessage.get()->set_angle_y(sensorValues_[AngleY]);
380
381    inertialsOutput_.setMessage(inertialsMessage);
382}
383
384void SensorsModule::updateSonarsMessage()
385{
386    portals::Message<messages::SonarState> sonarsMessage(0);
387
388    sonarsMessage.get()->set_us_left(sensorValues_[USLeft]);
389    sonarsMessage.get()->set_us_right(sensorValues_[USRight]);
390
391    sonarsOutput_.setMessage(sonarsMessage);
392}
393
394void SensorsModule::updateFSRMessage()
395{
396    portals::Message<messages::FSR> fsrMessage(0);
397
398    // Left foot FSR values.
399    fsrMessage.get()->set_lfl(sensorValues_[LFsrFL]);
400    fsrMessage.get()->set_lfr(sensorValues_[LFsrFR]);
401    fsrMessage.get()->set_lrl(sensorValues_[LFsrRL]);
402    fsrMessage.get()->set_lrr(sensorValues_[LFsrRR]);
403
404    // Right foot FSR values.
405    fsrMessage.get()->set_rfl(sensorValues_[RFsrFL]);
406    fsrMessage.get()->set_rfr(sensorValues_[RFsrFR]);
407    fsrMessage.get()->set_rrl(sensorValues_[RFsrRL]);
408    fsrMessage.get()->set_rrr(sensorValues_[RFsrRR]);
409
410    fsrOutput_.setMessage(fsrMessage);
411}
412
413void SensorsModule::updateBatteryMessage()
414{
415    portals::Message<messages::BatteryState> batteryMessage(0);
416
417    batteryMessage.get()->set_charge(sensorValues_[BatteryCharge]);
418
419    batteryOutput_.setMessage(batteryMessage);
420}
421
422void SensorsModule::updateStiffMessage()
423{
424    portals::Message<messages::StiffStatus> stiffMessage(0);
425
426    if (sensorValues_[HeadPitchStiff] > 0 ||
427        sensorValues_[HeadYawStiff] > 0 ||
428        sensorValues_[LAnklePitchStiff] > 0 ||
429        sensorValues_[LAnkleRollStiff] > 0 ||
430        sensorValues_[LElbowRollStiff] > 0 ||
431        sensorValues_[LElbowYawStiff] > 0 ||
432        sensorValues_[LHandStiff] > 0 ||
433        sensorValues_[LHipPitchStiff] > 0 ||
434        sensorValues_[LHipRollStiff] > 0 ||
435        sensorValues_[LHipYawPitchStiff] > 0 ||
436        sensorValues_[LKneePitchStiff] > 0 ||
437        sensorValues_[LShoulderPitchStiff] > 0 ||
438        sensorValues_[LShoulderRollStiff] > 0 ||
439        sensorValues_[LWristYawStiff] > 0 ||
440        sensorValues_[RAnklePitchStiff] > 0 ||
441        sensorValues_[RAnkleRollStiff] > 0 ||
442        sensorValues_[RElbowRollStiff] > 0 ||
443        sensorValues_[RElbowYawStiff] > 0 ||
444        sensorValues_[RHandStiff] > 0 ||
445        sensorValues_[RHipPitchStiff] > 0 ||
446        sensorValues_[RHipYawPitchStiff] > 0 ||
447        sensorValues_[RHipRollStiff] > 0 ||
448        sensorValues_[RKneePitchStiff] > 0 ||
449        sensorValues_[RShoulderPitchStiff] > 0 ||
450        sensorValues_[RShoulderRollStiff] > 0 ||
451        sensorValues_[RWristYawStiff] > 0)
452    {
453        stiffMessage.get()->set_on(1);
454    }
455    else
456    {
457        stiffMessage.get()->set_on(0);
458    }
459
460    stiffStatusOutput_.setMessage(stiffMessage);
461}
462
463// Helper method so that we can print out a Sweet Moves joint angle
464// tuple directly when we want to (ie 5 button presses)
465std::string makeSweetMoveTuple(const messages::JointAngles* angles)
466{
467    char output[240];
468
469    sprintf(output, "((%3.1f, %3.1f, %3.1f, %3.1f),\n(%3.1f, %3.1f, %3.1f, %3.1f, %3.1f, %3.1f),\n(%3.1f, %3.1f, %3.1f, %3.1f, %3.1f, %3.1f),\n(%3.1f, %3.1f, %3.1f, %3.1f))\n",
470            TO_DEG*angles->l_shoulder_pitch(),
471            TO_DEG*angles->l_shoulder_roll(),
472            TO_DEG*angles->l_elbow_yaw(),
473            TO_DEG*angles->l_elbow_roll(),
474            TO_DEG*angles->l_hip_yaw_pitch(),
475            TO_DEG*angles->l_hip_roll(),
476            TO_DEG*angles->l_hip_pitch(),
477            TO_DEG*angles->l_knee_pitch(),
478            TO_DEG*angles->l_ankle_pitch(),
479            TO_DEG*angles->l_ankle_roll(),
480            TO_DEG*angles->r_hip_yaw_pitch(),
481            TO_DEG*angles->r_hip_roll(),
482            TO_DEG*angles->r_hip_pitch(),
483            TO_DEG*angles->r_knee_pitch(),
484            TO_DEG*angles->r_ankle_pitch(),
485            TO_DEG*angles->r_ankle_roll(),
486            TO_DEG*angles->r_shoulder_pitch(),
487            TO_DEG*angles->r_shoulder_roll(),
488            TO_DEG*angles->r_elbow_yaw(),
489            TO_DEG*angles->r_elbow_roll());
490
491    return std::string(output);
492}
493
494void SensorsModule::run_()
495{
496    printInput.latch();
497
498    // Simply update all sensor readings from ALMemory.
499    updateSensorValues();
500
501    if(printInput.message().toggle() != lastPrint)
502    {
503        std::cout << makeSweetMoveTuple(jointsOutput_.getMessage(true).get())
504                  << std::endl;
505        lastPrint = !lastPrint;
506    }
507}
508
509} // namespace sensors
510} // namespace man