PageRenderTime 58ms CodeModel.GetById 29ms 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
Possible License(s): GPL-3.0, LGPL-3.0 
    

  1. #include "SensorsModule.h"
    2. 

  2. #include "Common.h"
    3. 

  3. #include <stdio.h>
    4. 

  4. 

  5. namespace man {
    6. 

  6. namespace sensors {
    7. 

  7. 

  8. SensorsModule::SensorsModule(boost::shared_ptr<AL::ALBroker> broker)
    9. 

  9.     : portals::Module(),
    10. 

  10.       jointsOutput_(base()),
    11. 

  11.       currentsOutput_(base()),
    12. 

  12.       temperatureOutput_(base()),
    13. 

  13.       chestboardButtonOutput_(base()),
    14. 

  14.       footbumperOutput_(base()),
    15. 

  15.       inertialsOutput_(base()),
    16. 

  16.       sonarsOutput_(base()),
    17. 

  17.       fsrOutput_(base()),
    18. 

  18.       batteryOutput_(base()),
    19. 

  19.       stiffStatusOutput_(base()),
    20. 

  20.       broker_(broker),
    21. 

  21.       fastMemoryAccess_(new AL::ALMemoryFastAccess()),
    22. 

  22.       sensorValues_(NUM_SENSOR_VALUES),
    23. 

  23.       sensorKeys_(NUM_SENSOR_VALUES)
    24. 

  24. {
    25. 

  25.     // Initialize the Aldebaran fast access memory interface
    26. 

  26.     // to quickly read sensor values from memory.
    27. 

  27.     initializeSensorFastAccess();
    28. 

  28.     initializeSonarValues();
    29. 

  29. }
    30. 

  30. 

  31. SensorsModule::~SensorsModule()
    32. 

  32. {
    33. 

  33. }
    34. 

  34. 

  35. void SensorsModule::initializeSensorFastAccess()
    36. 

  36. {
    37. 

  37.     int i = 0;
    38. 

  38. 

  39.     // Joint Angles
    40. 

  40.     for(; i < END_JOINTS; ++i)
    41. 

  41.     {
    42. 

  42.         sensorKeys_[i] = std::string("Device/SubDeviceList/") +
    43. 

  43.             SensorNames[i] + std::string("/Position/Sensor/Value");
    44. 

  44.     }
    45. 

  45.     i++;
    46. 

  46. 

  47.     // Joint Currents
    48. 

  48.     for(; i < END_CURRENTS; ++i)
    49. 

  49.     {
    50. 

  50.         // Subtract 27 from index in SensorsNames[] to get correct value.
    51. 

  51.         sensorKeys_[i] = std::string("Device/SubDeviceList/") +
    52. 

  52.             SensorNames[i-27] + std::string("/ElectricCurrent/Sensor/Value");
    53. 

  53.     }
    54. 

  54.     i++;
    55. 

  55. 

  56.     // Temperatures
    57. 

  57.     for(; i < END_TEMPERATURES; ++i)
    58. 

  58.     {
    59. 

  59.         // Subtract 2*27 from index in SensorsNames[] to get correct value.
    60. 

  60.         sensorKeys_[i] = std::string("Device/SubDeviceList/") +
    61. 

  61.             SensorNames[i-2*27] + std::string("/Temperature/Sensor/Value");
    62. 

  62.     }
    63. 

  63.     i++;
    64. 

  64. 

  65.     // FSR (Left foot)
    66. 

  66.     for(; i < END_FSRS_LEFT; ++i)
    67. 

  67.     {
    68. 

  68.         sensorKeys_[i] = std::string("Device/SubDeviceList/LFoot/FSR/") +
    69. 

  69.             SensorNames[i] + std::string("/Sensor/Value");
    70. 

  70.     }
    71. 

  71.     i++;
    72. 

  72. 

  73.     // FSR (Right foot)
    74. 

  74.     for(; i < END_FSRS_RIGHT; ++i)
    75. 

  75.     {
    76. 

  76.         sensorKeys_[i] = std::string("Device/SubDeviceList/RFoot/FSR/") +
    77. 

  77.             SensorNames[i] + std::string("/Sensor/Value");
    78. 

  78.     }
    79. 

  79.     i++;
    80. 

  80. 

  81.     // Inertial Sensors
    82. 

  82.     for(; i < END_INTERTIALS; ++i)
    83. 

  83.     {
    84. 

  84.         sensorKeys_[i] = std::string("Device/SubDeviceList/InertialSensor/") +
    85. 

  85.             SensorNames[i] + std::string("/Sensor/Value");
    86. 

  86.     }
    87. 

  87.     i++;
    88. 

  88. 

  89.     // There are 2 battery values.
    90. 

  90.     sensorKeys_[i] = std::string("Device/SubDeviceList/Battery/Charge/Sensor/Value");
    91. 

  91.     i++;
    92. 

  92.     /* IMPORTANT for some reason, battery charge cannot be read correctly unless
    93. 

  93.      * battery current is read also, who knows why, bad aldebaran code?
    94. 

  94.      * NOT ACTUALLY OUTPORTALED OR USED AT ALL, current is needed for bug fix */
    95. 

  95.     sensorKeys_[i] = std::string("Device/SubDeviceList/Battery/Current/Sensor/Value");
    96. 

  96.     i++;
    97. 

  97.     // There are 2 important sonars.
    98. 

  98.     sensorKeys_[i] = std::string("Device/SubDeviceList/US/Left/Sensor/Value");
    99. 

  99.     i++;
    100. 

  100.     sensorKeys_[i] = std::string("Device/SubDeviceList/US/Right/Sensor/Value");
    101. 

  101.     i++;
    102. 

  102.     // There are 4 foot bumpers.
    103. 

  103.     sensorKeys_[i] = std::string("Device/SubDeviceList/LFoot/Bumper/Left/Sensor/Value");
    104. 

  104.     i++;
    105. 

  105.     sensorKeys_[i] = std::string("Device/SubDeviceList/LFoot/Bumper/Right/Sensor/Value");
    106. 

  106.     i++;
    107. 

  107.     sensorKeys_[i] = std::string("Device/SubDeviceList/RFoot/Bumper/Left/Sensor/Value");
    108. 

  108.     i++;
    109. 

  109.     sensorKeys_[i] = std::string("Device/SubDeviceList/RFoot/Bumper/Right/Sensor/Value");
    110. 

  110.     i++;
    111. 

  111.     // There is a single chest button.
    112. 

  112.     sensorKeys_[i] = std::string("Device/SubDeviceList/ChestBoard/Button/Sensor/Value");
    113. 

  113.     i++;
    114. 

  114.     // All joints have stiffnesses associated with them.
    115. 

  115.     sensorKeys_[i] = std::string("Device/SubDeviceList/HeadPitch/Hardness/Actuator/Value");
    116. 

  116.     i++;
    117. 

  117.     sensorKeys_[i] = std::string("Device/SubDeviceList/HeadYaw/Hardness/Actuator/Value");
    118. 

  118.     i++;
    119. 

  119.     sensorKeys_[i] = std::string("Device/SubDeviceList/LAnklePitch/Hardness/Actuator/Value");
    120. 

  120.     i++;
    121. 

  121.     sensorKeys_[i] = std::string("Device/SubDeviceList/LAnkleRoll/Hardness/Actuator/Value");
    122. 

  122.     i++;
    123. 

  123.     sensorKeys_[i] = std::string("Device/SubDeviceList/LElbowRoll/Hardness/Actuator/Value");
    124. 

  124.     i++;
    125. 

  125.     sensorKeys_[i] = std::string("Device/SubDeviceList/LElbowYaw/Hardness/Actuator/Value");
    126. 

  126.     i++;
    127. 

  127.     sensorKeys_[i] = std::string("Device/SubDeviceList/LHand/Hardness/Actuator/Value");
    128. 

  128.     i++;
    129. 

  129.     sensorKeys_[i] = std::string("Device/SubDeviceList/LHipPitch/Hardness/Actuator/Value");
    130. 

  130.     i++;
    131. 

  131.     sensorKeys_[i] = std::string("Device/SubDeviceList/LHipRoll/Hardness/Actuator/Value");
    132. 

  132.     i++;
    133. 

  133.     sensorKeys_[i] = std::string("Device/SubDeviceList/LHipYawPitch/Hardness/Actuator/Value");
    134. 

  134.     i++;
    135. 

  135.     sensorKeys_[i] = std::string("Device/SubDeviceList/LKneePitch/Hardness/Actuator/Value");
    136. 

  136.     i++;
    137. 

  137.     sensorKeys_[i] = std::string("Device/SubDeviceList/LShoulderPitch/Hardness/Actuator/Value");
    138. 

  138.     i++;
    139. 

  139.     sensorKeys_[i] = std::string("Device/SubDeviceList/LShoulderRoll/Hardness/Actuator/Value");
    140. 

  140.     i++;
    141. 

  141.     sensorKeys_[i] = std::string("Device/SubDeviceList/LWristYaw/Hardness/Actuator/Value");
    142. 

  142.     i++;
    143. 

  143.     sensorKeys_[i] = std::string("Device/SubDeviceList/RAnklePitch/Hardness/Actuator/Value");
    144. 

  144.     i++;
    145. 

  145.     sensorKeys_[i] = std::string("Device/SubDeviceList/RAnkleRoll/Hardness/Actuator/Value");
    146. 

  146.     i++;
    147. 

  147.     sensorKeys_[i] = std::string("Device/SubDeviceList/RElbowRoll/Hardness/Actuator/Value");
    148. 

  148.     i++;
    149. 

  149.     sensorKeys_[i] = std::string("Device/SubDeviceList/RElbowYaw/Hardness/Actuator/Value");
    150. 

  150.     i++;
    151. 

  151.     sensorKeys_[i] = std::string("Device/SubDeviceList/RHand/Hardness/Actuator/Value");
    152. 

  152.     i++;
    153. 

  153.     sensorKeys_[i] = std::string("Device/SubDeviceList/RHipPitch/Hardness/Actuator/Value");
    154. 

  154.     i++;
    155. 

  155.     sensorKeys_[i] = std::string("Device/SubDeviceList/RHipYawPitch/Hardness/Actuator/Value");
    156. 

  156.     i++;
    157. 

  157.     sensorKeys_[i] = std::string("Device/SubDeviceList/RHipRoll/Hardness/Actuator/Value");
    158. 

  158.     i++;
    159. 

  159.     sensorKeys_[i] = std::string("Device/SubDeviceList/RKneePitch/Hardness/Actuator/Value");
    160. 

  160.     i++;
    161. 

  161.     sensorKeys_[i] = std::string("Device/SubDeviceList/RShoulderPitch/Hardness/Actuator/Value");
    162. 

  162.     i++;
    163. 

  163.     sensorKeys_[i]= std::string("Device/SubDeviceList/RShoulderRoll/Hardness/Actuator/Value");
    164. 

  164.     i++;
    165. 

  165.     sensorKeys_[i] = std::string("Device/SubDeviceList/RWristYaw/Hardness/Actuator/Value");
    166. 

  166. 

  167.     fastMemoryAccess_->ConnectToVariables(broker_, sensorKeys_);
    168. 

  168. 

  169.     std::cout << "SensorsModule : Sensor keys initialized." << std::endl;
    170. 

  170.     // for(std::vector<std::string>::iterator iter = sensorKeys_.begin();
    171. 

  171.     //  iter != sensorKeys_.end();
    172. 

  172.     //  ++iter)
    173. 

  173.     // {
    174. 

  174.     //  std::cout << *iter << std::endl;
    175. 

  175.     // }
    176. 

  176. }
    177. 

  177. 

  178. void SensorsModule::initializeSonarValues()
    179. 

  179. {
    180. 

  180.     // Get a proxy to the DCM.
    181. 

  181.     boost::shared_ptr<AL::DCMProxy> dcmProxy = broker_->getDcmProxy();
    182. 

  182.     if(dcmProxy != 0)
    183. 

  183.     {
    184. 

  184.         try
    185. 

  185.         {
    186. 

  186.             // For DCM::set() see:
    187. 

  187.             // http://www.aldebaran-robotics.com/documentation/naoqi/sensors/dcm-api.html#DCMProxy::set__AL::ALValueCR
    188. 

  188.             AL::ALValue dcmSonarCommand;
    189. 

  189. 

  190.             dcmSonarCommand.arraySetSize(3);
    191. 

  191.             dcmSonarCommand[0] = std::string("Device/SubDeviceList/US/Actuator/Value"); // Device name.
    192. 

  192.             dcmSonarCommand[1] = std::string("ClearAll"); // Delete all timed commands before adding this one.
    193. 

  193. 

  194.             dcmSonarCommand[2].arraySetSize(1); // A list of (1) timed-commands.
    195. 

  195.             dcmSonarCommand[2][0].arraySetSize(2);
    196. 

  196.             dcmSonarCommand[2][0][0] = 68.0; // The command itself.
    197. 

  197.             dcmSonarCommand[2][0][1] = dcmProxy->getTime(0); // The DCM time for the command to be applied.
    198. 

  198. 

  199.             // Send the timed command to the sonars.
    200. 

  200.             dcmProxy->set(dcmSonarCommand);
    201. 

  201.         }
    202. 

  202.         catch(AL::ALError& e)
    203. 

  203.         {
    204. 

  204.             std::cout << "SensorsModule : Failed to initialize sonars, "
    205. 

  205.                       << e.toString() << std::endl;
    206. 

  206.         }
    207. 

  207.     }
    208. 

  208. }
    209. 

  209. 

  210. void SensorsModule::updateSensorValues()
    211. 

  211. {
    212. 

  212.     //std::cout << "SensorsModule : Retrieving sensor values from NAOqi." << std::endl;
    213. 

  213.     // Update stored sensor values.
    214. 

  214.     fastMemoryAccess_->GetValues(sensorValues_);
    215. 

  215. 

  216.     updateJointsMessage();
    217. 

  217.     updateCurrentsMessage();
    218. 

  218.     updateTemperatureMessage();
    219. 

  219.     updateChestboardButtonMessage();
    220. 

  220.     updateFootbumperMessage();
    221. 

  221.     updateInertialsMessage();
    222. 

  222.     updateSonarsMessage();
    223. 

  223.     updateFSRMessage();
    224. 

  224.     updateBatteryMessage();
    225. 

  225.     updateStiffMessage();
    226. 

  226. 

  227.     // std::cout << "SensorsModule : Sensor values " << std::endl;
    228. 

  228.     // for(int i = 0; i < NUM_SENSOR_VALUES; ++i)
    229. 

  229.     // {
    230. 

  230.     //  std::cout << SensorNames[i] << " = " << sensorValues_[i] << std::endl;
    231. 

  231.     // }
    232. 

  232. }
    233. 

  233. 

  234. void SensorsModule::updateJointsMessage()
    235. 

  235. {
    236. 

  236.     portals::Message<messages::JointAngles> jointsMessage(0);
    237. 

  237. 

  238.     jointsMessage.get()->set_head_yaw(sensorValues_[HeadYaw]);
    239. 

  239.     jointsMessage.get()->set_head_pitch(sensorValues_[HeadPitch]);
    240. 

  240.     jointsMessage.get()->set_l_shoulder_pitch(sensorValues_[LShoulderPitch]);
    241. 

  241.     jointsMessage.get()->set_l_shoulder_roll(sensorValues_[LShoulderRoll]);
    242. 

  242.     jointsMessage.get()->set_l_elbow_yaw(sensorValues_[LElbowYaw]);
    243. 

  243.     jointsMessage.get()->set_l_elbow_roll(sensorValues_[LElbowRoll]);
    244. 

  244.     jointsMessage.get()->set_l_wrist_yaw(sensorValues_[LWristYaw]);
    245. 

  245.     jointsMessage.get()->set_l_hand(sensorValues_[LHand]);
    246. 

  246.     jointsMessage.get()->set_r_shoulder_pitch(sensorValues_[RShoulderPitch]);
    247. 

  247.     jointsMessage.get()->set_r_shoulder_roll(sensorValues_[RShoulderRoll]);
    248. 

  248.     jointsMessage.get()->set_r_elbow_yaw(sensorValues_[RElbowYaw]);
    249. 

  249.     jointsMessage.get()->set_r_elbow_roll(sensorValues_[RElbowRoll]);
    250. 

  250.     jointsMessage.get()->set_r_wrist_yaw(sensorValues_[RWristYaw]);
    251. 

  251.     jointsMessage.get()->set_r_hand(sensorValues_[RHand]);
    252. 

  252.     jointsMessage.get()->set_l_hip_yaw_pitch(sensorValues_[LHipYawPitch]);
    253. 

  253.     jointsMessage.get()->set_r_hip_yaw_pitch(sensorValues_[RHipYawPitch]);
    254. 

  254.     jointsMessage.get()->set_l_hip_roll(sensorValues_[LHipRoll]);
    255. 

  255.     jointsMessage.get()->set_l_hip_pitch(sensorValues_[LHipPitch]);
    256. 

  256.     jointsMessage.get()->set_l_knee_pitch(sensorValues_[LKneePitch]);
    257. 

  257.     jointsMessage.get()->set_l_ankle_pitch(sensorValues_[LAnklePitch]);
    258. 

  258.     jointsMessage.get()->set_l_ankle_roll(sensorValues_[LAnkleRoll]);
    259. 

  259.     jointsMessage.get()->set_r_hip_roll(sensorValues_[RHipRoll]);
    260. 

  260.     jointsMessage.get()->set_r_hip_pitch(sensorValues_[RHipPitch]);
    261. 

  261.     jointsMessage.get()->set_r_knee_pitch(sensorValues_[RKneePitch]);
    262. 

  262.     jointsMessage.get()->set_r_ankle_pitch(sensorValues_[RAnklePitch]);
    263. 

  263.     jointsMessage.get()->set_r_ankle_roll(sensorValues_[RAnkleRoll]);
    264. 

  264. 

  265.     jointsOutput_.setMessage(jointsMessage);
    266. 

  266. }
    267. 

  267. 

  268. void SensorsModule::updateCurrentsMessage()
    269. 

  269. {
    270. 

  270.     portals::Message<messages::JointAngles> jointsMessage(0);
    271. 

  271. 

  272.     jointsMessage.get()->set_head_yaw(sensorValues_[HeadYawCurrent]);
    273. 

  273.     jointsMessage.get()->set_head_pitch(sensorValues_[HeadPitchCurrent]);
    274. 

  274.     jointsMessage.get()->set_l_shoulder_pitch(sensorValues_[LShoulderPitchCurrent]);
    275. 

  275.     jointsMessage.get()->set_l_shoulder_roll(sensorValues_[LShoulderRollCurrent]);
    276. 

  276.     jointsMessage.get()->set_l_elbow_yaw(sensorValues_[LElbowYawCurrent]);
    277. 

  277.     jointsMessage.get()->set_l_elbow_roll(sensorValues_[LElbowRollCurrent]);
    278. 

  278.     jointsMessage.get()->set_l_wrist_yaw(sensorValues_[LWristYawCurrent]);
    279. 

  279.     jointsMessage.get()->set_l_hand(sensorValues_[LHandCurrent]);
    280. 

  280.     jointsMessage.get()->set_r_shoulder_pitch(sensorValues_[RShoulderPitchCurrent]);
    281. 

  281.     jointsMessage.get()->set_r_shoulder_roll(sensorValues_[RShoulderRollCurrent]);
    282. 

  282.     jointsMessage.get()->set_r_elbow_yaw(sensorValues_[RElbowYawCurrent]);
    283. 

  283.     jointsMessage.get()->set_r_elbow_roll(sensorValues_[RElbowRollCurrent]);
    284. 

  284.     jointsMessage.get()->set_r_wrist_yaw(sensorValues_[RWristYawCurrent]);
    285. 

  285.     jointsMessage.get()->set_r_hand(sensorValues_[RHandCurrent]);
    286. 

  286.     jointsMessage.get()->set_l_hip_yaw_pitch(sensorValues_[LHipYawPitchCurrent]);
    287. 

  287.     jointsMessage.get()->set_r_hip_yaw_pitch(sensorValues_[RHipYawPitchCurrent]);
    288. 

  288.     jointsMessage.get()->set_l_hip_roll(sensorValues_[LHipRollCurrent]);
    289. 

  289.     jointsMessage.get()->set_l_hip_pitch(sensorValues_[LHipPitchCurrent]);
    290. 

  290.     jointsMessage.get()->set_l_knee_pitch(sensorValues_[LKneePitchCurrent]);
    291. 

  291.     jointsMessage.get()->set_l_ankle_pitch(sensorValues_[LAnklePitchCurrent]);
    292. 

  292.     jointsMessage.get()->set_l_ankle_roll(sensorValues_[LAnkleRollCurrent]);
    293. 

  293.     jointsMessage.get()->set_r_hip_roll(sensorValues_[RHipRollCurrent]);
    294. 

  294.     jointsMessage.get()->set_r_hip_pitch(sensorValues_[RHipPitchCurrent]);
    295. 

  295.     jointsMessage.get()->set_r_knee_pitch(sensorValues_[RKneePitchCurrent]);
    296. 

  296.     jointsMessage.get()->set_r_ankle_pitch(sensorValues_[RAnklePitchCurrent]);
    297. 

  297.     jointsMessage.get()->set_r_ankle_roll(sensorValues_[RAnkleRollCurrent]);
    298. 

  298. 

  299.     currentsOutput_.setMessage(jointsMessage);
    300. 

  300. }
    301. 

  301. 

  302. void SensorsModule::updateTemperatureMessage()
    303. 

  303. {
    304. 

  304.     portals::Message<messages::JointAngles> temperaturesMessage(0);
    305. 

  305.     temperaturesMessage.get()->set_head_yaw(sensorValues_[HeadYawTemp]);
    306. 

  306.     temperaturesMessage.get()->set_head_pitch(sensorValues_[HeadPitchTemp]);
    307. 

  307.     temperaturesMessage.get()->set_l_shoulder_pitch(sensorValues_[LShoulderPitchTemp]);
    308. 

  308.     temperaturesMessage.get()->set_l_shoulder_roll(sensorValues_[LShoulderRollTemp]);
    309. 

  309.     temperaturesMessage.get()->set_l_elbow_yaw(sensorValues_[LElbowYawTemp]);
    310. 

  310.     temperaturesMessage.get()->set_l_elbow_roll(sensorValues_[LElbowRollTemp]);
    311. 

  311.     temperaturesMessage.get()->set_l_wrist_yaw(sensorValues_[LWristYawTemp]);
    312. 

  312.     temperaturesMessage.get()->set_l_hand(sensorValues_[LHandTemp]);
    313. 

  313.     temperaturesMessage.get()->set_r_shoulder_pitch(sensorValues_[RShoulderPitchTemp]);
    314. 

  314.     temperaturesMessage.get()->set_r_shoulder_roll(sensorValues_[RShoulderRollTemp]);
    315. 

  315.     temperaturesMessage.get()->set_r_elbow_yaw(sensorValues_[RElbowYawTemp]);
    316. 

  316.     temperaturesMessage.get()->set_r_elbow_roll(sensorValues_[RElbowRollTemp]);
    317. 

  317.     temperaturesMessage.get()->set_r_wrist_yaw(sensorValues_[RWristYawTemp]);
    318. 

  318.     temperaturesMessage.get()->set_r_hand(sensorValues_[RHandTemp]);
    319. 

  319.     temperaturesMessage.get()->set_l_hip_yaw_pitch(sensorValues_[LHipYawPitchTemp]);
    320. 

  320.     temperaturesMessage.get()->set_r_hip_yaw_pitch(sensorValues_[RHipYawPitchTemp]);
    321. 

  321.     temperaturesMessage.get()->set_l_hip_roll(sensorValues_[LHipRollTemp]);
    322. 

  322.     temperaturesMessage.get()->set_l_hip_pitch(sensorValues_[LHipPitchTemp]);
    323. 

  323.     temperaturesMessage.get()->set_l_knee_pitch(sensorValues_[LKneePitchTemp]);
    324. 

  324.     temperaturesMessage.get()->set_l_ankle_pitch(sensorValues_[LAnklePitchTemp]);
    325. 

  325.     temperaturesMessage.get()->set_l_ankle_roll(sensorValues_[LAnkleRollTemp]);
    326. 

  326.     temperaturesMessage.get()->set_r_hip_roll(sensorValues_[RHipRollTemp]);
    327. 

  327.     temperaturesMessage.get()->set_r_hip_pitch(sensorValues_[RHipPitchTemp]);
    328. 

  328.     temperaturesMessage.get()->set_r_knee_pitch(sensorValues_[RKneePitchTemp]);
    329. 

  329.     temperaturesMessage.get()->set_r_ankle_pitch(sensorValues_[RAnklePitchTemp]);
    330. 

  330.     temperaturesMessage.get()->set_r_ankle_roll(sensorValues_[RAnkleRollTemp]);
    331. 

  331. 

  332.     temperatureOutput_.setMessage(temperaturesMessage);
    333. 

  333. }
    334. 

  334. 

  335. void SensorsModule::updateChestboardButtonMessage()
    336. 

  336. {
    337. 

  337.     portals::Message<messages::ButtonState> chestboardMessage(0);
    338. 

  338. 

  339.     chestboardMessage.get()->set_pressed(
    340. 

  340.         sensorValues_[ChestboardButton] > 0.5f ? true : false
    341. 

  341.         );
    342. 

  342. 

  343.     chestboardButtonOutput_.setMessage(chestboardMessage);
    344. 

  344. }
    345. 

  345. 

  346. void SensorsModule::updateFootbumperMessage()
    347. 

  347. {
    348. 

  348.     portals::Message<messages::FootBumperState> footbumperMessage(0);
    349. 

  349. 

  350.     footbumperMessage.get()->mutable_l_foot_bumper_left() ->set_pressed(
    351. 

  351.         sensorValues_[LFootBumperLeft]  > 0.5f ? true : false
    352. 

  352.         );
    353. 

  353.     footbumperMessage.get()->mutable_l_foot_bumper_right()->set_pressed(
    354. 

  354.         sensorValues_[LFootBumperRight] > 0.5f ? true : false
    355. 

  355.         );
    356. 

  356. 

  357.     footbumperMessage.get()->mutable_r_foot_bumper_left() ->set_pressed(
    358. 

  358.         sensorValues_[RFootBumperLeft]  > 0.5f ? true : false
    359. 

  359.         );
    360. 

  360.     footbumperMessage.get()->mutable_r_foot_bumper_right()->set_pressed(
    361. 

  361.         sensorValues_[RFootBumperRight] > 0.5f ? true : false
    362. 

  362.         );
    363. 

  363. 

  364.     footbumperOutput_.setMessage(footbumperMessage);
    365. 

  365. }
    366. 

  366. 

  367. void SensorsModule::updateInertialsMessage()
    368. 

  368. {
    369. 

  369.     portals::Message<messages::InertialState> inertialsMessage(0);
    370. 

  370. 

  371.     inertialsMessage.get()->set_acc_x(sensorValues_[AccX]);
    372. 

  372.     inertialsMessage.get()->set_acc_y(sensorValues_[AccY]);
    373. 

  373.     inertialsMessage.get()->set_acc_z(sensorValues_[AccZ]);
    374. 

  374. 

  375.     inertialsMessage.get()->set_gyr_x(sensorValues_[GyrX]);
    376. 

  376.     inertialsMessage.get()->set_gyr_y(sensorValues_[GyrY]);
    377. 

  377. 

  378.     inertialsMessage.get()->set_angle_x(sensorValues_[AngleX]);
    379. 

  379.     inertialsMessage.get()->set_angle_y(sensorValues_[AngleY]);
    380. 

  380. 

  381.     inertialsOutput_.setMessage(inertialsMessage);
    382. 

  382. }
    383. 

  383. 

  384. void SensorsModule::updateSonarsMessage()
    385. 

  385. {
    386. 

  386.     portals::Message<messages::SonarState> sonarsMessage(0);
    387. 

  387. 

  388.     sonarsMessage.get()->set_us_left(sensorValues_[USLeft]);
    389. 

  389.     sonarsMessage.get()->set_us_right(sensorValues_[USRight]);
    390. 

  390. 

  391.     sonarsOutput_.setMessage(sonarsMessage);
    392. 

  392. }
    393. 

  393. 

  394. void SensorsModule::updateFSRMessage()
    395. 

  395. {
    396. 

  396.     portals::Message<messages::FSR> fsrMessage(0);
    397. 

  397. 

  398.     // Left foot FSR values.
    399. 

  399.     fsrMessage.get()->set_lfl(sensorValues_[LFsrFL]);
    400. 

  400.     fsrMessage.get()->set_lfr(sensorValues_[LFsrFR]);
    401. 

  401.     fsrMessage.get()->set_lrl(sensorValues_[LFsrRL]);
    402. 

  402.     fsrMessage.get()->set_lrr(sensorValues_[LFsrRR]);
    403. 

  403. 

  404.     // Right foot FSR values.
    405. 

  405.     fsrMessage.get()->set_rfl(sensorValues_[RFsrFL]);
    406. 

  406.     fsrMessage.get()->set_rfr(sensorValues_[RFsrFR]);
    407. 

  407.     fsrMessage.get()->set_rrl(sensorValues_[RFsrRL]);
    408. 

  408.     fsrMessage.get()->set_rrr(sensorValues_[RFsrRR]);
    409. 

  409. 

  410.     fsrOutput_.setMessage(fsrMessage);
    411. 

  411. }
    412. 

  412. 

  413. void SensorsModule::updateBatteryMessage()
    414. 

  414. {
    415. 

  415.     portals::Message<messages::BatteryState> batteryMessage(0);
    416. 

  416. 

  417.     batteryMessage.get()->set_charge(sensorValues_[BatteryCharge]);
    418. 

  418. 

  419.     batteryOutput_.setMessage(batteryMessage);
    420. 

  420. }
    421. 

  421. 

  422. void SensorsModule::updateStiffMessage()
    423. 

  423. {
    424. 

  424.     portals::Message<messages::StiffStatus> stiffMessage(0);
    425. 

  425. 

  426.     if (sensorValues_[HeadPitchStiff] > 0 ||
    427. 

  427.         sensorValues_[HeadYawStiff] > 0 ||
    428. 

  428.         sensorValues_[LAnklePitchStiff] > 0 ||
    429. 

  429.         sensorValues_[LAnkleRollStiff] > 0 ||
    430. 

  430.         sensorValues_[LElbowRollStiff] > 0 ||
    431. 

  431.         sensorValues_[LElbowYawStiff] > 0 ||
    432. 

  432.         sensorValues_[LHandStiff] > 0 ||
    433. 

  433.         sensorValues_[LHipPitchStiff] > 0 ||
    434. 

  434.         sensorValues_[LHipRollStiff] > 0 ||
    435. 

  435.         sensorValues_[LHipYawPitchStiff] > 0 ||
    436. 

  436.         sensorValues_[LKneePitchStiff] > 0 ||
    437. 

  437.         sensorValues_[LShoulderPitchStiff] > 0 ||
    438. 

  438.         sensorValues_[LShoulderRollStiff] > 0 ||
    439. 

  439.         sensorValues_[LWristYawStiff] > 0 ||
    440. 

  440.         sensorValues_[RAnklePitchStiff] > 0 ||
    441. 

  441.         sensorValues_[RAnkleRollStiff] > 0 ||
    442. 

  442.         sensorValues_[RElbowRollStiff] > 0 ||
    443. 

  443.         sensorValues_[RElbowYawStiff] > 0 ||
    444. 

  444.         sensorValues_[RHandStiff] > 0 ||
    445. 

  445.         sensorValues_[RHipPitchStiff] > 0 ||
    446. 

  446.         sensorValues_[RHipYawPitchStiff] > 0 ||
    447. 

  447.         sensorValues_[RHipRollStiff] > 0 ||
    448. 

  448.         sensorValues_[RKneePitchStiff] > 0 ||
    449. 

  449.         sensorValues_[RShoulderPitchStiff] > 0 ||
    450. 

  450.         sensorValues_[RShoulderRollStiff] > 0 ||
    451. 

  451.         sensorValues_[RWristYawStiff] > 0)
    452. 

  452.     {
    453. 

  453.         stiffMessage.get()->set_on(1);
    454. 

  454.     }
    455. 

  455.     else
    456. 

  456.     {
    457. 

  457.         stiffMessage.get()->set_on(0);
    458. 

  458.     }
    459. 

  459. 

  460.     stiffStatusOutput_.setMessage(stiffMessage);
    461. 

  461. }
    462. 

  462. 

  463. // Helper method so that we can print out a Sweet Moves joint angle
    464. 

  464. // tuple directly when we want to (ie 5 button presses)
    465. 

  465. std::string makeSweetMoveTuple(const messages::JointAngles* angles)
    466. 

  466. {
    467. 

  467.     char output[240];
    468. 

  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. 

  470.             TO_DEG*angles->l_shoulder_pitch(),
    471. 

  471.             TO_DEG*angles->l_shoulder_roll(),
    472. 

  472.             TO_DEG*angles->l_elbow_yaw(),
    473. 

  473.             TO_DEG*angles->l_elbow_roll(),
    474. 

  474.             TO_DEG*angles->l_hip_yaw_pitch(),
    475. 

  475.             TO_DEG*angles->l_hip_roll(),
    476. 

  476.             TO_DEG*angles->l_hip_pitch(),
    477. 

  477.             TO_DEG*angles->l_knee_pitch(),
    478. 

  478.             TO_DEG*angles->l_ankle_pitch(),
    479. 

  479.             TO_DEG*angles->l_ankle_roll(),
    480. 

  480.             TO_DEG*angles->r_hip_yaw_pitch(),
    481. 

  481.             TO_DEG*angles->r_hip_roll(),
    482. 

  482.             TO_DEG*angles->r_hip_pitch(),
    483. 

  483.             TO_DEG*angles->r_knee_pitch(),
    484. 

  484.             TO_DEG*angles->r_ankle_pitch(),
    485. 

  485.             TO_DEG*angles->r_ankle_roll(),
    486. 

  486.             TO_DEG*angles->r_shoulder_pitch(),
    487. 

  487.             TO_DEG*angles->r_shoulder_roll(),
    488. 

  488.             TO_DEG*angles->r_elbow_yaw(),
    489. 

  489.             TO_DEG*angles->r_elbow_roll());
    490. 

  490. 

  491.     return std::string(output);
    492. 

  492. }
    493. 

  493. 

  494. void SensorsModule::run_()
    495. 

  495. {
    496. 

  496.     printInput.latch();
    497. 

  497. 

  498.     // Simply update all sensor readings from ALMemory.
    499. 

  499.     updateSensorValues();
    500. 

  500. 

  501.     if(printInput.message().toggle() != lastPrint)
    502. 

  502.     {
    503. 

  503.         std::cout << makeSweetMoveTuple(jointsOutput_.getMessage(true).get())
    504. 

  504.                   << std::endl;
    505. 

  505.         lastPrint = !lastPrint;
    506. 

  506.     }
    507. 

  507. }
    508. 

  508. 

  509. } // namespace sensors
    510. 

  510. } // namespace man
    511. 

  511.