labjack_node.cpp
Go to the documentation of this file.
00001 
00026 #include "labjack_node.h"
00027 
00029 
00030 LabjackNode::LabjackNode(LabjackDriverInterface *ljdriver) :
00031     _nh_private("~"),
00032     _publish_rate(10),
00033     _ljdriver(ljdriver)
00034 {
00035     long error = 0;
00036     bool connected = false;
00037 
00038     while(!connected) {
00039         error = _ljdriver->config();
00040         if (error != 0) {
00041             ROS_WARN("[LABJACK] Trying after 5 seconds...");
00042             usleep(5000 * 1000);
00043         }
00044         else {
00045             connected = true;
00046 
00047             getAll();
00048         }
00049     }
00050 
00051     ROS_INFO("[LABJACK] Labjack detected. Waiting for a command...");
00052 }
00053 
00055 
00056 LabjackNode::~LabjackNode()
00057 {
00058 }
00059 
00061 
00062 void LabjackNode::init()
00063 {
00064     _touch_srv = _nh_private.advertiseService("get_touch_sensors", &LabjackNode::getTouchSensors, this);
00065     _get_voltage_srv = _nh_private.advertiseService("get_voltage", &LabjackNode::getPower, this);
00066     _get_is_plugged_srv = _nh_private.advertiseService("is_plugged", &LabjackNode::getIsPlugged, this);
00067     _get_state_srv = _nh_private.advertiseService("get_state", &LabjackNode::getStates, this);
00068     _set_state_srv = _nh_private.advertiseService("set_state", &LabjackNode::setStates, this);
00069 }
00070 
00072 
00073 void LabjackNode::spin()
00074 {
00075     while(_nh.ok()) {
00076         ros::spinOnce();
00077         _publish_rate.sleep();
00078     }
00079 }
00080 
00082 
00083 bool LabjackNode::getTouchSensors(touch_skill_msgs::LabjackTouchInfo::Request & req,
00084                                   touch_skill_msgs::LabjackTouchInfo::Response & resp)
00085 {
00086     touch aux = getTouch();
00087 
00088     for (int i = 0; i < touch_skill_msgs::MaggieTouchParts::NUMBER_SENSORS; i++) {
00089         resp.sensors_status[i] = aux.sensor[i];
00090     }
00091 
00092     return true;
00093 }
00094 
00096 
00097 bool LabjackNode::getPower(batteries_skill_msgs::LabjackBatteryInfo::Request & req,
00098                            batteries_skill_msgs::LabjackBatteryInfo::Response & resp)
00099 {
00100     resp.voltage = getVoltage();
00101 
00102     return true;
00103 }
00104 
00106 
00107 bool LabjackNode::getIsPlugged(batteries_skill_msgs::LabjackPlugInfo::Request & req,
00108                                batteries_skill_msgs::LabjackPlugInfo::Response & resp)
00109 {
00110     resp.is_plugged = isPlugged();
00111 
00112     return true;
00113 }
00114 
00116 
00117 bool LabjackNode::getStates(basic_states_skill_msgs::GetLabjackState::Request & req,
00118                             basic_states_skill_msgs::GetLabjackState::Response & resp)
00119 {
00120     resp.state = getState();
00121 
00122     return true;
00123 }
00124 
00126 
00127 bool LabjackNode::setStates(basic_states_skill_msgs::SetLabjackState::Request & req,
00128                             basic_states_skill_msgs::SetLabjackState::Response & resp)
00129 {
00130     if ((setState(req.state)) < 0) {
00131         ROS_ERROR("[LABJACK] Error setting new state: %d", req.state);
00132         return false;
00133     }
00134 
00135     return true;
00136 }
00137 
00139 
00140 t_ljdata LabjackNode::getAll()
00141 {
00142     t_ljdata data;
00143     int error = 0;
00144 
00145     long stateDs = 0, stateIOs = 0;
00146     if ((error = _ljdriver->readDIOs(&stateDs, &stateIOs)) != 0) {
00147         ROS_ERROR("[LABJACK] Error reading D/IO lines");
00148         stateDs = stateIOs = -1;
00149     }
00150     _labjack_data.DsState = stateDs;
00151     _labjack_data.IOsState = stateIOs;
00152 
00153     long lines03[] = {0,
00154                       1,
00155                       2,
00156                       3};
00157     long lines47[] = {4,
00158                       5,
00159                       6,
00160                       7};
00161     float voltages03[] = {0,
00162                           0,
00163                           0,
00164                           0};
00165     float voltages47[] = {0,
00166                           0,
00167                           0,
00168                           0};
00169     if ((error = _ljdriver->readAIs(4, lines03, voltages03)) != 0) {
00170         ROS_ERROR("[LABJACK] Error reading AI0..AI3 lines");
00171         voltages03[0] = voltages03[1] = voltages03[2] = voltages03[3] = -1;
00172     }
00173     if ((error = _ljdriver->readAIs(4, lines47, voltages47)) != 0) {
00174         ROS_ERROR("[LABJACK] Error reading AI4..AI7 lines");
00175         voltages47[0] = voltages47[1] = voltages47[2] = voltages47[3] = -1;
00176     }
00177     for (short i = 0; i < 4; i++) {
00178         _labjack_data.AIsState[i] = voltages03[i];
00179         _labjack_data.AIsState[i + 4] = voltages47[i];
00180     }
00181 
00182     long DsDirection = 0;
00183     if ((error = _ljdriver->readDirectionDs(&DsDirection)) != 0) {
00184         ROS_ERROR("[LABJACK] Error reading Ds directions");
00185         DsDirection = -1;
00186     }
00187     _labjack_data.DsDirection = DsDirection;
00188 
00189     data.DsState = _labjack_data.DsState;
00190     data.IOsState = _labjack_data.IOsState;
00191     for (short i = 0; i < NUM_LIN_AI; i++) {
00192         data.AIsState[i] = _labjack_data.AIsState[i];
00193     }
00194 
00195     return data;
00196 }
00197 
00199 
00200 float LabjackNode::getStateDs()
00201 {
00202     long stateDs = 0, stateIOs = 0;
00203 
00204     if (_ljdriver->readDIOs(&stateDs, &stateIOs) != 0) {
00205         ROS_ERROR("[LABJACK] Error reading D/IO lines");
00206         stateDs = stateIOs = -1;
00207     }
00208     _labjack_data.DsState = stateDs;
00209 
00210     return stateDs;
00211 }
00212 
00214 
00215 int LabjackNode::setStateDs(long stateD)
00216 {
00217     int error = 0;
00218     long stateIOs = 0;
00219 
00220     stateIOs = _labjack_data.IOsState;
00221     error = (int) _ljdriver->writeDIOsCONFIG(&stateD, &stateIOs);
00222 
00223     return error;
00224 }
00225 
00227 
00228 int LabjackNode::getStateD(long stateD)
00229 {
00230     int result = 0, error = 0;
00231     long mask = 0x0001;
00232 
00233     if (stateD < 0 || stateD >= NUM_LIN_D) {
00234         result = -1;
00235     }
00236     else {
00237         long stateDs = 0, stateIOs = 0;
00238         if ((error = _ljdriver->readDIOs(&stateDs, &stateIOs)) != 0) {
00239             ROS_ERROR("[LABJACK] Error reading D/IO lines");
00240             stateDs = stateIOs = -1;
00241         }
00242         _labjack_data.DsState = stateDs;
00243 
00244         mask <<= stateD;
00245         if (stateDs & mask) {
00246             result = 1;
00247         }
00248         else {
00249             result = 0;
00250         }
00251     }
00252 
00253     return result;
00254 }
00255 
00257 
00258 int LabjackNode::setStateD(long stateD)
00259 {
00260     int error = 0;
00261 
00262     // write D line and reconfig the directions
00263     error = (int) _ljdriver->writeDCONFIG(stateD, 1);
00264 
00265     return error;
00266 }
00267 
00269 
00270 int LabjackNode::clearStateD(long stateD)
00271 {
00272     int error = 0;
00273 
00274     error = (int) _ljdriver->writeDCONFIG(stateD, 0);
00275 
00276     return error;
00277 }
00278 
00280 
00281 long LabjackNode::getDsDirection()
00282 {
00283     int error = 0;
00284     long DsDirection = 0;
00285 
00286     if ((error = _ljdriver->readDirectionDs(&DsDirection)) != 0) {
00287         ROS_ERROR("[LABJACK] Error reading Ds directions");
00288         DsDirection = -1;
00289     }
00290     _labjack_data.DsDirection = DsDirection;
00291 
00292     return DsDirection;
00293 }
00294 
00296 
00297 float LabjackNode::getStateIOs()
00298 {
00299     int error = 0;
00300     long stateDs = 0, stateIOs = 0;
00301 
00302     if ((error = _ljdriver->readDIOs(&stateDs, &stateIOs)) != 0) {
00303         ROS_ERROR("[LABJACK] Error reading D/IO lines");
00304         stateDs = stateIOs = -1;
00305     }
00306     _labjack_data.IOsState = stateIOs;
00307 
00308     return stateIOs;
00309 }
00310 
00312 
00313 int LabjackNode::setStateIOs(long stateIO)
00314 {
00315     int error;
00316     long stateD;
00317 
00318     stateD = _labjack_data.DsState;
00319 
00320     error = (int) _ljdriver->writeDIOsCONFIG(&stateD, &stateIO);
00321 
00322     return error;
00323 }
00324 
00326 
00327 int LabjackNode::getStateIO(long stateIO)
00328 {
00329     int result = 0;
00330     long mask = 0x0001;
00331 
00332     if (stateIO < 0 || stateIO >= NUM_LIN_IO) {
00333         result = -1;
00334     }
00335     else {
00336         int error = 0;
00337         long stateDs = 0, stateIOs = 0;
00338 
00339         if ((error = _ljdriver->readDIOs(&stateDs, &stateIOs)) != 0) {
00340             ROS_ERROR("[LABJACK] Error reading D/IO lines");
00341             stateDs = stateIOs = -1;
00342         }
00343         _labjack_data.IOsState = stateIOs;
00344 
00345         mask <<= stateIO;
00346         if (stateIOs & mask)
00347             result = 1;
00348         else
00349             result = 0;
00350     }
00351 
00352     return result;
00353 }
00354 
00356 
00357 int LabjackNode::setStateIO(long stateIO)
00358 {
00359     int error = 0;
00360 
00361     error = (int) _ljdriver->writeIO(stateIO, 1);
00362 
00363     return error;
00364 }
00365 
00367 
00368 int LabjackNode::clearStateIO(long stateIO)
00369 {
00370     int error = 0;
00371 
00372     error = (int) _ljdriver->writeIO(stateIO, 0);
00373 
00374     return error;
00375 }
00376 
00378 
00379 float8 LabjackNode::getAIs()
00380 {
00381     float8 result;
00382 
00383     long lines03[] = {0,
00384                       1,
00385                       2,
00386                       3};
00387     long lines47[] = {4,
00388                       5,
00389                       6,
00390                       7};
00391     float voltages03[] = {0,
00392                           0,
00393                           0,
00394                           0};
00395     float voltages47[] = {0,
00396                           0,
00397                           0,
00398                           0};
00399 
00400     if (_ljdriver->readAIs(4, lines03, voltages03) != 0) {
00401         ROS_ERROR("[LABJACK] Error reading AI0..AI3 lines");
00402         voltages03[0] = voltages03[1] = voltages03[2] = voltages03[3] = -1;
00403     }
00404     if (_ljdriver->readAIs(4, lines47, voltages47) != 0) {
00405         ROS_ERROR("[LABJACK] Error reading AI4..AI7 lines");
00406         voltages47[0] = voltages47[1] = voltages47[2] = voltages47[3] = -1;
00407     }
00408     for (short i = 0; i < 4; i++) {
00409         _labjack_data.AIsState[i] = voltages03[i];
00410         _labjack_data.AIsState[i + 4] = voltages47[i];
00411     }
00412 
00413     for (short i = 0; i < NUM_LIN_AI; i++) {
00414         result.v_float8[i] = _labjack_data.AIsState[i];
00415     }
00416 
00417     return result;
00418 }
00419 
00421 
00422 float LabjackNode::getAI(long channel)
00423 {
00424     float result = 0.0;
00425     int error = 0;
00426 
00427     if (channel < 0 || channel >= NUM_LIN_AI) {
00428         result = -1;
00429     }
00430     else {
00431         if ((error = _ljdriver->readAI(channel, &result)) != 0) {
00432             ROS_ERROR("[LABJACK] Error reading AI%ld line", channel);
00433             result = -1;
00434         }
00435         _labjack_data.AIsState[channel] = result;
00436     }
00437 
00438     return result;
00439 }
00440 
00442 
00443 int LabjackNode::setAOs(float2 voltageAO)
00444 {
00445     int error = 0;
00446 
00447     error = (int) _ljdriver->writeAOs(voltageAO.v_float2[0], voltageAO.v_float2[1]);
00448 
00449     return error;
00450 }
00451 
00453 
00454 int LabjackNode::setPulseD(t_pulseData data)
00455 {
00456     int error = 0;
00457     long mask = 1 << (int) data.line;
00458     long directions = 0;
00459 
00460     if (_ljdriver->readDirectionDs(&directions) == 0) {
00461         // line is configured as output
00462         if ((directions & mask) != 0) {
00463             error = (int) _ljdriver->writePulse(data.level, data.line, data.microseconds, 0);
00464         }
00465         else {
00466             // line is configured as input
00467             ROS_WARN("[LABJACK] Impossible to write a pulse to an input line");
00468             error = -1;
00469         }
00470     }
00471     else {
00472         ROS_ERROR("[LABJACK] Error reading line direction");
00473         error = -1;
00474     }
00475 
00476     return error;
00477 }
00478 
00480 
00481 int LabjackNode::setPulseIO(t_pulseData data)
00482 {
00483     int error = 0;
00484     long mask = 1 << (int) data.line;
00485     long directions = 0;
00486 
00487     if (_ljdriver->readDirectionDs(&directions) == 0) {
00488         // line is configured as output
00489         if ((directions & mask) != 0) {
00490             error = (int) _ljdriver->writePulse(data.level, data.line, data.microseconds, 1);
00491         }
00492         else {
00493             // line is configured as input
00494             ROS_WARN("[LABJACK] Impossible to write a pulse to an input line");
00495             error = -1;
00496         }
00497     }
00498     else {
00499         ROS_ERROR("[LABJACK] Error reading line direction");
00500         error = -1;
00501     }
00502 
00503     return error;
00504 }
00505 
00507 
00508 int LabjackNode::getState()
00509 {
00510     int state = 0;
00511     float stateIO = getStateIOs();
00512 
00513     if (!((long) (stateIO) & EMERGENCY)) {
00514         state = basic_states_skill_msgs::States::EMERGENCY_STATE;
00515     }
00516     else if (!((long) (stateIO) & ENABLE_BASE) && !((long) (stateIO) & ENABLE_BODY)) {
00517         // sleep state => !base & !body (low level closed by rele)
00518         state = basic_states_skill_msgs::States::SLEEP_STATE;
00519     }
00520     else if (!((long) (stateIO) & ENABLE_BASE) && ((long) (stateIO) & ENABLE_BODY)) {
00521         // stop state => !base & body (low level closed by rele)
00522         state = basic_states_skill_msgs::States::STOP_STATE;
00523     }
00524     else if (((long) (stateIO) & ENABLE_BASE) && ((long) (stateIO) & ENABLE_BODY)) {
00525         // active state => base & body (low level closed by rele)
00526         state = basic_states_skill_msgs::States::ACTIVE_STATE;
00527     }
00528     else {
00529         state = basic_states_skill_msgs::States::UNDEFINED_STATE;
00530     }
00531 
00532     return state;
00533 }
00534 
00536 
00537 int LabjackNode::setState(int state)
00538 {
00539     int error = 0;
00540 
00541     switch(state) {
00542         case basic_states_skill_msgs::States::SLEEP_STATE:
00543             ROS_INFO("[LABJACK] SLEEP STATE");
00544             if (_ljdriver->writeIO(IO_ENABLE_BASE, DISABLE) != 0
00545                 || _ljdriver->writeIO(IO_ENABLE_OTHERS, DISABLE) != 0) {
00546                 ROS_ERROR("[LABJACK] Error in setState: changing state to SLEEP");
00547                 error = -1;
00548             }
00549             break;
00550         case basic_states_skill_msgs::States::STOP_STATE:
00551             ROS_INFO("[LABJACK] STOP STATE");
00552             if (_ljdriver->writeIO(IO_ENABLE_BASE, DISABLE) != 0 || _ljdriver->writeIO(IO_ENABLE_OTHERS, ENABLE) != 0) {
00553                 ROS_ERROR("[LABJACK] Error in setState: changing state to STOP");
00554                 error = -1;
00555             }
00556             ROS_DEBUG("[LABJACK] Checking state: %d == %d", basic_states_skill_msgs::States::STOP_STATE, getState());
00557             break;
00558         case basic_states_skill_msgs::States::ACTIVE_STATE:
00559             ROS_INFO("[LABJACK] ACTIVE STATE");
00560             if (_ljdriver->writeIO(IO_ENABLE_BASE, ENABLE) != 0 || _ljdriver->writeIO(IO_ENABLE_OTHERS, ENABLE) != 0) {
00561                 ROS_ERROR("[LABJACK] Error in setState: changing state to ACTIVE");
00562                 error = -1;
00563             }
00564             break;
00565         case basic_states_skill_msgs::States::EMERGENCY_STATE:
00566             ROS_INFO("[LABJACK] EMERGENCY STATE");
00567             if (setEmergency()) {
00568                 ROS_ERROR("[LABJACK] Error in setState: changing state to EMERGENCY");
00569                 error = -1;
00570             }
00571             break;
00572         default:
00573             ROS_INFO("[LABJACK] UNDEFINED STATE");
00574             error = -1;
00575     }
00576 
00577     return error;
00578 }
00579 
00581 
00582 int LabjackNode::enableBase(bool enable)
00583 {
00584     int error = 0;
00585     long line_enable_base = ENABLE_BASE;
00586 
00587     error = (enable) ? clearStateIO(line_enable_base) : setStateIO(line_enable_base);
00588 
00589     return error;
00590 }
00591 
00593 
00594 int LabjackNode::enableBody(bool enable)
00595 {
00596     int result = 0;
00597     long line_enable_body = ENABLE_BODY;
00598 
00599     result = (enable) ? clearStateIO(line_enable_body) : setStateIO(line_enable_body);
00600 
00601     return result;
00602 }
00603 
00605 
00606 int LabjackNode::setWatchdog()
00607 {
00608     int error = 0;
00609     long timeout = TIMEOUT_WD;
00610     int lineD_WD = D_WATCHDOG;
00611     long state = 0;
00612 
00613     error = (int) _ljdriver->enableWatchdog(timeout, lineD_WD, state);
00614 
00615     return error;
00616 }
00617 
00619 
00620 int LabjackNode::clearWatchdog()
00621 {
00622     int error = 0;
00623 
00624     error = (int) _ljdriver->disableWatchdog(D_WATCHDOG);
00625 
00626     return error;
00627 }
00628 
00630 
00631 int LabjackNode::isEmergency()
00632 {
00633     int result = 0;
00634     long emergency_line = IO_EMERGENCY;
00635 
00636     result = getStateIO(emergency_line);
00637 
00638     return result;
00639 }
00640 
00642 
00643 int LabjackNode::setEmergency()
00644 {
00645     int error = 0;
00646 
00647     // send a low level pulse of 10ms
00648     if ((_ljdriver->writePulse(0, IO_SETA_SF, 10000, 1)) != 0) {
00649         error = -1;
00650     }
00651 
00652     if (!error) {
00653         // disable lines for the base and the body
00654         if (_ljdriver->writeIO(IO_ENABLE_BASE, DISABLE) != 0 || _ljdriver->writeIO(IO_ENABLE_OTHERS, DISABLE) != 0) {
00655             error = -1;
00656         }
00657     }
00658 
00659     return error;
00660 }
00661 
00663 
00664 float LabjackNode::getVoltage()
00665 {
00666     float result = 0.0;
00667     float v = 0.0;
00668     long ai_voltage_base = AI_VOLTAGE_BASE;
00669 
00670     v = getAI(ai_voltage_base);
00671     if (v == -1) {
00672         ROS_ERROR("[LABJACK] Error getting the voltage");
00673         result = v;
00674     }
00675     else {
00676         result = v * GAIN_VOLT;
00677     }
00678 
00679     return result;
00680 }
00681 
00683 
00684 bool LabjackNode::isPlugged()
00685 {
00686     bool result = true;
00687     float v = 0.0;
00688     long ai_plugged = AI_PLUGGED;
00689 
00690     v = getAI(ai_plugged);
00691     if (v == -1) {
00692         ROS_ERROR("[LABJACK] Error reading the charger");
00693         result = false;
00694     }
00695     else {
00696         (v > 3.0) ? result = true : result = false;
00697     }
00698 
00699     return result;
00700 }
00701 
00703 
00704 touch LabjackNode::getTouch()
00705 {
00706     static touch result;
00707     long mask = 0;
00708     int error = 0;
00709 
00710     long stateDs = 0, stateIOs = 0;
00711     if ((error = _ljdriver->readDIOs(&stateDs, &stateIOs)) != 0) {
00712         ROS_ERROR("[LABJACK] Error reading D/IO lines");
00713         stateDs = stateIOs = -1;
00714     }
00715     _labjack_data.DsState = stateDs;
00716 
00717     // touch sensors are activated to low level (0- touched, 1- non touched)
00718     mask = HEAD; // point to D1
00719     // check lines D1 to D3, touch sensors associated
00720     for (short i = 1; i <= 3; i++) {
00721         if ((~stateDs) & mask) {
00722             result.sensor[i - 1] = TOUCHED;
00723         }
00724         else {
00725             result.sensor[i - 1] = UNTOUCHED;
00726         }
00727         mask <<= 1;
00728     }
00729 
00730     mask = LEFT_SHOULDER; // point to D5
00731     // check lines D5 to D7, touch sensor associated
00732     for (short i = 5; i <= 7; i++) {
00733         if ((~stateDs) & mask) {
00734             result.sensor[i - 2] = TOUCHED;
00735         }
00736         else {
00737             result.sensor[i - 2] = UNTOUCHED;
00738         }
00739         mask <<= 1;
00740     }
00741 
00742     mask = RIGHT_FOREARM; // point to D11
00743     // check lines D11 to D15, touch sensors associated
00744     for (short i = 11; i <= 15; i++) {
00745         if ((~stateDs) & mask) {
00746             result.sensor[i - 5] = TOUCHED;
00747         }
00748         else {
00749             result.sensor[i - 5] = UNTOUCHED;
00750         }
00751         mask <<= 1;
00752     }
00753 
00754     return result;
00755 }
00756 


labjack
Author(s): Raul Perula-Martinez
autogenerated on Wed Apr 1 2015 10:17:06