JointControlCommand_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/JointControlCommandGripper.h"
00007 #include "robodyn_mechanisms/JointControlCommandSeriesElastic.h"
00008 #include "robodyn_mechanisms/JointControlCommandWrist.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 bool hasParam(std::string paramName)
00045 {
00046     return (testParamMap.find(paramName) != testParamMap.end());
00047 }
00048 
00049 bool getBoolParam(std::string paramName, bool& value)
00050 {
00051     value = boost::get<bool>(testParamMap[paramName]);
00052     return true;
00053 }
00054 
00055 bool getStringParam(std::string paramName, std::string& value)
00056 {
00057     value = boost::get<std::string>(testParamMap[paramName]);
00058     return true;
00059 }
00060 
00061 bool getDoubleParam(std::string paramName, double& value)
00062 {
00063     value = boost::get<double>(testParamMap[paramName]);
00064     return true;
00065 }
00066 
00067 bool getIntParam(std::string paramName, int& value)
00068 {
00069     value = boost::get<int>(testParamMap[paramName]);
00070     return true;
00071 }
00072 
00073 void setBoolParam(std::string paramName, bool value)
00074 {
00075     testParamMap[paramName] = value;
00076 }
00077 
00078 void setStringParam(std::string paramName, std::string value)
00079 {
00080     testParamMap[paramName] = value;
00081 }
00082 
00083 void setDoubleParam(std::string paramName, double value)
00084 {
00085     testParamMap[paramName] = value;
00086 }
00087 
00088 void setIntParam(std::string paramName, int value)
00089 {
00090     testParamMap[paramName] = value;
00091 }
00092 
00093 class JointControlCommandFsmTest : public ::testing::Test
00094 {
00095 protected:
00096     virtual void SetUp()
00097     {
00098         // Initialize objects
00099         nodeRegisterManager = boost::make_shared<NodeRegisterManager>();
00100         jointControlFsmMap.clear();
00101 
00102         // Load RobotInstance
00103         packagePath      = ros::package::getPath("robot_instance");
00104         configPath       = packagePath + "/test/config";
00105         configSafetyPath = packagePath + "/test/configSafety";
00106         fileName         = "TestRobotInstance.xml";
00107         instance         = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName);
00108 
00109         // Set up function pointers
00110         testLiveCoeffs.clear();
00111         io.getRegisterFile = getRegisterFile;
00112         io.getControlFile  = getControlFile;
00113         io.hasLiveCoeff    = hasLiveCoeff;
00114         io.getLiveCoeff    = getLiveCoeff;
00115         io.setLiveCoeff    = setLiveCoeff;
00116         io.hasParam         = hasParam;
00117         io.getBoolParam     = getBoolParam;
00118         io.getIntParam      = getIntParam;
00119         io.getDoubleParam   = getDoubleParam;
00120         io.getStringParam   = getStringParam;
00121         io.setBoolParam     = setBoolParam;
00122         io.setIntParam      = setIntParam;
00123         io.setDoubleParam   = setDoubleParam;
00124         io.setStringParam   = setStringParam;
00125 
00126         // Initialize nodeRegisterManager and jointControlFsmMap
00127         for (unsigned int i = 0; i < instance->mechanisms.size(); i++)
00128         {
00129             mechanism = instance->mechanisms[i];
00130             fileName  = getRegisterFile(mechanism);
00131             jointType = JointControlManagerFactory::Private::getPropertyFromFile(fileName, "NodeType");
00132 
00133             // Simple types
00134             if (jointType == "SeriesElastic")
00135             {
00136                 nodeRegisterManager->addNode(mechanism, fileName);
00137                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlCommandSeriesElastic>(mechanism, io, nodeRegisterManager);
00138             }
00139             else if (jointType == "Rigid")
00140             {
00141                 nodeRegisterManager->addNode(mechanism, fileName);
00142                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlCommandSeriesElastic>(mechanism, io, nodeRegisterManager);
00143             }
00144             else if (jointType == "Gripper")
00145             {
00146                 nodeRegisterManager->addNode(mechanism, fileName);
00147                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlCommandGripper>(mechanism, io, nodeRegisterManager);
00148             }
00149 
00150             // Complex types
00151             else if (jointType == "Wrist")
00152             {
00153                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlCommandWrist>(mechanism, io);
00154             }
00155             else if (jointType == "Thumb")
00156             {
00157                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlCommandFinger>(mechanism, io);
00158             }
00159             else if (jointType == "PrimaryFinger")
00160             {
00161                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlCommandFinger>(mechanism, io);
00162             }
00163             else if (jointType == "SecondaryFingers")
00164             {
00165                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlCommandFinger>(mechanism, io);
00166             }
00167             else
00168             {
00169                 std::stringstream err;
00170                 err << "Unsupported joint type [" << jointType << "] found in ControlFile";
00171                 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointControlCommandFsmTest", log4cpp::Priority::ERROR, err.str());
00172                 ASSERT_TRUE(false);
00173             }
00174         }
00175     }
00176 
00177     virtual void TearDown()
00178     {
00179     }
00180 
00181     std::string                                                                 packagePath;
00182     std::string                                                                 configPath, configSafetyPath, fileName;
00183     NodeRegisterManagerPtr                                                      nodeRegisterManager;
00184     std::string                                                                 jointType;
00185     std::string                                                                 mechanism;
00186     std::map< std::string, boost::shared_ptr<JointControlCommandInterface> > jointControlFsmMap;
00187     JointControlCommonInterface::IoFunctions                                    io;
00188     r2_msgs::JointControlData                                       commandedStates;
00189 };
00190 
00191 TEST_F(JointControlCommandFsmTest, InitialBootloader)
00192 {
00193     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00194     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, commandedStates.controlMode.state);
00195     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00196     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, commandedStates.controlMode.state);
00197     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00198     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, commandedStates.controlMode.state);
00199     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00200     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, commandedStates.controlMode.state);
00201     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00202     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, commandedStates.controlMode.state);
00203     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00204     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, commandedStates.controlMode.state);
00205 
00206     //constants
00207     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "InitAPS2toAPS1"));
00208 
00209     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "CrabEnable"));
00210     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "AtiEnable"));
00211 
00212     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "InitAPS2toAPS1"));
00213 
00214     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0", "InitAPS2toAPS1"));
00215 
00216     //variables
00217     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BootEnable"));
00218     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BridgeEnable"));
00219     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "MotorEnable"));
00220 
00221     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BootEnable"));
00222     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BridgeEnable"));
00223     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotorEnable"));
00224 
00225     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BootEnable"));
00226     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BridgeEnable"));
00227     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "MotorEnable"));
00228     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BrakeRelease"));
00229 
00230     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0", "BootEnable"));
00231     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BridgeEnable"));
00232     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0", "MotorEnable"));
00233     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0", "BrakeRelease"));
00234 
00235     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode")));
00236     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode")));
00237 }
00238 
00239 TEST_F(JointControlCommandFsmTest, Off)
00240 {
00241     jointControlFsmMap["r2/left_leg/joint0"]->off();
00242     jointControlFsmMap["r2/right_leg/gripper/joint0"]->off();
00243     jointControlFsmMap["r2/left_arm/joint0"]->off();
00244     jointControlFsmMap["r2/neck/joint0"]->off();
00245     jointControlFsmMap["r2/right_arm/wrist"]->off();
00246     jointControlFsmMap["r2/right_arm/hand/index"]->off();
00247 
00248     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00249     EXPECT_EQ(r2_msgs::JointControlMode::OFF, commandedStates.controlMode.state);
00250     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00251     EXPECT_EQ(r2_msgs::JointControlMode::OFF, commandedStates.controlMode.state);
00252     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00253     EXPECT_EQ(r2_msgs::JointControlMode::OFF, commandedStates.controlMode.state);
00254     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00255     EXPECT_EQ(r2_msgs::JointControlMode::OFF, commandedStates.controlMode.state);
00256     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00257     EXPECT_EQ(r2_msgs::JointControlMode::OFF, commandedStates.controlMode.state);
00258     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00259     EXPECT_EQ(r2_msgs::JointControlMode::OFF, commandedStates.controlMode.state);
00260 
00261     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BootEnable"));
00262     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BridgeEnable"));
00263     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "MotorEnable"));
00264 
00265     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BootEnable"));
00266     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BridgeEnable"));
00267     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotorEnable"));
00268 
00269     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BootEnable"));
00270     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BridgeEnable"));
00271     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "MotorEnable"));
00272     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BrakeRelease"));
00273 
00274     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BootEnable"));
00275     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BridgeEnable"));
00276     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0", "MotorEnable"));
00277     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0", "BrakeRelease"));
00278 
00279     EXPECT_EQ(r2_msgs::JointControlMode::OFF, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode")));
00280     EXPECT_EQ(r2_msgs::JointControlMode::OFF, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode")));
00281 
00282 }
00283 
00284 TEST_F(JointControlCommandFsmTest, Park)
00285 {
00286     jointControlFsmMap["r2/left_leg/joint0"]->off();
00287     jointControlFsmMap["r2/right_leg/gripper/joint0"]->off();
00288     jointControlFsmMap["r2/left_arm/joint0"]->off();
00289     jointControlFsmMap["r2/neck/joint0"]->off();
00290     jointControlFsmMap["r2/left_leg/joint0"]->park();
00291     jointControlFsmMap["r2/right_leg/gripper/joint0"]->park();
00292     jointControlFsmMap["r2/left_arm/joint0"]->park();
00293     jointControlFsmMap["r2/neck/joint0"]->park();
00294     jointControlFsmMap["r2/right_arm/wrist"]->park();
00295     jointControlFsmMap["r2/right_arm/hand/index"]->park();
00296 
00297     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00298     EXPECT_EQ(r2_msgs::JointControlMode::PARK, commandedStates.controlMode.state);
00299     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00300     EXPECT_EQ(r2_msgs::JointControlMode::PARK, commandedStates.controlMode.state);
00301     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00302     EXPECT_EQ(r2_msgs::JointControlMode::PARK, commandedStates.controlMode.state);
00303     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00304     EXPECT_EQ(r2_msgs::JointControlMode::PARK, commandedStates.controlMode.state);
00305     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00306     EXPECT_EQ(r2_msgs::JointControlMode::PARK, commandedStates.controlMode.state);
00307     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00308     EXPECT_EQ(r2_msgs::JointControlMode::PARK, commandedStates.controlMode.state);
00309 
00310     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BootEnable"));
00311     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BridgeEnable"));
00312     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "MotorEnable"));
00313 
00314     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BootEnable"));
00315     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BridgeEnable"));
00316     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotorEnable"));
00317 
00318     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BootEnable"));
00319     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BridgeEnable"));
00320     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "MotorEnable"));
00321     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BrakeRelease"));
00322 
00323     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BootEnable"));
00324     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BridgeEnable"));
00325     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "MotorEnable"));
00326     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0", "BrakeRelease"));
00327 
00328     EXPECT_EQ(r2_msgs::JointControlMode::PARK, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode")));
00329     EXPECT_EQ(r2_msgs::JointControlMode::PARK, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode")));
00330 }
00331 
00332 TEST_F(JointControlCommandFsmTest, Neutral)
00333 {
00334     jointControlFsmMap["r2/left_leg/joint0"]->off();
00335     jointControlFsmMap["r2/right_leg/gripper/joint0"]->off();
00336     jointControlFsmMap["r2/left_arm/joint0"]->off();
00337     jointControlFsmMap["r2/neck/joint0"]->off();
00338     jointControlFsmMap["r2/left_leg/joint0"]->neutral();
00339     jointControlFsmMap["r2/right_leg/gripper/joint0"]->neutral();
00340     jointControlFsmMap["r2/left_arm/joint0"]->neutral();
00341     jointControlFsmMap["r2/neck/joint0"]->neutral();
00342     jointControlFsmMap["r2/right_arm/wrist"]->neutral();
00343     jointControlFsmMap["r2/right_arm/hand/index"]->neutral();
00344 
00345     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00346     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, commandedStates.controlMode.state);
00347     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00348     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, commandedStates.controlMode.state);
00349     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00350     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, commandedStates.controlMode.state);
00351     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00352     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, commandedStates.controlMode.state);
00353     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00354     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, commandedStates.controlMode.state);
00355     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00356     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, commandedStates.controlMode.state);
00357 
00358     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BootEnable"));
00359     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BridgeEnable"));
00360     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "MotorEnable"));
00361     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BrakeRelease"));
00362 
00363     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BootEnable"));
00364     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BridgeEnable"));
00365     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotorEnable"));
00367     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BrakeRelease"));
00368 
00369     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BootEnable"));
00370     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BridgeEnable"));
00371     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "MotorEnable"));
00372     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BrakeRelease"));
00373 
00374     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BootEnable"));
00375     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BridgeEnable"));
00376     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0", "MotorEnable"));
00377     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BrakeRelease"));
00378 
00379     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode")));
00380     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode")));
00381 }
00382 
00383 TEST_F(JointControlCommandFsmTest, Drive)
00384 {
00385     jointControlFsmMap["r2/left_leg/joint0"]->off();
00386     jointControlFsmMap["r2/right_leg/gripper/joint0"]->off();
00387     jointControlFsmMap["r2/left_arm/joint0"]->off();
00388     jointControlFsmMap["r2/neck/joint0"]->off();
00389     jointControlFsmMap["r2/left_leg/joint0"]->park();
00390     jointControlFsmMap["r2/right_leg/gripper/joint0"]->park();
00391     jointControlFsmMap["r2/left_arm/joint0"]->park();
00392     jointControlFsmMap["r2/neck/joint0"]->park();
00393     jointControlFsmMap["r2/left_leg/joint0"]->drive();
00394     jointControlFsmMap["r2/right_leg/gripper/joint0"]->drive();
00395     jointControlFsmMap["r2/left_arm/joint0"]->drive();
00396     jointControlFsmMap["r2/neck/joint0"]->drive();
00397     jointControlFsmMap["r2/right_arm/wrist"]->drive();
00398     jointControlFsmMap["r2/right_arm/hand/index"]->drive();
00399 
00400     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00401     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, commandedStates.controlMode.state);
00402     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00403     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, commandedStates.controlMode.state);
00404     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00405     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, commandedStates.controlMode.state);
00406     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00407     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, commandedStates.controlMode.state);
00408     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00409     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, commandedStates.controlMode.state);
00410     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00411     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, commandedStates.controlMode.state);
00412 
00413     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BootEnable"));
00414     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "BridgeEnable"));
00415     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "MotorEnable"));
00416 
00417     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BootEnable"));
00418     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "BridgeEnable"));
00419     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotorEnable"));
00420 
00421     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BootEnable"));
00422     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BridgeEnable"));
00423     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "MotorEnable"));
00424     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "BrakeRelease"));
00425 
00426     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BootEnable"));
00427     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BridgeEnable"));
00428     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "MotorEnable"));
00429     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "BrakeRelease"));
00430 
00431     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode")));
00432     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode")));
00433 }
00434 
00435 TEST_F(JointControlCommandFsmTest, InitialCommandMode)
00436 {
00437     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00438     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, commandedStates.commandMode.state);
00439     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00440     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, commandedStates.commandMode.state);
00441     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00442     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, commandedStates.commandMode.state);
00443     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00444     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, commandedStates.commandMode.state);
00445     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00446     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, commandedStates.commandMode.state);
00447     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00448     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, commandedStates.commandMode.state);
00449 
00450 
00451     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "MotComSource"));
00452     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotComSource"));
00453     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "MotComSource"));
00454     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0",              "MotComSource"));
00455 
00456     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CommandMode")));
00457     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CommandMode")));
00458 }
00459 
00460 //TEST_F(JointControlCommandFsmTest, CantMultiLoopWithoutCoeffs)
00461 //{
00462 //    jointControlFsmMap["r2/left_leg/joint0"]->multiLoopStep();
00463 //    jointControlFsmMap["r2/right_leg/gripper/joint0"]->multiLoopStep();
00464 //    jointControlFsmMap["r2/left_arm/joint0"]->multiLoopStep();
00465 //    jointControlFsmMap["r2/neck/joint0"]->multiLoopStep();
00466 
00467 //    EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlFsmMap["r2/left_leg/joint0"]->getStates().commandMode.state);
00468 //    EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates().commandMode.state);
00469 //    EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlFsmMap["r2/left_arm/joint0"]->getStates().commandMode.state);
00470 //    EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlFsmMap["r2/neck/joint0"]->getStates().commandMode.state);
00471 //    EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "MotComSource"));
00472 //    EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotComSource"));
00473 //    EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "MotComSource"));
00474 //    EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0",              "MotComSource"));
00475 
00476 //    jointControlFsmMap["r2/left_leg/joint0"]->multiLoopSmooth();
00477 //    jointControlFsmMap["r2/right_leg/gripper/joint0"]->multiLoopSmooth();
00478 //    jointControlFsmMap["r2/left_arm/joint0"]->multiLoopSmooth();
00479 //    jointControlFsmMap["r2/neck/joint0"]->multiLoopSmooth();
00480 
00481 //    EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlFsmMap["r2/left_leg/joint0"]->getStates().commandMode.state);
00482 //    EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates().commandMode.state);
00483 //    EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlFsmMap["r2/left_arm/joint0"]->getStates().commandMode.state);
00484 //    EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, jointControlFsmMap["r2/neck/joint0"]->getStates().commandMode.state);
00485 //    EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "MotComSource"));
00486 //    EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotComSource"));
00487 //    EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "MotComSource"));
00488 //    EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0",              "MotComSource"));
00489 //}
00490 
00491 TEST_F(JointControlCommandFsmTest, StallMode)
00492 {
00493     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00494     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0100; // 0000 0001 0000 0000
00495     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00496     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0100; // 0000 0001 0000 0000
00497 
00498     jointControlFsmMap["r2/left_leg/joint0"]->stallMode();
00499     jointControlFsmMap["r2/right_leg/gripper/joint0"]->stallMode();
00500     jointControlFsmMap["r2/left_arm/joint0"]->stallMode();
00501     jointControlFsmMap["r2/neck/joint0"]->stallMode();
00502     jointControlFsmMap["r2/right_arm/wrist"]->stallMode();
00503     jointControlFsmMap["r2/right_arm/hand/index"]->stallMode();
00504 
00505     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00506     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, commandedStates.commandMode.state);
00507     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00508     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, commandedStates.commandMode.state);
00509     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00510     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, commandedStates.commandMode.state);
00511     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00512     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, commandedStates.commandMode.state);
00513     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00514     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, commandedStates.commandMode.state);
00515     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00516     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, commandedStates.commandMode.state);
00517 
00518     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "MotComSource"));
00519     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "ControlMode"));
00520 
00521     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotComSource"));
00522     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "ControlMode"));
00523 
00524     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "MotComSource"));
00525     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "ControlMode"));
00526 
00527     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "MotComSource"));
00528     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "ControlMode"));
00529 
00530     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CommandMode")));
00531     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CommandMode")));
00532 
00533 }
00534 
00535 TEST_F(JointControlCommandFsmTest, MultiLoopStep)
00536 {
00537     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00538     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0100; // 0000 0001 0000 0000
00539     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00540     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0100; // 0000 0001 0000 0000
00541 
00542     jointControlFsmMap["r2/left_leg/joint0"]->multiLoopStep();
00543     jointControlFsmMap["r2/right_leg/gripper/joint0"]->multiLoopStep();
00544     jointControlFsmMap["r2/left_arm/joint0"]->multiLoopStep();
00545     jointControlFsmMap["r2/neck/joint0"]->multiLoopStep();
00546     jointControlFsmMap["r2/right_arm/wrist"]->multiLoopStep();
00547     jointControlFsmMap["r2/right_arm/hand/index"]->multiLoopStep();
00548 
00549     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00550     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, commandedStates.commandMode.state);
00551     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00552     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, commandedStates.commandMode.state);
00553     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00554     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, commandedStates.commandMode.state);
00555     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00556     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, commandedStates.commandMode.state);
00557     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00558     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, commandedStates.commandMode.state);
00559     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00560     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, commandedStates.commandMode.state);
00561 
00562     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "MotComSource"));
00563     EXPECT_EQ(3, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "ControlMode"));
00564     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "PosComVelocity"));
00565 
00566     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotComSource"));
00567     EXPECT_EQ(3, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "ControlMode"));
00568 
00569     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "MotComSource"));
00570     EXPECT_EQ(3, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "ControlMode"));
00571     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "PosComVelocity"));
00572 
00573     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "MotComSource"));
00574     EXPECT_EQ(3, nodeRegisterManager->getControlValue("r2/neck/joint0", "ControlMode"));
00575     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0", "PosComVelocity"));
00576 
00577     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CommandMode")));
00578     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CommandMode")));
00579 
00580 }
00581 
00582 TEST_F(JointControlCommandFsmTest, MultiLoopSmooth)
00583 {
00584     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00585     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0100; // 0000 0001 0000 0000
00586     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00587     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0100; // 0000 0001 0000 0000
00588 
00589     jointControlFsmMap["r2/left_leg/joint0"]->multiLoopSmooth();
00590     jointControlFsmMap["r2/right_leg/gripper/joint0"]->multiLoopSmooth();
00591     jointControlFsmMap["r2/left_arm/joint0"]->multiLoopSmooth();
00592     jointControlFsmMap["r2/neck/joint0"]->multiLoopSmooth();
00593     jointControlFsmMap["r2/right_arm/wrist"]->multiLoopSmooth();
00594     jointControlFsmMap["r2/right_arm/hand/index"]->multiLoopSmooth();
00595 
00596     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00597     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, commandedStates.commandMode.state);
00598     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00599     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, commandedStates.commandMode.state);
00600     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00601     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, commandedStates.commandMode.state);
00602     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00603     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, commandedStates.commandMode.state);
00604     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00605     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, commandedStates.commandMode.state);
00606     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00607     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, commandedStates.commandMode.state);
00608 
00609     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "MotComSource"));
00610     EXPECT_EQ(3, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "ControlMode"));
00611     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0", "PosComVelocity"));
00612 
00613     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "MotComSource"));
00614     EXPECT_EQ(3, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "ControlMode"));
00615 
00616     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "MotComSource"));
00617     EXPECT_EQ(3, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "ControlMode"));
00618     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0", "PosComVelocity"));
00619 
00620     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "MotComSource"));
00621     EXPECT_EQ(3, nodeRegisterManager->getControlValue("r2/neck/joint0", "ControlMode"));
00622     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0", "PosComVelocity"));
00623 
00624     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CommandMode")));
00625     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CommandMode")));
00626 
00627 }
00628 
00629 TEST_F(JointControlCommandFsmTest, InitialCalibrationMode)
00630 {
00631     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00632     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00633     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00634     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00635     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00636     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00637     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00638     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00639     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00640     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, commandedStates.calibrationMode.state);
00641     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00642     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, commandedStates.calibrationMode.state);
00643 
00644     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "CalibrationMode"));
00645     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "CalibrationMode"));
00646     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "CalibrationMode"));
00647     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0",              "CalibrationMode"));
00648 
00649     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode")));
00650     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CalibrationMode")));
00651 
00652 }
00653 
00654 TEST_F(JointControlCommandFsmTest, EnableCalibrationModeAndBack)
00655 {
00656     jointControlFsmMap["r2/left_leg/joint0"]->enableCalibrationMode();
00657     jointControlFsmMap["r2/right_leg/gripper/joint0"]->enableCalibrationMode();
00658     jointControlFsmMap["r2/left_arm/joint0"]->enableCalibrationMode();
00659     jointControlFsmMap["r2/neck/joint0"]->enableCalibrationMode();
00660     jointControlFsmMap["r2/right_arm/wrist"]->enableCalibrationMode();
00661     jointControlFsmMap["r2/right_arm/hand/index"]->enableCalibrationMode();
00662 
00663     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00664     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, commandedStates.calibrationMode.state);
00665     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00666     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, commandedStates.calibrationMode.state);
00667     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00668     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, commandedStates.calibrationMode.state);
00669     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00670     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, commandedStates.calibrationMode.state);
00671     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00672     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, commandedStates.calibrationMode.state);
00673     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00674     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, commandedStates.calibrationMode.state);
00675 
00676     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "CalibrationMode"));
00677     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "CalibrationMode"));
00678     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "CalibrationMode"));
00679     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0",              "CalibrationMode"));
00680 
00681     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode")));
00682     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CalibrationMode")));
00683 
00684     jointControlFsmMap["r2/left_leg/joint0"]->disableCalibrationMode();
00685     jointControlFsmMap["r2/right_leg/gripper/joint0"]->disableCalibrationMode();
00686     jointControlFsmMap["r2/left_arm/joint0"]->disableCalibrationMode();
00687     jointControlFsmMap["r2/neck/joint0"]->disableCalibrationMode();
00688     jointControlFsmMap["r2/right_arm/wrist"]->disableCalibrationMode();
00689     jointControlFsmMap["r2/right_arm/hand/index"]->disableCalibrationMode();
00690 
00691     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00692     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00693     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00694     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00695     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00696     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00697     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00698     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00699     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00700     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00701     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00702     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, commandedStates.calibrationMode.state);
00703 
00704     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "CalibrationMode"));
00705     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "CalibrationMode"));
00706     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "CalibrationMode"));
00707     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0",              "CalibrationMode"));
00708 
00709     // you can command the commanded state to disable, but it won't update the coeff, only the objects that actually calibrate should update the coeff
00710     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode")));
00711     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CalibrationMode")));
00712 
00713 }
00714 
00715 TEST_F(JointControlCommandFsmTest, InitToDisableClearFault)
00716 {
00717     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00718     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00719     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00720     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00721     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00722     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00723     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00724     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00725     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00726     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00727     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00728     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00729 
00730     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "ClearFault"));
00731     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "ClearFault"));
00732     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "ClearFault"));
00733     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0",              "ClearFault"));
00734 
00735     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ClearFaultState")));
00736     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ClearFaultState")));
00737 }
00738 
00739 TEST_F(JointControlCommandFsmTest, EnableClearFaultModeAndBack)
00740 {
00741     jointControlFsmMap["r2/left_leg/joint0"]->enableClearFaultMode();
00742     jointControlFsmMap["r2/right_leg/gripper/joint0"]->enableClearFaultMode();
00743     jointControlFsmMap["r2/left_arm/joint0"]->enableClearFaultMode();
00744     jointControlFsmMap["r2/neck/joint0"]->enableClearFaultMode();
00745     jointControlFsmMap["r2/right_arm/wrist"]->enableClearFaultMode();
00746     jointControlFsmMap["r2/right_arm/hand/index"]->enableClearFaultMode();
00747 
00748     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00749     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, commandedStates.clearFaultMode.state);
00750     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00751     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, commandedStates.clearFaultMode.state);
00752     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00753     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, commandedStates.clearFaultMode.state);
00754     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00755     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, commandedStates.clearFaultMode.state);
00756     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00757     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, commandedStates.clearFaultMode.state);
00758     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00759     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, commandedStates.clearFaultMode.state);
00760 
00761     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "ClearFault"));
00762     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "ClearFault"));
00763     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "ClearFault"));
00764     EXPECT_EQ(1, nodeRegisterManager->getControlValue("r2/neck/joint0",              "ClearFault"));
00765 
00766     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ClearFaultState")));
00767     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ClearFaultState")));
00768 
00769     jointControlFsmMap["r2/left_leg/joint0"]->disableClearFaultMode();
00770     jointControlFsmMap["r2/right_leg/gripper/joint0"]->disableClearFaultMode();
00771     jointControlFsmMap["r2/left_arm/joint0"]->disableClearFaultMode();
00772     jointControlFsmMap["r2/neck/joint0"]->disableClearFaultMode();
00773     jointControlFsmMap["r2/right_arm/wrist"]->disableClearFaultMode();
00774     jointControlFsmMap["r2/right_arm/hand/index"]->disableClearFaultMode();
00775 
00776     jointControlFsmMap["r2/left_leg/joint0"]->getStates(commandedStates);
00777     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00778     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(commandedStates);
00779     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00780     jointControlFsmMap["r2/left_arm/joint0"]->getStates(commandedStates);
00781     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00782     jointControlFsmMap["r2/neck/joint0"]->getStates(commandedStates);
00783     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00784     jointControlFsmMap["r2/right_arm/wrist"]->getStates(commandedStates);
00785     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00786     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(commandedStates);
00787     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, commandedStates.clearFaultMode.state);
00788 
00789     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_leg/joint0",          "ClearFault"));
00790     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/right_leg/gripper/joint0", "ClearFault"));
00791     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/left_arm/joint0",          "ClearFault"));
00792     EXPECT_EQ(0, nodeRegisterManager->getControlValue("r2/neck/joint0",              "ClearFault"));
00793 
00794     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ClearFaultState")));
00795     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, io.getLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ClearFaultState")));
00796 
00797 }
00798 
00799 int main(int argc, char** argv)
00800 {
00801     testing::InitGoogleTest(&argc, argv);
00802     return RUN_ALL_TESTS();
00803 }

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