robodyn_mechanisms: JointControlManagerGripper.cpp Source File

Go to the documentation of this file.
00001 
00005 #include "robodyn_mechanisms/JointControlManagerGripper.h"
00006 /***************************************************************************/
00017 JointControlManagerGripper::JointControlManagerGripper(const std::string& mechanism, IoFunctions ioFunctions, double timeLimit, const std::string& type, NodeRegisterManagerPtr nodeRegisterManager)
00018     : JointControlManagerInterface(mechanism, ioFunctions, timeLimit, type), nodeRegisterManager(nodeRegisterManager), logCategory("gov.nasa.JointControlManagerGripper")
00019 {
00020     setParameters();
00021 
00022     // Initialize variables
00023     lastControlModeMatchTime = lastCommandModeMatchTime = lastCalibrationModeMatchTime = lastClearFaultModeMatchTime = ros::Time::now();
00024 
00025     // Instantiate state machines
00026     jointControlActual  = boost::make_shared<JointControlActualGripper> (mechanism, ioFunctions, nodeRegisterManager);
00027     jointControlCommand = boost::make_shared<JointControlCommandGripper>(mechanism, ioFunctions, nodeRegisterManager);
00028 
00029 }
00030 
00031 JointControlManagerGripper::~JointControlManagerGripper()
00032 {
00033     // Nothing
00034 }
00035 
00036 void JointControlManagerGripper::setParameters()
00037 {
00038     defaultStates.controlMode.state     = r2_msgs::JointControlMode::OFF;
00039     defaultStates.commandMode.state     = r2_msgs::JointControlCommandMode::MOTCOM;
00040     defaultStates.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
00041     defaultStates.clearFaultMode.state  = r2_msgs::JointControlClearFaultMode::DISABLE;
00042     defaultStates.coeffState.state      = r2_msgs::JointControlCoeffState::NOTLOADED;
00043 
00045     prevActualStates = defaultStates;
00046     actualStates = defaultStates;
00047     commandStates = defaultStates;
00048 }
00049 
00050 /***************************************************************************/
00057 r2_msgs::JointControlData JointControlManagerGripper::getActualStates(void)
00058 {
00059     prevActualStates = actualStates;
00060     jointControlActual->getStates(actualStates);
00061 
00062     if (powerState.data == r2_msgs::PowerState::POWER_OFF)
00063     {
00064         actualStates = defaultStates;
00065     }
00066 
00068     if (actualStates.clearFaultMode.state == r2_msgs::JointControlClearFaultMode::ENABLE &&
00069             commandStates.clearFaultMode.state == r2_msgs::JointControlClearFaultMode::ENABLE)
00070     {
00071         jointControlCommand->disableClearFaultMode();
00072     }
00073 
00074     return actualStates;
00075 }
00076 
00077 /***************************************************************************/
00084 r2_msgs::JointControlData JointControlManagerGripper::getCommandStates(void)
00085 {
00086     jointControlCommand->getStates(commandStates);
00087     return commandStates;
00088 }
00089 
00090 /***************************************************************************/
00097 void JointControlManagerGripper::setCommandStates(const r2_msgs::JointControlData& command)
00098 {
00099     switch (command.controlMode.state)
00100     {
00101         case r2_msgs::JointControlMode::BOOTLOADER:
00102 
00104             if ( (powerState.data == r2_msgs::PowerState::MOTOR48_POWER) ||
00105                     (powerState.data == r2_msgs::PowerState::MOTOR96_POWER) ||
00106                     (powerState.data == r2_msgs::PowerState::MOTOR_POWER))
00107             {
00108                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00109                 break;
00110             }
00111 
00114             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED    ||
00115                      actualStates.controlMode.state == r2_msgs::JointControlMode::BOOTLOADER ||
00116                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF))
00117             {
00118                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00119                 break;
00120             }
00121 
00122             jointControlCommand->bootLoader();
00123             break;
00124 
00125         case r2_msgs::JointControlMode::OFF:
00126 
00128             if ((powerState.data == r2_msgs::PowerState::MOTOR48_POWER) ||
00129                     (powerState.data == r2_msgs::PowerState::MOTOR96_POWER) ||
00130                     (powerState.data == r2_msgs::PowerState::MOTOR_POWER))
00131             {
00132                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00133                 break;
00134             }
00135 
00137             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED    ||
00138                      actualStates.controlMode.state == r2_msgs::JointControlMode::BOOTLOADER ||
00139                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF))
00140             {
00141                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00142                 break;
00143             }
00144 
00145             jointControlCommand->off();
00146             break;
00147 
00148         case r2_msgs::JointControlMode::PARK:
00149 
00151             if (powerState.data == r2_msgs::PowerState::POWER_OFF)
00152             {
00153                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00154                 break;
00155             }
00156 
00158             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED ||
00159                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF     ||
00160                      actualStates.controlMode.state == r2_msgs::JointControlMode::PARK    ||
00161                      actualStates.controlMode.state == r2_msgs::JointControlMode::NEUTRAL ||
00162                      actualStates.controlMode.state == r2_msgs::JointControlMode::DRIVE))
00163             {
00164                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00165                 break;
00166             }
00167 
00168             jointControlCommand->park();
00169             break;
00170 
00171         case r2_msgs::JointControlMode::NEUTRAL:
00172 
00174             if (powerState.data == r2_msgs::PowerState::POWER_OFF)
00175             {
00176                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00177                 break;
00178             }
00179 
00181             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED ||
00182                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF     ||
00183                      actualStates.controlMode.state == r2_msgs::JointControlMode::PARK    ||
00184                      actualStates.controlMode.state == r2_msgs::JointControlMode::NEUTRAL))
00185             {
00186                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00187                 break;
00188             }
00189 
00190             jointControlCommand->neutral();
00191             break;
00192 
00193         case r2_msgs::JointControlMode::DRIVE:
00194 
00196             if (powerState.faulted ||
00197                     powerState.data == r2_msgs::PowerState::LOGIC_POWER ||
00198                     powerState.data == r2_msgs::PowerState::POWER_OFF ||
00199                     powerState.data == r2_msgs::PowerState::TRANSITIONING ||
00200                     powerState.data == r2_msgs::PowerState::UNKNOWN)
00201             {
00202                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00203                 break;
00204             }
00205 
00207             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::PARK    ||
00208                      actualStates.controlMode.state == r2_msgs::JointControlMode::NEUTRAL ||
00209                      actualStates.controlMode.state == r2_msgs::JointControlMode::DRIVE))
00210             {
00211                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00212                 break;
00213             }
00214 
00215             jointControlCommand->drive();
00216             break;
00217 
00218         default:
00219             // nothing
00220             break;
00221     }
00222 
00223     switch (command.commandMode.state)
00224     {
00225         case r2_msgs::JointControlCommandMode::MOTCOM:
00226             if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00227             {
00228                 jointControlCommand->motCom();
00229             }
00230             else
00231             {
00232                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode)
00233                         << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00234             }
00235 
00236             break;
00237 
00238         case r2_msgs::JointControlCommandMode::STALLMODE:
00239             if ((actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE) &&
00240                     (actualStates.coeffState.state  == r2_msgs::JointControlCoeffState::LOADED))
00241             {
00242                 jointControlCommand->stallMode();
00243             }
00244             else
00245             {
00246                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00247                         << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode)
00248                         << ", current coeffs state: " << RosMsgPrinter::jointControlCoeffsLoadedStateToString(actualStates.coeffState);
00249             }
00250 
00251             break;
00252 
00253         case r2_msgs::JointControlCommandMode::MULTILOOPSTEP:
00254             if (actualStates.coeffState.state  == r2_msgs::JointControlCoeffState::LOADED)
00255             {
00256                 if ((actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH) ||
00257                         (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSTEP))
00258                 {
00259                     jointControlCommand->multiLoopStep();
00260                 }
00261                 else if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00262                 {
00263                     jointControlCommand->multiLoopStep();
00264                 }
00265                 else
00266                 {
00267                     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00268                             << "current control mode:" << RosMsgPrinter::jointControlModeToString(actualStates.controlMode)
00269                             << "current command mode: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00270                 }
00271             }
00272             else
00273             {
00274                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00275                         << ", current coeffs state: " << RosMsgPrinter::jointControlCoeffsLoadedStateToString(actualStates.coeffState);
00276             }
00277 
00278             break;
00279 
00280         case r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH:
00281             if (actualStates.coeffState.state  == r2_msgs::JointControlCoeffState::LOADED)
00282             {
00283                 if ((actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH) ||
00284                         (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSTEP))
00285                 {
00286                     jointControlCommand->multiLoopStep();
00287                     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00288                             << ", commanding MULTILOOPSTEP instead";
00289 
00290                 }
00291                 else if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00292                 {
00293                     jointControlCommand->multiLoopStep();
00294                     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00295                             << ", commanding MULTILOOPSTEP instead";
00296 
00297                 }
00298                 else
00299                 {
00300                     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00301                             << "current control mode:" << RosMsgPrinter::jointControlModeToString(actualStates.controlMode)
00302                             << "current command mode: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00303                 }
00304             }
00305             else
00306             {
00307                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00308                         << ", current coeffs state: " << RosMsgPrinter::jointControlCoeffsLoadedStateToString(actualStates.coeffState);
00309             }
00310 
00311             break;
00312 
00313         default:
00314             // nothing
00315             break;
00316     }
00317 
00318     switch (command.calibrationMode.state)
00319     {
00320         case r2_msgs::JointControlCalibrationMode::DISABLE:
00321             if (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MOTCOM)
00322             {
00323                 jointControlCommand->disableCalibrationMode();
00324             }
00325             else
00326             {
00327                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlCalibrationMode::DISABLE not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00328             }
00329 
00330             break;
00331 
00332         case r2_msgs::JointControlCalibrationMode::ENABLE:
00333             if (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MOTCOM)
00334             {
00335                 jointControlCommand->enableCalibrationMode();
00336             }
00337             else
00338             {
00339                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlCalibrationMode::ENABLE not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00340             }
00341 
00342             break;
00343 
00344         default:
00345             // nothing
00346             break;
00347     }
00348 
00349     switch (command.clearFaultMode.state)
00350     {
00351         case r2_msgs::JointControlClearFaultMode::DISABLE:
00352             jointControlCommand->disableClearFaultMode();
00353             break;
00354 
00355         case r2_msgs::JointControlClearFaultMode::ENABLE:
00356             jointControlCommand->enableClearFaultMode();
00357             break;
00358 
00359         default:
00360             // nothing
00361             break;
00362     }
00363 }
00364 
00365 /***************************************************************************/
00372 bool JointControlManagerGripper::verifyStates(void)
00373 {
00374     return verifyControlModeState() && verifyCommandModeState() && verifyCalibrationModeState() && verifyClearFaultModeState();
00375 }
00376 
00377 /***************************************************************************/
00384 bool JointControlManagerGripper::verifyControlModeState(void)
00385 {
00386     if (actualStates.controlMode.state == commandStates.controlMode.state)
00387     {
00388         lastControlModeMatchTime = ros::Time::now();
00389         return true;
00390     }
00391 
00392     // Handle faults
00393     if (actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED)
00394     {
00395         if (prevActualStates.controlMode.state != r2_msgs::JointControlMode::FAULTED)
00396         {
00397             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Joint faulted: " << mechanism;
00398         }
00399 
00400         return false;
00401     }
00402 
00403     // Handle other state mismatches
00404     ros::Duration timeSinceLastMatch = ros::Time::now() - lastControlModeMatchTime;
00405 
00406     if (timeSinceLastMatch.toSec() > timeLimit)
00407     {
00408         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Timed out waiting for " << RosMsgPrinter::jointControlModeToString(commandStates.controlMode) << " received " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode) << " on mechanism: " << mechanism;
00409         return false;
00410     }
00411 
00412     return true;
00413 }
00414 
00415 /***************************************************************************/
00422 bool JointControlManagerGripper::verifyCommandModeState(void)
00423 {
00424     if (actualStates.commandMode.state == commandStates.commandMode.state)
00425     {
00426         lastCommandModeMatchTime = ros::Time::now();
00427         return true;
00428     }
00429 
00430     // Handle state mismatches
00431     ros::Duration timeSinceLastMatch = ros::Time::now() - lastCommandModeMatchTime;
00432 
00433     if (timeSinceLastMatch.toSec() > timeLimit)
00434     {
00435         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Timed out waiting for " << RosMsgPrinter::jointControlCommandModeToString(commandStates.commandMode) << " received " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode) << " on mechanism: " << mechanism;
00436         return false;
00437     }
00438 
00439     return true;
00440 }
00441 
00442 /***************************************************************************/
00449 bool JointControlManagerGripper::verifyCalibrationModeState(void)
00450 {
00451     if (actualStates.calibrationMode.state == commandStates.calibrationMode.state)
00452     {
00453         lastCalibrationModeMatchTime = ros::Time::now();
00454         return true;
00455     }
00456 
00457     // Handle state mismatches
00458     ros::Duration timeSinceLastMatch = ros::Time::now() - lastCalibrationModeMatchTime;
00459 
00460     if (timeSinceLastMatch.toSec() > timeLimit)
00461     {
00462         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Timed out waiting for " << RosMsgPrinter::jointControlCalibrationModeToString(commandStates.calibrationMode) << " received " << RosMsgPrinter::jointControlCalibrationModeToString(actualStates.calibrationMode) << " on mechanism: " << mechanism;
00463         return false;
00464     }
00465 
00466     return true;
00467 }
00468 
00469 /***************************************************************************/
00476 bool JointControlManagerGripper::verifyClearFaultModeState(void)
00477 {
00478     if (actualStates.clearFaultMode.state == commandStates.clearFaultMode.state)
00479     {
00480         lastClearFaultModeMatchTime = ros::Time::now();
00481         return true;
00482     }
00483 
00484     // Handle state mismatches
00485     ros::Duration timeSinceLastMatch = ros::Time::now() - lastClearFaultModeMatchTime;
00486 
00487     if (timeSinceLastMatch.toSec() > timeLimit)
00488     {
00489         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Timed out waiting for " << RosMsgPrinter::jointControlClearFaultModeToString(commandStates.clearFaultMode) << " received " << RosMsgPrinter::jointControlClearFaultModeToString(actualStates.clearFaultMode) << " on mechanism: " << mechanism;
00490 
00491         if (actualStates.clearFaultMode.state == r2_msgs::JointControlClearFaultMode::DISABLE)
00492         {
00493             jointControlCommand->disableClearFaultMode();
00494         }
00495 
00496         return false;
00497     }
00498 
00499     return true;
00500 }
00501 
00502 /***************************************************************************/
00509 diagnostic_msgs::DiagnosticStatus JointControlManagerGripper::getFaults()
00510 {
00511     diagnostic_msgs::DiagnosticStatus faultStatus;
00512     jointControlActual->getFaults(faultStatus);
00513     return faultStatus;
00514 }