JointControlManager_Test.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <gtest/gtest.h>
00003 #include <boost/shared_ptr.hpp>
00004 #include "robot_instance/RobotInstanceFactory.h"
00005 #include "robot_instance/NodeRegisterManager.h"
00006 #include "robodyn_mechanisms/JointControlManagerGripper.h"
00007 #include "robodyn_mechanisms/JointControlManagerSeriesElastic.h"
00008 #include "robodyn_mechanisms/JointControlManagerWrist.h"
00009 #include "robodyn_mechanisms/JointControlManagerFactory.h"
00010 #include <ros/package.h>
00011 
00012 using namespace std;
00013 
00014 // Fake the io functions
00015 std::map<std::string, float> testLiveCoeffs;
00016 ParamMap                     testParamMap;
00017 RobotInstancePtr             instance;
00018 
00019 std::string getRegisterFile(std::string mechanism)
00020 {
00021     return instance->configurationSafetyBasePath + RobotInstance::REGISTER_PATH + instance->mechanismRegisterMap[mechanism];
00022 }
00023 
00024 std::string getControlFile(std::string mechanism)
00025 {
00026     return instance->configurationSafetyBasePath + RobotInstance::APIMAP_PATH + instance->mechanismControlMap[mechanism];
00027 }
00028 
00029 bool hasLiveCoeff(std::string coeff)
00030 {
00031     return testLiveCoeffs.count(coeff) > 0;
00032 }
00033 
00034 float getLiveCoeff(std::string coeff)
00035 {
00036     return testLiveCoeffs[coeff];
00037 }
00038 
00039 void setLiveCoeff(std::string coeff, float value)
00040 {
00041     testLiveCoeffs[coeff] = value;
00042 }
00043 
00044 
00045 bool hasParam(std::string paramName)
00046 {
00047     return (testParamMap.find(paramName) != testParamMap.end());
00048 }
00049 
00050 bool getBoolParam(std::string paramName, bool& value)
00051 {
00052     value = boost::get<bool>(testParamMap[paramName]);
00053     return true;
00054 }
00055 
00056 bool getStringParam(std::string paramName, std::string& value)
00057 {
00058     value = boost::get<std::string>(testParamMap[paramName]);
00059     return true;
00060 }
00061 
00062 bool getDoubleParam(std::string paramName, double& value)
00063 {
00064     value = boost::get<double>(testParamMap[paramName]);
00065     return true;
00066 }
00067 
00068 bool getIntParam(std::string paramName, int& value)
00069 {
00070     value = boost::get<int>(testParamMap[paramName]);
00071     return true;
00072 }
00073 
00074 void setBoolParam(std::string paramName, bool value)
00075 {
00076     testParamMap[paramName] = value;
00077 }
00078 
00079 void setStringParam(std::string paramName, std::string value)
00080 {
00081     testParamMap[paramName] = value;
00082 }
00083 
00084 void setDoubleParam(std::string paramName, double value)
00085 {
00086     testParamMap[paramName] = value;
00087 }
00088 
00089 void setIntParam(std::string paramName, int value)
00090 {
00091     testParamMap[paramName] = value;
00092 }
00093 
00094 class JointControlManagerTest : public ::testing::Test
00095 {
00096 protected:
00097     virtual void SetUp()
00098     {
00099         // Initialize objects
00100         nodeRegisterManager = boost::make_shared<NodeRegisterManager>();
00101         jointControlManagerMap.clear();
00102         timeLimit = 0.05;
00103         ros::Time::init();
00104         logicPowerState.data = r2_msgs::PowerState::LOGIC_POWER;
00105         offPowerState.data   = r2_msgs::PowerState::POWER_OFF;
00106         motorPowerState.data = r2_msgs::PowerState::MOTOR_POWER;
00107 
00108         // Load RobotInstance
00109         packagePath      = ros::package::getPath("robot_instance");
00110         configPath       = packagePath + "/test/config";
00111         configSafetyPath = packagePath + "/test/configSafety";
00112         fileName         = "TestRobotInstance.xml";
00113         instance         = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName);
00114 
00115         // Set up function pointers
00116         testLiveCoeffs.clear();
00117         io.getRegisterFile = getRegisterFile;
00118         io.getControlFile  = getControlFile;
00119         io.hasLiveCoeff    = hasLiveCoeff;
00120         io.getLiveCoeff    = getLiveCoeff;
00121         io.setLiveCoeff    = setLiveCoeff;
00122         io.hasParam         = hasParam;
00123         io.getBoolParam     = getBoolParam;
00124         io.getIntParam      = getIntParam;
00125         io.getDoubleParam   = getDoubleParam;
00126         io.getStringParam   = getStringParam;
00127         io.setBoolParam     = setBoolParam;
00128         io.setIntParam      = setIntParam;
00129         io.setDoubleParam   = setDoubleParam;
00130         io.setStringParam   = setStringParam;
00131 
00132         // Initialize nodeRegisterManager and jointControlManagerMap
00133         for (unsigned int i = 0; i < instance->mechanisms.size(); i++)
00134         {
00135             mechanism = instance->mechanisms[i];
00136             ASSERT_NO_THROW(jointControlManagerMap[mechanism] = JointControlManagerFactory::generate(mechanism, io, timeLimit, nodeRegisterManager));
00137 
00138             // Fake no FPGA faults
00139             nodeRegisterManager->nodeRegisterMap[mechanism].status[mechanism + "/StatReg1"].registerValue = 0x9000; // 1001 0000 0000 0000
00140 
00141             if (mechanism == "r2/right_arm/wrist")
00142             {
00143                 // Fake live coeffs
00144                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "PitchLimit"),            0);
00145                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "YawLimit"),              0);
00146                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "LittlesideSliderLimit"), 0);
00147                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "ThumbsideSliderLimit"),  0);
00148                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "SensorError"),           0);
00149                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "SliderDiffError"),       0);
00150             }
00151         }
00152 
00153     }
00154 
00155     virtual void TearDown()
00156     {
00157     }
00158 
00159     std::string                              packagePath;
00160     std::string                              configPath, configSafetyPath, fileName;
00161     NodeRegisterManagerPtr                   nodeRegisterManager;
00162     std::string                              mechanism;
00163     JointControlManagerMap                   jointControlManagerMap;
00164     r2_msgs::JointControlData    commandMsg;
00165     double                                   timeLimit;
00166     JointControlCommonInterface::IoFunctions io;
00167     r2_msgs::PowerState          logicPowerState;
00168     r2_msgs::PowerState          offPowerState;
00169     r2_msgs::PowerState          motorPowerState;
00170 };
00171 
00172 TEST_F(JointControlManagerTest, InitialControlModeStates)
00173 {
00174     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00175     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00176     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00177     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00178     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00179     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00180 
00181     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00182     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00183     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00184     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00185     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00186     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00187 }
00188 
00189 TEST_F(JointControlManagerTest, CheckFpgaFaults)
00190 {
00191     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00192     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00193     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00194     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00195 
00196     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00197     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00198     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00199     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00200 
00201     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x0000; // 0000 0000 0000 0000
00202     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x0000; // 0000 0000 0000 0000
00203     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x0000; // 0000 0000 0000 0000
00204     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x0000; // 0000 0000 0000 0000
00205 
00206     jointControlManagerMap["r2/left_leg/joint0"]->verifyStates();
00207     jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates();
00208     jointControlManagerMap["r2/left_arm/joint0"]->verifyStates();
00209     jointControlManagerMap["r2/neck/joint0"]->verifyStates();
00210 
00211     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED,    jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00212     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED,    jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00213     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED,    jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00214     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED,    jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00215     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00216     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00217     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00218     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00219 
00220 }
00221 
00222 TEST_F(JointControlManagerTest, Off)
00223 {
00224     // off with logic power off
00225     commandMsg.controlMode.state = r2_msgs::JointControlMode::OFF;
00226     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00227     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00228     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00229     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00230     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00231     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00232 
00233 
00234     jointControlManagerMap["r2/left_leg/joint0"]->setPowerState(offPowerState);
00235     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setPowerState(offPowerState);
00236     jointControlManagerMap["r2/left_arm/joint0"]->setPowerState(offPowerState);
00237     jointControlManagerMap["r2/neck/joint0"]->setPowerState(offPowerState);
00238     jointControlManagerMap["r2/right_arm/wrist"]->setPowerState(offPowerState);
00239     jointControlManagerMap["r2/right_arm/hand/index"]->setPowerState(offPowerState);
00240 
00241     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00242     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00243     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00244     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00245     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00246     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00247 
00248     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00249     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00250     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00251     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00252     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00253     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00254 
00255     // wrists and fingers should reset their calibration mode on logic power off
00256     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().calibrationMode.state);
00257     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().calibrationMode.state);
00258 
00259     // Back into BOOTLOADER with logic Power ON(changed with #RDEV-1239)
00260     commandMsg.controlMode.state = r2_msgs::JointControlMode::BOOTLOADER;
00261     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00262     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00263     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00264     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00265     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00266     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00267 
00268     jointControlManagerMap["r2/left_leg/joint0"]->setPowerState(logicPowerState);
00269     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setPowerState(logicPowerState);
00270     jointControlManagerMap["r2/left_arm/joint0"]->setPowerState(logicPowerState);
00271     jointControlManagerMap["r2/neck/joint0"]->setPowerState(logicPowerState);
00272     jointControlManagerMap["r2/right_arm/wrist"]->setPowerState(logicPowerState);
00273     jointControlManagerMap["r2/right_arm/hand/index"]->setPowerState(logicPowerState);
00274 
00275     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00276     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00277     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00278     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00279     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00280     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00281 
00282     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00283     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00284     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00285     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00286     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00287     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00288 
00289     // off with logic power on
00290     commandMsg.controlMode.state = r2_msgs::JointControlMode::OFF;
00291     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00292     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00293     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00294     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00295     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00296     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00297 
00298     jointControlManagerMap["r2/left_leg/joint0"]->setPowerState(logicPowerState);
00299     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setPowerState(logicPowerState);
00300     jointControlManagerMap["r2/left_arm/joint0"]->setPowerState(logicPowerState);
00301     jointControlManagerMap["r2/neck/joint0"]->setPowerState(logicPowerState);
00302     jointControlManagerMap["r2/right_arm/wrist"]->setPowerState(logicPowerState);
00303     jointControlManagerMap["r2/right_arm/hand/index"]->setPowerState(logicPowerState);
00304 
00305     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00306     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00307     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00308     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00309 
00310     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00311     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9010; // 1001 0000 0001 0000
00312     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00313     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9010; // 1001 0000 0001 0000
00314 
00315     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00316     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00317     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00318     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00319     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00320 
00321     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00322     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00323     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00324     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00325     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00326 
00327     // DISABLE calibration in OFF should automatically transition to PARK for wrists
00328     std::cout << "coeff name: " << StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode") << std::endl;
00329     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode"), r2_msgs::JointControlCalibrationMode::DISABLE);
00330     jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state;
00331     jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state;
00332     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00333     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00334 }
00335 
00336 TEST_F(JointControlManagerTest, Park)
00337 {
00338     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00339     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00340     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00341     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00342     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00343     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00344 
00345     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00346     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00347     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00348     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00349     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00350     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00351 
00352     // Can't PARK from BOOTLOADER
00353     commandMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00354     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00355     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00356     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00357     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00358     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00359     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00360 
00361     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00362     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00363     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00364     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00365     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00366     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00367 
00368     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00369     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00370     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00371     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00372     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00373     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00374 
00375     // Can PARK through OFF
00376     commandMsg.controlMode.state = r2_msgs::JointControlMode::OFF;
00377     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00378     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00379     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00380     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00381     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00382     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00383 
00384     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00385     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00386     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00387     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00388 
00389     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00390     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9010; // 1001 0000 0001 0000
00391     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00392     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9010; // 1001 0000 0001 0000
00393 
00394     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00395     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00396     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00397     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00398     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00399     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00400 
00401     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00402     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00403     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00404     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00405     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00406     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00407 
00408     commandMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00409     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00410     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00411     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00412     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00413     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00414     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00415 
00416     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00417     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9011; // 1001 0000 0001 0001
00418     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00419     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9011; // 1001 0000 0001 0001
00420 
00421     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00422     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00423     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00424     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00425     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00426     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00427 
00428     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00429     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00430     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00431     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00432     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00433     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00434 
00435     //automatic transition to enable calibration mode on wrists
00436     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().calibrationMode.state);
00437 
00438     // Can't go back into BOOTLOADER
00439     commandMsg.controlMode.state = r2_msgs::JointControlMode::BOOTLOADER;
00440     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00441     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00442     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00443     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00444     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00445     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00446 
00447     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00448     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00449     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00450     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00451     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00452     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00453 
00454     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00455     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00456     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00457     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00458     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00459     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00460 
00461     // Cant get back to OFF
00462     commandMsg.controlMode.state = r2_msgs::JointControlMode::OFF;
00463     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00464     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00465     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00466     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00467     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00468     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00469 
00470     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00471     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00472     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00473     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00474     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00475     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00476 
00477     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00478     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00479     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00480     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00481     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00482     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00483 }
00484 
00485 TEST_F(JointControlManagerTest, Neutral)
00486 {
00487     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00488     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00489     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00490     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00491     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00492     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00493 
00494     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00495     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00496     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00497     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00498     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00499     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00500 
00501     // Can't NEUTRAL from BOOTLOADER (SEA)
00502     commandMsg.controlMode.state = r2_msgs::JointControlMode::NEUTRAL;
00503     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00504     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00505     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00506     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00507     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00508     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00509 
00510     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00511     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00512     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00513     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00514     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00515     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00516 
00517     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00518     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00519     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00520     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00521     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00522     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00523 
00524     // Can NEUTRAL through OFF
00525     commandMsg.controlMode.state = r2_msgs::JointControlMode::OFF;
00526     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00527     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00528     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00529     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00530     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00531     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00532 
00533     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00534     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00535     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00536     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00537 
00538     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00539     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9010; // 1001 0000 0001 0000
00540     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00541     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9010; // 1001 0000 0001 0000
00542 
00543     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00544     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00545     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00546     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00547     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00548     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00549 
00550     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00551     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00552     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00553     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00554     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00555     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00556 
00557     commandMsg.controlMode.state = r2_msgs::JointControlMode::NEUTRAL;
00558     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00559     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00560     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00561     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00562     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00563     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00564 
00565     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9012; // 1001 0000 0001 0010
00566     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9012; // 1001 0000 0001 0010
00567     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9012; // 1001 0000 0001 0010
00568     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9012; // 1001 0000 0001 0010
00569 
00570     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00571     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00572     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00573     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00574     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00575     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00576 
00577     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00578     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00579     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00580     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00581     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00582     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00583 
00584     //automatic transition to enable calibration mode on wrists
00585     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().calibrationMode.state);
00586 
00587     // Can go to PARK
00588     commandMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00589     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00590     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00591     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00592     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00593     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00594     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00595 
00596     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00597     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9011; // 1001 0000 0001 0001
00598     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00599     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9011; // 1001 0000 0001 0001
00600 
00601     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00602     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00603     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00604     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00605     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00606     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00607 
00608     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00609     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00610     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00611     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00612     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00613     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00614 
00615     // Can NEUTRAL from PARK
00616     commandMsg.controlMode.state = r2_msgs::JointControlMode::NEUTRAL;
00617     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00618     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00619     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00620     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00621     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00622     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00623 
00624     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9012; // 1001 0000 0001 0010
00625     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9012; // 1001 0000 0001 0010
00626     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9012; // 1001 0000 0001 0010
00627     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9012; // 1001 0000 0001 0010
00628 
00629     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00630     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00631     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00632     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00633     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00634     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00635 
00636     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00637     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00638     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00639     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00640     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00641     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00642 
00643     // Can't OFF from NEUTRAL
00644     commandMsg.controlMode.state = r2_msgs::JointControlMode::OFF;
00645     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00646     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00647     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00648     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00649     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00650     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00651 
00652     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00653     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00654     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00655     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00656     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00657     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00658 
00659     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00660     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00661     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00662     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00663     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00664     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00665 }
00666 
00667 TEST_F(JointControlManagerTest, Drive)
00668 {
00669     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00670     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00671     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00672     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00673     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00674     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00675 
00676     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00677     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00678     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00679     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00680     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00681     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00682 
00683     // Can't DRIVE from BOOTLOADER
00684     commandMsg.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00685     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00686     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00687     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00688     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00689     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00690     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00691 
00692     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00693     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00694     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00695     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00696     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00697     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00698 
00699     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00700     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00701     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00702     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00703     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00704     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00705 
00706     commandMsg.controlMode.state = r2_msgs::JointControlMode::OFF;
00707     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00708     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00709     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00710     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00711     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00712     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00713 
00714     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00715     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00716     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00717     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00718 
00719     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00720     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9010; // 1001 0000 0001 0000
00721     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00722     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9010; // 1001 0000 0001 0000
00723 
00724     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00725     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00726     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00727     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00728     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00729     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00730 
00731     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00732     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00733     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00734     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00735     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00736     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00737 
00738     // Can't DRIVE from OFF
00739     commandMsg.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00740     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00741     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00742     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00743     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00744     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00745     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00746 
00747     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00748     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00749     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00750     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00751     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00752     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00753 
00754     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00755     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00756     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00757     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00758     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00759     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00760 
00761     commandMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00762     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00763     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00764     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00765     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00766     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00767     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00768 
00769     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00770     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9011; // 1001 0000 0001 0001
00771     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00772     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9011; // 1001 0000 0001 0001
00773 
00774     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00775     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00776     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00777     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00778     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00779     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00780 
00781     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00782     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00783     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00784     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00785     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00786     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00787 
00788     //automatic transition to enable calibration mode on wrists
00789     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().calibrationMode.state);
00790     io.setLiveCoeff("r2/right_arm/wrist/CalibrationMode", r2_msgs::JointControlCalibrationMode::DISABLE);
00791     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().calibrationMode.state);
00792 
00793     //fake tube tare completed on fingers
00794     io.setLiveCoeff("r2/right_arm/hand/index/CalibrationMode", r2_msgs::JointControlCalibrationMode::DISABLE);
00795     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().calibrationMode.state);
00796 
00797     // Can't DRIVE without MOTORPOWER
00798     commandMsg.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00799     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00800     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00801     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00802     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00803     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00804     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00805 
00806     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00807     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00808     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00809     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00810     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00811     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00812 
00813     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00814     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00815     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00816     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00817     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00818     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00819 
00820     jointControlManagerMap["r2/left_leg/joint0"]->setPowerState(motorPowerState);
00821     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setPowerState(motorPowerState);
00822     jointControlManagerMap["r2/left_arm/joint0"]->setPowerState(motorPowerState);
00823     jointControlManagerMap["r2/neck/joint0"]->setPowerState(motorPowerState);
00824     jointControlManagerMap["r2/right_arm/wrist"]->setPowerState(motorPowerState);
00825     jointControlManagerMap["r2/right_arm/hand/index"]->setPowerState(motorPowerState);
00826 
00827     commandMsg.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00828     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00829     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00830     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00831     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00832     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00833     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00834 
00835     // Can DRIVE through PARK
00836     commandMsg.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00837     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00838     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00839     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00840     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00841     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00842     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00843 
00844     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9013; // 1001 0000 0001 0011
00845     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9013; // 1001 0000 0001 0011
00846     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9013; // 1001 0000 0001 0011
00847     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9013; // 1001 0000 0001 0011
00848 
00849     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00850     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00851     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00852     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00853     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00854     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00855 
00856     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00857     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00858     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00859     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00860     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00861     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00862 
00863     // Can PARK from DRIVE
00864     commandMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00865     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00866     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00867     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00868     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00869     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00870     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00871 
00872     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00873     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9011; // 1001 0000 0001 0001
00874     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00875     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9011; // 1001 0000 0001 0001
00876 
00877     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00878     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00879     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00880     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00881     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00882     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00883 
00884     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00885     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00886     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00887     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00888     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00889     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00890 
00891     // Can't NEUTRAL from DRIVE
00892     commandMsg.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00893     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00894     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00895     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00896     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00897     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00898     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00899 
00900     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9013; // 1001 0000 0001 0011
00901     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9013; // 1001 0000 0001 0011
00902     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9013; // 1001 0000 0001 0011
00903     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9013; // 1001 0000 0001 0011
00904 
00905     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00906     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00907     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00908     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00909     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00910     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00911 
00912     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00913     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00914     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00915     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00916     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00917     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00918 
00919     commandMsg.controlMode.state = r2_msgs::JointControlMode::NEUTRAL;
00920     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00921     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00922     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00923     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00924     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00925     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00926 
00927     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00928     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00929     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00930     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00931     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00932     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00933 
00934     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00935     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00936     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00937     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00938     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00939     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00940 
00941     // Can't OFF from DRIVE
00942     commandMsg.controlMode.state = r2_msgs::JointControlMode::OFF;
00943     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00944     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00945     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00946     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
00947     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
00948     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
00949 
00950     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
00951     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
00952     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
00953     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
00954     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
00955     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
00956 
00957     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00958     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00959     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00960     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00961     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00962     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00963 }
00964 
00965 TEST_F(JointControlManagerTest, ClearFault)
00966 {
00967 
00968     //fake actual = drive
00969     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00970     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00971     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00972     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00973     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9053; // 1001 0000 0101 0011
00974     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9053; // 1001 0000 0101 0011
00975     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9053; // 1001 0000 0101 0011
00976     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9053; // 1001 0000 0101 0011
00977     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode"),          r2_msgs::JointControlMode::DRIVE);
00978     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode"),     r2_msgs::JointControlMode::DRIVE);
00979     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode"),      r2_msgs::JointControlCalibrationMode::DISABLE);
00980     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CalibrationMode"), r2_msgs::JointControlCalibrationMode::DISABLE);
00981     jointControlManagerMap["r2/left_leg/joint0"]->setPowerState(motorPowerState);
00982     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setPowerState(motorPowerState);
00983     jointControlManagerMap["r2/left_arm/joint0"]->setPowerState(motorPowerState);
00984     jointControlManagerMap["r2/neck/joint0"]->setPowerState(motorPowerState);
00985     jointControlManagerMap["r2/right_arm/wrist"]->setPowerState(motorPowerState);
00986     jointControlManagerMap["r2/right_arm/hand/index"]->setPowerState(motorPowerState);
00987 
00988     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
00989     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
00990     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
00991     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
00992     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
00993     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
00994 
00995     commandMsg.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00996     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
00997     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
00998     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
00999     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01000     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01001     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01002 
01003     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
01004     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
01005     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
01006     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
01007     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
01008     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
01009 
01010     // Fake coeffs loaded and joint faulted
01011     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0180; // 0000 0001 1000 0000
01012     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0180; // 0000 0001 1000 0000
01013     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0180; // 0000 0001 1000 0000
01014     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0180; // 0000 0001 1000 0000
01015     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "PitchLimit"), 1);
01016 
01017     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
01018     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
01019     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
01020     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
01021     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
01022     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
01023 
01024     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
01025     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
01026     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
01027     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
01028     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
01029     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
01030 
01031     EXPECT_FALSE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01032     EXPECT_FALSE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01033     EXPECT_FALSE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01034     EXPECT_FALSE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01035     EXPECT_FALSE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01036     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01037 
01038     commandMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
01039     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01040     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01041     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01042     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01043     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01044     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01045 
01046     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
01047     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
01048     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
01049     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
01050     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
01051     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
01052 
01053     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
01054     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
01055     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
01056     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
01057     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
01058     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
01059 
01060     commandMsg.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::ENABLE;
01061     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01062     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01063     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01064     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01065     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01066     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01067 
01068     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x01C0; // 0000 0001 1100 0000
01069     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x01C0; // 0000 0001 1100 0000
01070     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x01C0; // 0000 0001 1100 0000
01071     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x01C0; // 0000 0001 1100 0000
01072 
01073     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().clearFaultMode.state);
01074     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().clearFaultMode.state);
01075     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().clearFaultMode.state);
01076     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().clearFaultMode.state);
01077     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().clearFaultMode.state);
01078     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().clearFaultMode.state);
01079 
01080     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().clearFaultMode.state);
01081     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().clearFaultMode.state);
01082     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().clearFaultMode.state);
01083     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().clearFaultMode.state);
01084     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().clearFaultMode.state);
01085     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().clearFaultMode.state);
01086 
01087     commandMsg.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::DISABLE;
01088     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01089     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01090     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01091     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01092     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01093     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01094 
01095     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
01096     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9011; // 1001 0000 0001 0001
01097     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
01098     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9011; // 1001 0000 0001 0001
01099     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0000; // 0000 0000 0000 0000
01100     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0000; // 0000 0000 0000 0000
01101     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0000; // 0000 0000 0000 0000
01102     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0000; // 0000 0000 0000 0000
01103     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "PitchLimit"), 0);
01104 
01105     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().clearFaultMode.state);
01106     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().clearFaultMode.state);
01107     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().clearFaultMode.state);
01108     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().clearFaultMode.state);
01109     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().clearFaultMode.state);
01110     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().clearFaultMode.state);
01111 
01112     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().clearFaultMode.state);
01113     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().clearFaultMode.state);
01114     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().clearFaultMode.state);
01115     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().clearFaultMode.state);
01116     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().clearFaultMode.state);
01117     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().clearFaultMode.state);
01118 
01119     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
01120     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
01121     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
01122     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
01123     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
01124     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
01125 
01126     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
01127     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
01128     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
01129     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
01130     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
01131     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
01132 }
01133 
01134 TEST_F(JointControlManagerTest, InitialCommandModeStates)
01135 {
01136     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01137     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01138     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01139     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01140     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01141     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01142 
01143     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01144     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01145     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01146     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01147     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01148     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01149 }
01150 
01151 TEST_F(JointControlManagerTest, MotcomMode)
01152 {
01153     // Motcom
01154     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::MOTCOM;
01155     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01156     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01157     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01158     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01159     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01160     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01161 
01162     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01163     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01164     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01165     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01166     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01167     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01168 
01169     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01170     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01171     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01172     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01173     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01174     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01175 }
01176 
01177 TEST_F(JointControlManagerTest, ActuatorMode)
01178 {
01179     // Motcom
01180     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::ACTUATOR;
01181     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01182     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01183     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01184     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01185     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01186     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01187 
01188     //Actuator mode only valid on wrist and fingers
01189     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01190     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01191     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01192     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01193     EXPECT_EQ(r2_msgs::JointControlCommandMode::ACTUATOR, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01194     EXPECT_EQ(r2_msgs::JointControlCommandMode::ACTUATOR, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01195 
01196     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01197     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01198     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01199     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01200     EXPECT_EQ(r2_msgs::JointControlCommandMode::ACTUATOR, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01201     EXPECT_EQ(r2_msgs::JointControlCommandMode::ACTUATOR, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01202 }
01203 
01204 TEST_F(JointControlManagerTest, MultiLoopStep)
01205 {
01206     // MultiLoopStep w/out coeffs loaded should fail
01207     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
01208     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01209     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01210     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01211     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01212     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01213     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01214 
01215     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01216     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01217     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01218     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01219     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01220     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01221 
01222     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01223     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01224     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01225     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01226     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01227     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01228 
01229     // Fake coeffs loaded and MultiLoop
01230     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
01231     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0103; // 0000 0001 0000 0011
01232     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
01233     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0103; // 0000 0001 0000 0011
01234 
01235     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().coeffState.state);
01236     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().coeffState.state);
01237     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().coeffState.state);
01238     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/neck/joint0"]->getActualStates().coeffState.state);
01239     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().coeffState.state);
01240     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().coeffState.state);
01241 
01242     // MultiLoopStep
01243     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
01244     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01245     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01246     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01247     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01248     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01249     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01250 
01251     // Fake MotComSource == 1
01252     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01253     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
01254     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01255     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
01256 
01257     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01258     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01259     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01260     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01261     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01262     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01263 
01264     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01265     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01266     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01267     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01268     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01269     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01270 
01271     // MotCom
01272     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::MOTCOM;
01273     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01274     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01275     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01276     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01277     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01278     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01279 
01280     // UnFake MotComSource == 1
01281     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
01282     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9000; // 1001 0000 0000 0000
01283     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
01284     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9000; // 1001 0000 0000 0000
01285 
01286     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01287     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01288     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01289     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01290     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01291     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01292 
01293     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01294     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01295     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01296     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01297     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01298     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01299 
01300     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "MotComSource"));
01301     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotComSource"));
01302     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "MotComSource"));
01303     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0",              "MotComSource"));
01304 
01305     // MultiLoopStep
01306     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
01307     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01308     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01309     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01310     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01311     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01312     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01313 
01314     // Fake MotComSource == 1
01315     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01316     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
01317     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01318     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
01319 
01320     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01321     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01322     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01323     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01324     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01325     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01326 
01327     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01328     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01329     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01330     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01331     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01332     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01333 }
01334 
01335 TEST_F(JointControlManagerTest, StallMode)
01336 {
01337     // Stallmode w/out coeffs loaded should fail
01338     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::STALLMODE;
01339     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01340     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01341     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01342     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01343     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01344     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01345 
01346     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01347     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01348     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01349     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01350     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01351     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01352 
01353     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01354     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01355     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01356     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01357     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01358     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01359 
01360     // Fake coeffs loaded and StallMode
01361     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0101; // 0000 0001 0000 0001
01362     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0101; // 0000 0001 0000 0001
01363     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0101; // 0000 0001 0000 0001
01364     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0101; // 0000 0001 0000 0001
01365 
01366     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().coeffState.state);
01367     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().coeffState.state);
01368     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().coeffState.state);
01369     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/neck/joint0"]->getActualStates().coeffState.state);
01370     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().coeffState.state);
01371     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().coeffState.state);
01372 
01373     // StallMode
01374     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::STALLMODE;
01375     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01376     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01377     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01378     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01379     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01380     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01381 
01382     // Fake MotComSource == 1
01383     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01384     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
01385     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01386     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
01387 
01388     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01389     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01390     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01391     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01392     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01393     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01394 
01395     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01396     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01397     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01398     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01399     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01400     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01401 }
01402 
01403 TEST_F(JointControlManagerTest, MultiLoopSmooth)
01404 {
01405     // MultiLoopSmooth w/out coeffs loaded should fail
01406     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH;
01407     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01408     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01409     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01410     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01411     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01412     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01413 
01414     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01415     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01416     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01417     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01418     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01419     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01420 
01421     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01422     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01423     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01424     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01425     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01426     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01427 
01428     // Fake coeffs loaded and MultiLoop
01429     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
01430     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0103; // 0000 0001 0000 0011
01431     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
01432     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0103; // 0000 0001 0000 0011
01433 
01434     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().coeffState.state);
01435     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().coeffState.state);
01436     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().coeffState.state);
01437     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/neck/joint0"]->getActualStates().coeffState.state);
01438     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().coeffState.state);
01439     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().coeffState.state);
01440 
01441     // MultiLoopSmooth
01442     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH;
01443     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01444     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01445     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01446     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01447     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01448     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01449 
01450     // Fake MotComSource == 1
01451     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01452     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
01453     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01454     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
01455 
01456     // Fake PosComVelocity == 1
01457     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue = 0x0100; // 0000 0001 0000 0000
01458     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue = 0x0100; // 0000 0001 0000 0000
01459     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue         = 0x0100; // 0000 0001 0000 0000
01460 
01461     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01462     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP,   jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01463     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01464     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01465     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01466     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01467 
01468     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01469     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP,   jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01470     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01471     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01472     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01473     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01474 }
01475 
01476 TEST_F(JointControlManagerTest, InitialCalibrationModeStates)
01477 {
01478     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().calibrationMode.state);
01479     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().calibrationMode.state);
01480     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().calibrationMode.state);
01481     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().calibrationMode.state);
01482     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().calibrationMode.state);
01483     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().calibrationMode.state);
01484 
01485     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().calibrationMode.state);
01486     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().calibrationMode.state);
01487     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().calibrationMode.state);
01488     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().calibrationMode.state);
01489     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().calibrationMode.state);
01490     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().calibrationMode.state);
01491 }
01492 
01493 TEST_F(JointControlManagerTest, EnableCalibrationMode)
01494 {
01495     commandMsg.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
01496     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01497     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01498     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01499     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01500     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01501     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01502 
01503     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x8000; // 1000 0000 0000 0000
01504     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x8000; // 1000 0000 0000 0000
01505     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x8000; // 1000 0000 0000 0000
01506     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x8000; // 1000 0000 0000 0000
01507 
01508     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().calibrationMode.state);
01509     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().calibrationMode.state);
01510     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().calibrationMode.state);
01511     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().calibrationMode.state);
01512     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().calibrationMode.state);
01513     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().calibrationMode.state);
01514 
01515     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().calibrationMode.state);
01516     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().calibrationMode.state);
01517     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().calibrationMode.state);
01518     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().calibrationMode.state);
01519     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().calibrationMode.state);
01520     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().calibrationMode.state);
01521 }
01522 
01523 TEST_F(JointControlManagerTest, InitialClearFaultModeStates)
01524 {
01525     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().clearFaultMode.state);
01526     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().clearFaultMode.state);
01527     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().clearFaultMode.state);
01528     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().clearFaultMode.state);
01529     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().clearFaultMode.state);
01530     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().clearFaultMode.state);
01531 
01532     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().clearFaultMode.state);
01533     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().clearFaultMode.state);
01534     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().clearFaultMode.state);
01535     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().clearFaultMode.state);
01536     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().clearFaultMode.state);
01537     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().clearFaultMode.state);
01538 }
01539 
01540 TEST_F(JointControlManagerTest, EnableClearFaultMode)
01541 {
01542     commandMsg.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::ENABLE;
01543     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01544     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01545     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01546     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01547     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01548     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01549 
01550     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0040; // 0000 0000 0100 0000
01551     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0040; // 0000 0000 0100 0000
01552     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0040; // 0000 0000 0100 0000
01553     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0040; // 0000 0000 0100 0000
01554 
01555     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().clearFaultMode.state);
01556     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().clearFaultMode.state);
01557     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().clearFaultMode.state);
01558     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().clearFaultMode.state);
01559     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().clearFaultMode.state);
01560     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().clearFaultMode.state);
01561 
01562     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().clearFaultMode.state);
01563     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().clearFaultMode.state);
01564     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().clearFaultMode.state);
01565     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().clearFaultMode.state);
01566     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().clearFaultMode.state);
01567     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().clearFaultMode.state);
01568 }
01569 
01570 TEST_F(JointControlManagerTest, VerifyControlModeState)
01571 {
01572     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01573     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01574     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01575     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01576     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01577     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01578 
01579     commandMsg.controlMode.state = r2_msgs::JointControlMode::OFF;
01580     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01581     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01582     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01583     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01584     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01585     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01586 
01587     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
01588     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
01589     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
01590     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
01591 
01592     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
01593     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9010; // 1001 0000 0001 0000
01594     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
01595     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9010; // 1001 0000 0001 0000
01596 
01597     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
01598     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
01599     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
01600     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
01601     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
01602     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
01603 
01604     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
01605     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
01606     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
01607     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
01608     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
01609     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
01610 
01611     usleep(timeLimit * 1000000);
01612     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01613     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01614     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01615     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01616     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01617     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01618 
01619     commandMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
01620     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01621     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01622     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01623     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01624     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01625     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01626 
01627     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().controlMode.state);
01628     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().controlMode.state);
01629     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().controlMode.state);
01630     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().controlMode.state);
01631     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().controlMode.state);
01632     EXPECT_EQ(r2_msgs::JointControlMode::PARK, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().controlMode.state);
01633 
01634     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode"),          r2_msgs::JointControlMode::OFF);
01635     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode"),     r2_msgs::JointControlMode::OFF);
01636 
01637     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().controlMode.state);
01638     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().controlMode.state);
01639     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().controlMode.state);
01640     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/neck/joint0"]->getActualStates().controlMode.state);
01641     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().controlMode.state);
01642     EXPECT_EQ(r2_msgs::JointControlMode::OFF, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().controlMode.state);
01643 
01644     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01645     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01646     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01647     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01648     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01649     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01650 
01651     usleep(timeLimit * 1000000);
01652     EXPECT_FALSE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01653     EXPECT_FALSE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01654     EXPECT_FALSE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01655     EXPECT_FALSE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01656     EXPECT_FALSE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01657     EXPECT_FALSE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01658 }
01659 
01660 TEST_F(JointControlManagerTest, VerifyCommandModeState)
01661 {
01662     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01663     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01664     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01665     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01666     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01667     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01668 
01669     // Fake coeffs loaded
01670     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
01671     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0103; // 0000 0001 0000 0011
01672     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
01673     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0103; // 0000 0001 0000 0011
01674 
01675     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,          jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01676     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,          jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01677     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,          jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01678     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,          jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01679     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01680     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01681 
01682     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,          jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01683     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,          jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01684     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,          jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01685     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,          jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01686     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01687     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01688 
01689     usleep(timeLimit * 1000000);
01690     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01691     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01692     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01693     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01694     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01695     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01696 
01697     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
01698     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01699     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01700     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01701     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01702     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01703     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01704 
01705     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01706     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP,   jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01707     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01708     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01709     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01710     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01711 
01712     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CommandMode"),          r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH);
01713     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CommandMode"),     r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH);
01714 
01715     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,      jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01716     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,        jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01717     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,      jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01718     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM,      jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01719     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH,      jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01720     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH,      jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01721 
01722     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01723     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01724     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01725     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01726     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01727     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01728 
01729     usleep(timeLimit * 1000000);
01730     EXPECT_FALSE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01731     EXPECT_FALSE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01732     EXPECT_FALSE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01733     EXPECT_FALSE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01734     EXPECT_FALSE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01735     EXPECT_FALSE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01736 
01737     commandMsg.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH;
01738     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01739     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01740     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01741     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01742     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01743     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01744 
01745     // Fake MotComSource == 1
01746     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01747     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
01748     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
01749     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
01750 
01751     // Fake PosComVelocity == 1
01752     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue = 0x0100; // 0000 0001 0000 0000
01753     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue = 0x0100; // 0000 0001 0000 0000
01754     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue         = 0x0100; // 0000 0001 0000 0000
01755 
01756     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().commandMode.state);
01757     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP,   jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().commandMode.state);
01758     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().commandMode.state);
01759     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().commandMode.state);
01760     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().commandMode.state);
01761     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().commandMode.state);
01762 
01763     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH,      jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().commandMode.state);
01764     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP,        jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().commandMode.state);
01765     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH,      jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().commandMode.state);
01766     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH,      jointControlManagerMap["r2/neck/joint0"]->getActualStates().commandMode.state);
01767     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH,      jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().commandMode.state);
01768     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH,      jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().commandMode.state);
01769 
01770     usleep(timeLimit * 1000000);
01771     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01772     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01773     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01774     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01775     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01776     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01777 }
01778 
01779 TEST_F(JointControlManagerTest, VerifyCalibrationModeState)
01780 {
01781     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01782     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01783     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01784     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01785     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01786     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01787 
01788     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().calibrationMode.state);
01789     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().calibrationMode.state);
01790     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().calibrationMode.state);
01791     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().calibrationMode.state);
01792     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().calibrationMode.state);
01793     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().calibrationMode.state);
01794 
01795     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE,      jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().calibrationMode.state);
01796     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE,      jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().calibrationMode.state);
01797     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE,      jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().calibrationMode.state);
01798     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE,      jointControlManagerMap["r2/neck/joint0"]->getActualStates().calibrationMode.state);
01799     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().calibrationMode.state);
01800     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().calibrationMode.state);
01801 
01802     commandMsg.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
01803     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01804     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01805     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01806     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01807     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01808     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01809 
01810     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().calibrationMode.state);
01811     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().calibrationMode.state);
01812     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().calibrationMode.state);
01813     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().calibrationMode.state);
01814     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().calibrationMode.state);
01815     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().calibrationMode.state);
01816 
01817     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode"),         r2_msgs::JointControlCalibrationMode::DISABLE);
01818     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CalibrationMode"),    r2_msgs::JointControlCalibrationMode::DISABLE);
01819 
01820     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().calibrationMode.state);
01821     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().calibrationMode.state);
01822     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().calibrationMode.state);
01823     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().calibrationMode.state);
01824     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().calibrationMode.state);
01825     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().calibrationMode.state);
01826 
01827     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01828     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01829     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01830     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01831     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01832     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01833 
01834     usleep(timeLimit * 1000000);
01835     EXPECT_FALSE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01836     EXPECT_FALSE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01837     EXPECT_FALSE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01838     EXPECT_FALSE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01839     EXPECT_FALSE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01840     EXPECT_FALSE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01841 
01842     commandMsg.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
01843     jointControlManagerMap["r2/left_leg/joint0"]->setCommandStates(commandMsg);
01844     jointControlManagerMap["r2/right_leg/gripper/joint0"]->setCommandStates(commandMsg);
01845     jointControlManagerMap["r2/left_arm/joint0"]->setCommandStates(commandMsg);
01846     jointControlManagerMap["r2/neck/joint0"]->setCommandStates(commandMsg);
01847     jointControlManagerMap["r2/right_arm/wrist"]->setCommandStates(commandMsg);
01848     jointControlManagerMap["r2/right_arm/hand/index"]->setCommandStates(commandMsg);
01849 
01850     // Fake CalibrationMode == 1
01851     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x8000; // 1000 0000 0000 0000
01852     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x8000; // 1000 0000 0000 0000
01853     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x8000; // 1000 0000 0000 0000
01854     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x8000; // 1000 0000 0000 000
01855 
01856     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_leg/joint0"]->getCommandStates().calibrationMode.state);
01857     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getCommandStates().calibrationMode.state);
01858     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_arm/joint0"]->getCommandStates().calibrationMode.state);
01859     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/neck/joint0"]->getCommandStates().calibrationMode.state);
01860     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/wrist"]->getCommandStates().calibrationMode.state);
01861     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getCommandStates().calibrationMode.state);
01862 
01863     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_leg/joint0"]->getActualStates().calibrationMode.state);
01864     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_leg/gripper/joint0"]->getActualStates().calibrationMode.state);
01865     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/left_arm/joint0"]->getActualStates().calibrationMode.state);
01866     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/neck/joint0"]->getActualStates().calibrationMode.state);
01867     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/wrist"]->getActualStates().calibrationMode.state);
01868     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, jointControlManagerMap["r2/right_arm/hand/index"]->getActualStates().calibrationMode.state);
01869 
01870     usleep(timeLimit * 1000000);
01871     EXPECT_TRUE(jointControlManagerMap["r2/left_leg/joint0"]->verifyStates());
01872     EXPECT_TRUE(jointControlManagerMap["r2/right_leg/gripper/joint0"]->verifyStates());
01873     EXPECT_TRUE(jointControlManagerMap["r2/left_arm/joint0"]->verifyStates());
01874     EXPECT_TRUE(jointControlManagerMap["r2/neck/joint0"]->verifyStates());
01875     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/wrist"]->verifyStates());
01876     EXPECT_TRUE(jointControlManagerMap["r2/right_arm/hand/index"]->verifyStates());
01877 }
01878 
01879 int main(int argc, char** argv)
01880 {
01881     testing::InitGoogleTest(&argc, argv);
01882     return RUN_ALL_TESTS();
01883 }

robodyn_mechanisms
Author(s):
autogenerated on Thu Jun 6 2019 21:22:49