JointControlActual_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/JointControlActualGripper.h"
00007 #include "robodyn_mechanisms/JointControlActualSeriesElastic.h"
00008 #include "robodyn_mechanisms/JointControlActualWrist.h"
00009 #include "robodyn_mechanisms/JointControlManagerFactory.h"
00010 #include "r2_msgs/PowerState.h"
00011 #include <ros/package.h>
00012 
00013 using namespace std;
00014 
00015 // Fake the io functions
00016 std::map<std::string, float> testLiveCoeffs;
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 class JointControlActualFsmTest : public ::testing::Test
00045 {
00046 protected:
00047     virtual void SetUp()
00048     {
00049         // Initialize objects
00050         nodeRegisterManager = boost::make_shared<NodeRegisterManager>();
00051         jointControlFsmMap.clear();
00052 
00053         // Load RobotInstance
00054         packagePath      = ros::package::getPath("robot_instance");
00055         configPath       = packagePath + "/test/config";
00056         configSafetyPath = packagePath + "/test/configSafety";
00057         fileName         = "TestRobotInstance.xml";
00058         instance         = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName);
00059 
00060         // Set up function pointers
00061         testLiveCoeffs.clear();
00062         io.getRegisterFile = getRegisterFile;
00063         io.getControlFile  = getControlFile;
00064         io.hasLiveCoeff    = hasLiveCoeff;
00065         io.getLiveCoeff    = getLiveCoeff;
00066         io.setLiveCoeff    = setLiveCoeff;
00067 
00068         // Initialize nodeRegisterManager and jointControlFsmMap
00069         for (unsigned int i = 0; i < instance->mechanisms.size(); i++)
00070         {
00071             mechanism = instance->mechanisms[i];
00072             fileName  = getRegisterFile(mechanism);
00073             jointType = JointControlManagerFactory::Private::getPropertyFromFile(fileName, "NodeType");
00074 
00075             // Simple types
00076             if (jointType == "SeriesElastic")
00077             {
00078                 nodeRegisterManager->addNode(mechanism, fileName);
00079                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlActualSeriesElastic>(mechanism, io, nodeRegisterManager);
00080                 nodeRegisterManager->nodeRegisterMap[mechanism].status[mechanism + "/StatReg1"].registerValue = 0x9000; // 1001 0000 0000 0000
00081             }
00082             else if (jointType == "Rigid")
00083             {
00084                 nodeRegisterManager->addNode(mechanism, fileName);
00085                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlActualSeriesElastic>(mechanism, io, nodeRegisterManager);
00086                 nodeRegisterManager->nodeRegisterMap[mechanism].status[mechanism + "/StatReg1"].registerValue = 0x9000; // 1001 0000 0000 0000
00087             }
00088             else if (jointType == "Gripper")
00089             {
00090                 nodeRegisterManager->addNode(mechanism, fileName);
00091                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlActualGripper>(mechanism, io, nodeRegisterManager);
00092                 nodeRegisterManager->nodeRegisterMap[mechanism].status[mechanism + "/StatReg1"].registerValue = 0x9000; // 1001 0000 0000 0000
00093             }
00094 
00095             // Complex types
00096             else if (jointType == "Wrist")
00097             {
00098                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlActualWrist>(mechanism, io);
00099                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "PitchLimit"),            0);
00100                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "YawLimit"),              0);
00101                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "LittlesideSliderLimit"), 0);
00102                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "ThumbsideSliderLimit"),  0);
00103                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "SensorError"),           0);
00104                 io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "SliderDiffError"),       0);
00105             }
00106             else if (jointType == "Thumb")
00107             {
00108                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlActualFinger>(mechanism, io);
00109             }
00110             else if (jointType == "PrimaryFinger")
00111             {
00112                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlActualFinger>(mechanism, io);
00113             }
00114             else if (jointType == "SecondaryFingers")
00115             {
00116                 jointControlFsmMap[mechanism] = boost::make_shared<JointControlActualFinger>(mechanism, io);
00117             }
00118             else
00119             {
00120                 std::stringstream err;
00121                 err << "Unsupported joint type [" << jointType << "] found in ControlFile";
00122                 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointControlActualFsmTest", log4cpp::Priority::ERROR, err.str());
00123                 ASSERT_TRUE(false);
00124             }
00125         }
00126     }
00127 
00128     virtual void TearDown()
00129     {
00130     }
00131 
00132     std::string                                                                packagePath;
00133     std::string                                                                configPath, configSafetyPath, fileName;
00134     NodeRegisterManagerPtr                                                     nodeRegisterManager;
00135     std::string                                                                jointType;
00136     std::string                                                                mechanism;
00137     std::map< std::string, boost::shared_ptr<JointControlActualInterface> > jointControlFsmMap;
00138     JointControlCommonInterface::IoFunctions                                   io;
00139     r2_msgs::JointControlData                                      actualStates;
00140 };
00141 
00142 TEST_F(JointControlActualFsmTest, InitialBootLoader)
00143 {
00144     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00145     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00146     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00147     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00148     jointControlFsmMap["r2/right_arm/wrist"]->updateControlModeState();
00149     jointControlFsmMap["r2/right_arm/hand/index"]->updateControlModeState();
00150 
00151     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00152     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, actualStates.controlMode.state);
00153     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00154     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, actualStates.controlMode.state);
00155     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00156     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, actualStates.controlMode.state);
00157     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00158     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, actualStates.controlMode.state);
00159     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00160     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, actualStates.controlMode.state);
00161     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00162     EXPECT_EQ(r2_msgs::JointControlMode::BOOTLOADER, actualStates.controlMode.state);
00163 
00164     EXPECT_EQ(0, nodeRegisterManager->getStatusValue("r2/left_leg/joint0", "NotInBootloader"));
00165     EXPECT_EQ(0, nodeRegisterManager->getStatusValue("r2/right_leg/gripper/joint0", "NotInBootloader"));
00166     EXPECT_EQ(0, nodeRegisterManager->getStatusValue("r2/left_arm/joint0", "NotInBootloader"));
00167     EXPECT_EQ(0, nodeRegisterManager->getStatusValue("r2/neck/joint0", "NotInBootloader"));
00168 }
00169 
00170 TEST_F(JointControlActualFsmTest, FpgaFaults)
00171 {
00172     // Fake CommAlive is dead
00173     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x8000; // 1000 0000 0000 0000
00174     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x8000; // 1000 0000 0000 0000
00175     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x8000; // 1000 0000 0000 0000
00176     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x8000; // 1000 0000 0000 0000
00177 
00178     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00179     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00180     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00181     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00182 
00183     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00184     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00185     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00186     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00187     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00188     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00189     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00190     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00191 
00192     // UnFake CommAlive is dead
00193     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
00194     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9000; // 1001 0000 0000 0000
00195     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
00196     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9000; // 1001 0000 0000 0000
00197 
00198     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00199     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00200     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00201     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00202 
00203     // Fake ProcAlive is dead
00204     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x1000; // 0001 0000 0000 0000
00205     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x1000; // 0001 0000 0000 0000
00206     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x1000; // 0001 0000 0000 0000
00207     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x1000; // 0001 0000 0000 0000
00208 
00209     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00210     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00211     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00212     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00213 
00214     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00215     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00216     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00217     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00218     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00219     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00220     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00221     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00222 
00223     // UnFake ProcAlive is dead
00224     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
00225     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9000; // 1001 0000 0000 0000
00226     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
00227     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9000; // 1001 0000 0000 0000
00228 
00229     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00230     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00231     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00232     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00233 }
00234 
00235 TEST_F(JointControlActualFsmTest, Off)
00236 {
00237     // Fake out of Bootloader
00238     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00239     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00240     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00241     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue                           = 0x0004; // 0000 0000 0000 0100
00242     //off
00243     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00244     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9010; // 1001 0000 0001 0000
00245     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00246     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9010; // 1001 0000 0001 0000
00247 
00248     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00249     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00250     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00251     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00252 
00253     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00254     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00255     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00256     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00257     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00258     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00259     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00260     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00261 
00262     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode"),           r2_msgs::JointControlMode::OFF);
00263     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode"),      r2_msgs::JointControlMode::OFF);
00264 
00265     jointControlFsmMap["r2/right_arm/wrist"]->updateControlModeState();
00266     jointControlFsmMap["r2/right_arm/hand/index"]->updateControlModeState();
00267 
00268     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00269     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00270     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00271     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00272 }
00273 
00274 TEST_F(JointControlActualFsmTest, FaultIfInMotCom)
00275 {
00276     // Fake out of Bootloader
00277     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00278     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00279     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00280     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue                           = 0x0004; // 0000 0000 0000 0100
00281 
00282     // Fake Embedded MotCom and BridgeEnable
00283     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00284     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9010; // 1001 0000 0001 0000
00285     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00286     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9010; // 1001 0000 0001 0000
00287 
00288     // Fake coeffs loaded and joint faulted
00289     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0180; // 0000 0001 1000 0000
00290     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0180; // 0000 0001 1000 0000
00291     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0180; // 0000 0001 1000 0000
00292     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0180; // 0000 0001 1000 0000
00293 
00294     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00295     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00296     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00297     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00298 
00299     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00300     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00301     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00302     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00303     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00304     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00305     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00306     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00307 }
00308 
00309 TEST_F(JointControlActualFsmTest, Faults)
00310 {
00311     // Fake getting into MultiLoopStep
00312     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
00313     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0103; // 0000 0001 0000 0011
00314     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
00315     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0103; // 0000 0001 0000 0011
00316     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00317     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
00318     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00319     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
00320     jointControlFsmMap["r2/left_leg/joint0"]->updateCommandModeState();
00321     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCommandModeState();
00322     jointControlFsmMap["r2/left_arm/joint0"]->updateCommandModeState();
00323     jointControlFsmMap["r2/neck/joint0"]->updateCommandModeState();
00324 
00325     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00326     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00327     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00328     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00329     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00330     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00331     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00332     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00333 
00334     // Fake out of Bootloader
00335     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00336     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00337     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00338     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue                           = 0x0004; // 0000 0000 0000 0100
00339 
00340     // Fake Embedded MotCom and BridgeEnable
00341     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9050; // 1001 0000 0101 0000
00342     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9050; // 1001 0000 0101 0000
00343     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9050; // 1001 0000 0101 0000
00344     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9050; // 1001 0000 0101 0000
00345 
00346     // Fake coeffs loaded and joint faulted
00347     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0180; // 0000 0001 1000 0000
00348     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0180; // 0000 0001 1000 0000
00349     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0180; // 0000 0001 1000 0000
00350     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0180; // 0000 0001 1000 0000
00351 
00352     // Fake complex calibrated and faulted
00353     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "IsCalibrated"), 1);
00354     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "PitchLimit"),   1);
00355 
00356     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00357     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00358     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00359     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00360     jointControlFsmMap["r2/right_arm/wrist"]->updateControlModeState();
00361 
00362     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00363     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00364     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00365     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00366     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00367     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00368     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00369     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00370     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00371     EXPECT_EQ(r2_msgs::JointControlMode::FAULTED, actualStates.controlMode.state);
00372 }
00373 
00374 TEST_F(JointControlActualFsmTest, Park)
00375 {
00376     // Fake out of Bootloader
00377     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00378     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00379     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00380     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue                           = 0x0004; // 0000 0000 0000 0100
00381 
00382     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00383     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9011; // 1001 0000 0001 0001
00384     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9011; // 1001 0000 0001 0001
00385     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9011; // 1001 0000 0001 0001
00386 
00387     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00388     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00389     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00390     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00391 
00392     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00393     EXPECT_EQ(r2_msgs::JointControlMode::PARK, actualStates.controlMode.state);
00394     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00395     EXPECT_EQ(r2_msgs::JointControlMode::PARK, actualStates.controlMode.state);
00396     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00397     EXPECT_EQ(r2_msgs::JointControlMode::PARK, actualStates.controlMode.state);
00398     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00399     EXPECT_EQ(r2_msgs::JointControlMode::PARK, actualStates.controlMode.state);
00400 
00401     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode"),           r2_msgs::JointControlMode::PARK);
00402     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode"),      r2_msgs::JointControlMode::PARK);
00403 
00404     jointControlFsmMap["r2/right_arm/wrist"]->updateControlModeState();
00405     jointControlFsmMap["r2/right_arm/hand/index"]->updateControlModeState();
00406 
00407     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00408     EXPECT_EQ(r2_msgs::JointControlMode::PARK, actualStates.controlMode.state);
00409     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00410     EXPECT_EQ(r2_msgs::JointControlMode::PARK, actualStates.controlMode.state);
00411 }
00412 
00413 TEST_F(JointControlActualFsmTest, Neutral)
00414 {
00415     // Fake out of Bootloader
00416     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00417     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00418     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00419     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue                           = 0x0004; // 0000 0000 0000 0100
00420 
00421     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9012; // 1001 0000 0001 0010
00422     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9012; // 1001 0000 0001 0010
00423     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9012; // 1001 0000 0001 0010
00424     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9012; // 1001 0000 0001 0010
00425 
00426     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00427     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00428     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00429     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00430 
00431     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00432     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, actualStates.controlMode.state);
00433     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00434     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, actualStates.controlMode.state);
00435     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00436     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, actualStates.controlMode.state);
00437     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00438     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, actualStates.controlMode.state);
00439 
00440     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode"),           r2_msgs::JointControlMode::NEUTRAL);
00441     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode"),      r2_msgs::JointControlMode::NEUTRAL);
00442 
00443     jointControlFsmMap["r2/right_arm/wrist"]->updateControlModeState();
00444     jointControlFsmMap["r2/right_arm/hand/index"]->updateControlModeState();
00445 
00446     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00447     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, actualStates.controlMode.state);
00448     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00449     EXPECT_EQ(r2_msgs::JointControlMode::NEUTRAL, actualStates.controlMode.state);
00450 }
00451 
00452 TEST_F(JointControlActualFsmTest, Drive)
00453 {
00454     // Fake out of Bootloader
00455     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00456     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00457     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00458     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue                           = 0x0004; // 0000 0000 0000 0100
00459 
00460     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9013; // 1001 0000 0001 0011
00461     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9013; // 1001 0000 0001 0011
00462     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9013; // 1001 0000 0001 0011
00463     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9013; // 1001 0000 0001 0011
00464 
00465     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00466     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00467     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00468     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00469     jointControlFsmMap["r2/right_arm/wrist"]->updateControlModeState();
00470 
00471     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00472     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, actualStates.controlMode.state);
00473     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00474     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, actualStates.controlMode.state);
00475     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00476     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, actualStates.controlMode.state);
00477     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00478     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, actualStates.controlMode.state);
00479 
00480     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "ControlMode"),           r2_msgs::JointControlMode::DRIVE);
00481     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "ControlMode"),      r2_msgs::JointControlMode::DRIVE);
00482 
00483     jointControlFsmMap["r2/right_arm/wrist"]->updateControlModeState();
00484     jointControlFsmMap["r2/right_arm/hand/index"]->updateControlModeState();
00485 
00486     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00487     EXPECT_EQ(r2_msgs::JointControlMode::PARK, actualStates.controlMode.state);
00488     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00489     EXPECT_EQ(r2_msgs::JointControlMode::PARK, actualStates.controlMode.state);
00490 
00491     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode"),           r2_msgs::JointControlCalibrationMode::DISABLE);
00492     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CalibrationMode"),      r2_msgs::JointControlCalibrationMode::DISABLE);
00493 
00494     jointControlFsmMap["r2/right_arm/wrist"]->updateControlModeState();
00495     jointControlFsmMap["r2/right_arm/hand/index"]->updateControlModeState();
00496 
00497     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00498     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, actualStates.controlMode.state);
00499     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00500     EXPECT_EQ(r2_msgs::JointControlMode::DRIVE, actualStates.controlMode.state);
00501 
00502 }
00503 
00504 TEST_F(JointControlActualFsmTest, AFewControlModeInvalid)
00505 {
00506     // Fake out of Bootloader
00507     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00508     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
00509     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
00510     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue                           = 0x0004; // 0000 0000 0000 0100
00511 
00512     // MotorEnable while BridgeDisable
00513     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9001; // 1001 0000 0000 0001
00514     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9001; // 1001 0000 0000 0001
00515     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9001; // 1001 0000 0000 0001
00516     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9001; // 1001 0000 0000 0001
00517 
00518     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00519     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00520     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00521     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00522 
00523     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00524     EXPECT_EQ(r2_msgs::JointControlMode::INVALID, actualStates.controlMode.state);
00525     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00526     EXPECT_EQ(r2_msgs::JointControlMode::INVALID, actualStates.controlMode.state);
00527     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00528     EXPECT_EQ(r2_msgs::JointControlMode::INVALID, actualStates.controlMode.state);
00529     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00530     EXPECT_EQ(r2_msgs::JointControlMode::INVALID, actualStates.controlMode.state);
00531 
00532     // Reset to off
00533     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00534     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9010; // 1001 0000 0001 0000
00535     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9010; // 1001 0000 0001 0000
00536     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9010; // 1001 0000 0001 0000
00537 
00538     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00539     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00540     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00541     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00542 
00543     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00544     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00545     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00546     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00547     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00548     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00549     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00550     EXPECT_EQ(r2_msgs::JointControlMode::OFF, actualStates.controlMode.state);
00551 }
00552 
00553 TEST_F(JointControlActualFsmTest, InitialCommandMode)
00554 {
00555     jointControlFsmMap["r2/left_leg/joint0"]->updateCommandModeState();
00556     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCommandModeState();
00557     jointControlFsmMap["r2/left_arm/joint0"]->updateCommandModeState();
00558     jointControlFsmMap["r2/neck/joint0"]->updateCommandModeState();
00559     jointControlFsmMap["r2/right_arm/wrist"]->updateCommandModeState();
00560     jointControlFsmMap["r2/right_arm/hand/index"]->updateCommandModeState();
00561 
00562     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00563     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, actualStates.commandMode.state);
00564     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00565     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, actualStates.commandMode.state);
00566     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00567     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, actualStates.commandMode.state);
00568     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00569     EXPECT_EQ(r2_msgs::JointControlCommandMode::MOTCOM, actualStates.commandMode.state);
00570     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00571     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, actualStates.commandMode.state);
00572     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00573     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, actualStates.commandMode.state);
00574 
00575 }
00576 
00577 TEST_F(JointControlActualFsmTest, CantMultiLoopWithNoCoeffs)
00578 {
00579     // Fake coeffs not loaded and MultiLoop
00580     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0003; // 0000 0000 0000 0011
00581     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0003; // 0000 0000 0000 0011
00582     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0003; // 0000 0000 0000 0011
00583     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0003; // 0000 0000 0000 0011
00584 
00585     // Fake MotComSource == 1
00586     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00587     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
00588     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00589     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
00590 
00591     jointControlFsmMap["r2/left_leg/joint0"]->updateCommandModeState();
00592     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCommandModeState();
00593     jointControlFsmMap["r2/left_arm/joint0"]->updateCommandModeState();
00594     jointControlFsmMap["r2/neck/joint0"]->updateCommandModeState();
00595 
00596     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00597     EXPECT_EQ(r2_msgs::JointControlCommandMode::INVALID, actualStates.commandMode.state);
00598     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00599     EXPECT_EQ(r2_msgs::JointControlCommandMode::INVALID, actualStates.commandMode.state);
00600     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00601     EXPECT_EQ(r2_msgs::JointControlCommandMode::INVALID, actualStates.commandMode.state);
00602     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00603     EXPECT_EQ(r2_msgs::JointControlCommandMode::INVALID, actualStates.commandMode.state);
00604 }
00605 
00606 TEST_F(JointControlActualFsmTest, StallMode)
00607 {
00608     // Fake coeffs loaded and StallMode
00609     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0101; // 0000 0001 0000 0001
00610     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0101; // 0000 0001 0000 0001
00611     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0101; // 0000 0001 0000 0001
00612     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0101; // 0000 0001 0000 0001
00613 
00614     // Fake MotComSource == 1
00615     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00616     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
00617     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00618     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
00619 
00620     jointControlFsmMap["r2/left_leg/joint0"]->updateCommandModeState();
00621     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCommandModeState();
00622     jointControlFsmMap["r2/left_arm/joint0"]->updateCommandModeState();
00623     jointControlFsmMap["r2/neck/joint0"]->updateCommandModeState();
00624 
00625     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00626     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, actualStates.commandMode.state);
00627     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00628     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, actualStates.commandMode.state);
00629     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00630     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, actualStates.commandMode.state);
00631     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00632     EXPECT_EQ(r2_msgs::JointControlCommandMode::STALLMODE, actualStates.commandMode.state);
00633 }
00634 
00635 TEST_F(JointControlActualFsmTest, MultiLoopStep)
00636 {
00637     // Fake coeffs loaded and MultiLoop
00638     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
00639     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0103; // 0000 0001 0000 0011
00640     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
00641     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0103; // 0000 0001 0000 0011
00642 
00643     // Fake MotComSource == 1
00644     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00645     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
00646     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00647     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
00648 
00649     jointControlFsmMap["r2/left_leg/joint0"]->updateCommandModeState();
00650     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCommandModeState();
00651     jointControlFsmMap["r2/left_arm/joint0"]->updateCommandModeState();
00652     jointControlFsmMap["r2/neck/joint0"]->updateCommandModeState();
00653 
00654     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00655     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00656     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00657     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00658     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00659     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00660     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00661     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00662 
00663     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CommandMode"),           r2_msgs::JointControlCommandMode::MULTILOOPSTEP);
00664     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CommandMode"),      r2_msgs::JointControlCommandMode::MULTILOOPSTEP);
00665 
00666     jointControlFsmMap["r2/right_arm/wrist"]->updateCommandModeState();
00667     jointControlFsmMap["r2/right_arm/hand/index"]->updateCommandModeState();
00668 
00669     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00670     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00671     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00672     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00673 }
00674 
00675 TEST_F(JointControlActualFsmTest, MultiLoopSmooth)
00676 {
00677     // Fake coeffs loaded and MultiLoop
00678     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
00679     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0103; // 0000 0001 0000 0011
00680     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0103; // 0000 0001 0000 0011
00681     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0103; // 0000 0001 0000 0011
00682 
00683     // Fake MotComSource == 1
00684     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00685     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9040; // 1001 0000 0100 0000
00686     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9040; // 1001 0000 0100 0000
00687     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9040; // 1001 0000 0100 0000
00688 
00689     // Fake PosComVelocity == 1
00690     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00691     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0100; // 0000 0001 0000 0000
00692     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00693     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue                           = 0x0100; // 0000 0001 0000 0000
00694 
00695     jointControlFsmMap["r2/left_leg/joint0"]->updateCommandModeState();
00696     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCommandModeState();
00697     jointControlFsmMap["r2/left_arm/joint0"]->updateCommandModeState();
00698     jointControlFsmMap["r2/neck/joint0"]->updateCommandModeState();
00699 
00700     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00701     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, actualStates.commandMode.state);
00702     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00703     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSTEP, actualStates.commandMode.state);
00704     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00705     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, actualStates.commandMode.state);
00706     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00707     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, actualStates.commandMode.state);
00708 
00709     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CommandMode"),           r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH);
00710     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CommandMode"),      r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH);
00711 
00712     jointControlFsmMap["r2/right_arm/wrist"]->updateCommandModeState();
00713     jointControlFsmMap["r2/right_arm/hand/index"]->updateCommandModeState();
00714 
00715     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00716     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, actualStates.commandMode.state);
00717     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00718     EXPECT_EQ(r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH, actualStates.commandMode.state);
00719 }
00720 
00721 TEST_F(JointControlActualFsmTest, InitialCalibrationMode)
00722 {
00723     jointControlFsmMap["r2/left_leg/joint0"]->updateCalibrationModeState();
00724     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCalibrationModeState();
00725     jointControlFsmMap["r2/left_arm/joint0"]->updateCalibrationModeState();
00726     jointControlFsmMap["r2/neck/joint0"]->updateCalibrationModeState();
00727     jointControlFsmMap["r2/right_arm/wrist"]->updateCalibrationModeState();
00728     jointControlFsmMap["r2/right_arm/hand/index"]->updateCalibrationModeState();
00729 
00730     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00731     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00732     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00733     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00734     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00735     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00736     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00737     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00738     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00739     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, actualStates.calibrationMode.state);
00740     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00741     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, actualStates.calibrationMode.state);
00742 }
00743 
00744 TEST_F(JointControlActualFsmTest, EnableCalibrationAndBack)
00745 {
00746     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x8000; // 1000 0000 0000 0000
00747     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x8000; // 1000 0000 0000 0000
00748     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x8000; // 1000 0000 0000 0000
00749     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x8000; // 1000 0000 0000 0000
00750 
00751     jointControlFsmMap["r2/left_leg/joint0"]->updateCalibrationModeState();
00752     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCalibrationModeState();
00753     jointControlFsmMap["r2/left_arm/joint0"]->updateCalibrationModeState();
00754     jointControlFsmMap["r2/neck/joint0"]->updateCalibrationModeState();
00755 
00756     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00757     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, actualStates.calibrationMode.state);
00758     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00759     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, actualStates.calibrationMode.state);
00760     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00761     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, actualStates.calibrationMode.state);
00762     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00763     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, actualStates.calibrationMode.state);
00764 
00765     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0000; // 0000 0000 0000 0000
00766     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0000; // 0000 0000 0000 0000
00767     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0000; // 0000 0000 0000 0000
00768     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0000; // 0000 0000 0000 0000
00769 
00770     jointControlFsmMap["r2/left_leg/joint0"]->updateCalibrationModeState();
00771     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCalibrationModeState();
00772     jointControlFsmMap["r2/left_arm/joint0"]->updateCalibrationModeState();
00773     jointControlFsmMap["r2/neck/joint0"]->updateCalibrationModeState();
00774 
00775     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00776     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00777     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00778     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00779     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00780     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00781     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00782     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00783 
00784     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode"),           r2_msgs::JointControlCalibrationMode::ENABLE);
00785     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CalibrationMode"),      r2_msgs::JointControlCalibrationMode::ENABLE);
00786 
00787     jointControlFsmMap["r2/right_arm/wrist"]->updateCalibrationModeState();
00788     jointControlFsmMap["r2/right_arm/hand/index"]->updateCalibrationModeState();
00789 
00790     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00791     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, actualStates.calibrationMode.state);
00792     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00793     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, actualStates.calibrationMode.state);
00794 
00795     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/wrist", "CalibrationMode"),           r2_msgs::JointControlCalibrationMode::DISABLE);
00796     io.setLiveCoeff(StringUtilities::makeFullyQualifiedRoboDynElement("r2/right_arm/hand/index", "CalibrationMode"),      r2_msgs::JointControlCalibrationMode::DISABLE);
00797 
00798     jointControlFsmMap["r2/right_arm/wrist"]->updateCalibrationModeState();
00799     jointControlFsmMap["r2/right_arm/hand/index"]->updateCalibrationModeState();
00800 
00801     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00802     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00803     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00804     EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, actualStates.calibrationMode.state);
00805 }
00806 
00807 TEST_F(JointControlActualFsmTest, InitialClearFaultState)
00808 {
00809     jointControlFsmMap["r2/left_leg/joint0"]->updateClearFaultModeState();
00810     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateClearFaultModeState();
00811     jointControlFsmMap["r2/left_arm/joint0"]->updateClearFaultModeState();
00812     jointControlFsmMap["r2/neck/joint0"]->updateClearFaultModeState();
00813     jointControlFsmMap["r2/right_arm/wrist"]->updateClearFaultModeState();
00814     jointControlFsmMap["r2/right_arm/hand/thumb"]->updateClearFaultModeState();
00815     jointControlFsmMap["r2/right_arm/hand/index"]->updateClearFaultModeState();
00816     jointControlFsmMap["r2/right_arm/hand/ringlittle"]->updateClearFaultModeState();
00817 
00818     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00819     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00820     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00821     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00822     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00823     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00824     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00825     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00826     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00827     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00828     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00829     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00830 }
00831 
00832 TEST_F(JointControlActualFsmTest, EnableClearFaultAndBack)
00833 {
00834     // Fake clear fault ack
00835     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0040; // 0000 0000 0100 0000
00836     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0040; // 0000 0000 0100 0000
00837     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0040; // 0000 0000 0100 0000
00838     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0040; // 0000 0000 0100 0000
00839 
00840     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00841     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, actualStates.clearFaultMode.state);
00842     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00843     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, actualStates.clearFaultMode.state);
00844     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00845     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, actualStates.clearFaultMode.state);
00846     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00847     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::ENABLE, actualStates.clearFaultMode.state);
00848 
00849     // Unfake clear fault ack
00850     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0000; // 0000 0000 0000 0000
00851     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0000; // 0000 0000 0000 0000
00852     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0000; // 0000 0000 0000 0000
00853     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0000; // 0000 0000 0000 0000
00854 
00855     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00856     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00857     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00858     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00859     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00860     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00861     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00862     EXPECT_EQ(r2_msgs::JointControlClearFaultMode::DISABLE, actualStates.clearFaultMode.state);
00863 }
00864 
00865 TEST_F(JointControlActualFsmTest, InitialCoeffState)
00866 {
00867     jointControlFsmMap["r2/left_leg/joint0"]->updateCoeffState();
00868     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCoeffState();
00869     jointControlFsmMap["r2/left_arm/joint0"]->updateCoeffState();
00870     jointControlFsmMap["r2/neck/joint0"]->updateCoeffState();
00871     jointControlFsmMap["r2/right_arm/wrist"]->updateCoeffState();
00872     jointControlFsmMap["r2/right_arm/hand/thumb"]->updateCoeffState();
00873     jointControlFsmMap["r2/right_arm/hand/index"]->updateCoeffState();
00874     jointControlFsmMap["r2/right_arm/hand/ringlittle"]->updateCoeffState();
00875 
00876     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00877     EXPECT_EQ(r2_msgs::JointControlCoeffState::NOTLOADED, actualStates.coeffState.state);
00878     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00879     EXPECT_EQ(r2_msgs::JointControlCoeffState::NOTLOADED, actualStates.coeffState.state);
00880     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00881     EXPECT_EQ(r2_msgs::JointControlCoeffState::NOTLOADED, actualStates.coeffState.state);
00882     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00883     EXPECT_EQ(r2_msgs::JointControlCoeffState::NOTLOADED, actualStates.coeffState.state);
00884     jointControlFsmMap["r2/right_arm/wrist"]->getStates(actualStates);
00885     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, actualStates.coeffState.state);
00886     jointControlFsmMap["r2/right_arm/hand/index"]->getStates(actualStates);
00887     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, actualStates.coeffState.state);
00888 }
00889 
00890 TEST_F(JointControlActualFsmTest, ShowCoeffsLoadedAndBack)
00891 {
00892     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00893     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0100; // 0000 0001 0000 0000
00894     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0100; // 0000 0001 0000 0000
00895     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0100; // 0000 0001 0000 0000
00896 
00897     jointControlFsmMap["r2/left_leg/joint0"]->updateCoeffState();
00898     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCoeffState();
00899     jointControlFsmMap["r2/left_arm/joint0"]->updateCoeffState();
00900     jointControlFsmMap["r2/neck/joint0"]->updateCoeffState();
00901 
00902     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00903     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, actualStates.coeffState.state);
00904     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00905     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, actualStates.coeffState.state);
00906     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00907     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, actualStates.coeffState.state);
00908     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00909     EXPECT_EQ(r2_msgs::JointControlCoeffState::LOADED, actualStates.coeffState.state);
00910 
00911     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0000; // 0000 0000 0000 0000
00912     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0000; // 0000 0000 0000 0000
00913     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0000; // 0000 0000 0000 0000
00914     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0000; // 0000 0000 0000 0000
00915 
00916     jointControlFsmMap["r2/left_leg/joint0"]->updateCoeffState();
00917     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateCoeffState();
00918     jointControlFsmMap["r2/left_arm/joint0"]->updateCoeffState();
00919     jointControlFsmMap["r2/neck/joint0"]->updateCoeffState();
00920 
00921     jointControlFsmMap["r2/left_leg/joint0"]->getStates(actualStates);
00922     EXPECT_EQ(r2_msgs::JointControlCoeffState::NOTLOADED, actualStates.coeffState.state);
00923     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getStates(actualStates);
00924     EXPECT_EQ(r2_msgs::JointControlCoeffState::NOTLOADED, actualStates.coeffState.state);
00925     jointControlFsmMap["r2/left_arm/joint0"]->getStates(actualStates);
00926     EXPECT_EQ(r2_msgs::JointControlCoeffState::NOTLOADED, actualStates.coeffState.state);
00927     jointControlFsmMap["r2/neck/joint0"]->getStates(actualStates);
00928     EXPECT_EQ(r2_msgs::JointControlCoeffState::NOTLOADED, actualStates.coeffState.state);
00929 }
00930 
00931 TEST_F(JointControlActualFsmTest, getFaults)
00932 {
00933     diagnostic_msgs::DiagnosticStatus status;
00935 
00936     // Fake CommAlive is dead
00937     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x8000; // 1000 0000 0000 0000
00938     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x8000; // 1000 0000 0000 0000
00939     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x8000; // 1000 0000 0000 0000
00940     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x8000; // 1000 0000 0000 0000
00941 
00942     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00943     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00944     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00945     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00946 
00947     jointControlFsmMap["r2/left_leg/joint0"]->getFaults(status);
00948     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
00949     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getFaults(status);
00950     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
00951     jointControlFsmMap["r2/left_arm/joint0"]->getFaults(status);
00952     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
00953     jointControlFsmMap["r2/neck/joint0"]->getFaults(status);
00954     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
00955 
00956     // UnFake CommAlive is dead
00957     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
00958     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9000; // 1001 0000 0000 0000
00959     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
00960     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9000; // 1001 0000 0000 0000
00961 
00962     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00963     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00964     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00965     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00966 
00967     jointControlFsmMap["r2/left_leg/joint0"]->getFaults(status);
00968     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level);
00969     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getFaults(status);
00970     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level);
00971     jointControlFsmMap["r2/left_arm/joint0"]->getFaults(status);
00972     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level);
00973     jointControlFsmMap["r2/neck/joint0"]->getFaults(status);
00974     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level);
00975 
00976     // Fake ProcAlive is dead
00977     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x1000; // 0001 0000 0000 0000
00978     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x1000; // 0001 0000 0000 0000
00979     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x1000; // 0001 0000 0000 0000
00980     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x1000; // 0001 0000 0000 0000
00981 
00982     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
00983     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
00984     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
00985     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
00986 
00987     jointControlFsmMap["r2/left_leg/joint0"]->getFaults(status);
00988     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
00989     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getFaults(status);
00990     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
00991     jointControlFsmMap["r2/left_arm/joint0"]->getFaults(status);
00992     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
00993     jointControlFsmMap["r2/neck/joint0"]->getFaults(status);
00994     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
00995 
00996     // UnFake ProcAlive is dead
00997     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
00998     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9000; // 1001 0000 0000 0000
00999     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9000; // 1001 0000 0000 0000
01000     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9000; // 1001 0000 0000 0000
01001 
01002     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
01003     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
01004     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
01005     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
01006 
01007     jointControlFsmMap["r2/left_leg/joint0"]->getFaults(status);
01008     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level);
01009     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getFaults(status);
01010     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level);
01011     jointControlFsmMap["r2/left_arm/joint0"]->getFaults(status);
01012     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level);
01013     jointControlFsmMap["r2/neck/joint0"]->getFaults(status);
01014     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level);
01015 
01016     // Fake out of Bootloader
01017     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
01018     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg3"].registerValue = 0x0004; // 0000 0000 0000 0100
01019     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg3"].registerValue                   = 0x0004; // 0000 0000 0000 0100
01020     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg3"].registerValue                           = 0x0004; // 0000 0000 0000 0100
01021 
01022     // Fake Embedded MotCom and BridgeEnable
01023     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue                   = 0x9050; // 1001 0000 0101 0000
01024     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg1"].registerValue = 0x9050; // 1001 0000 0101 0000
01025     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg1"].registerValue                   = 0x9050; // 1001 0000 0101 0000
01026     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg1"].registerValue                           = 0x9050; // 1001 0000 0101 0000
01027 
01028     // Fake coeffs loaded and joint faulted
01029     nodeRegisterManager->nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg2"].registerValue                   = 0x0180; // 0000 0001 1000 0000
01030     nodeRegisterManager->nodeRegisterMap["r2/right_leg/gripper/joint0"].status["r2/right_leg/gripper/joint0/StatReg2"].registerValue = 0x0180; // 0000 0001 1000 0000
01031     nodeRegisterManager->nodeRegisterMap["r2/left_arm/joint0"].status["r2/left_arm/joint0/StatReg2"].registerValue                   = 0x0180; // 0000 0001 1000 0000
01032     nodeRegisterManager->nodeRegisterMap["r2/neck/joint0"].status["r2/neck/joint0/StatReg2"].registerValue                           = 0x0180; // 0000 0001 1000 0000
01033 
01034     jointControlFsmMap["r2/left_leg/joint0"]->updateControlModeState();
01035     jointControlFsmMap["r2/right_leg/gripper/joint0"]->updateControlModeState();
01036     jointControlFsmMap["r2/left_arm/joint0"]->updateControlModeState();
01037     jointControlFsmMap["r2/neck/joint0"]->updateControlModeState();
01038 
01039     jointControlFsmMap["r2/left_leg/joint0"]->getFaults(status);
01040     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
01041     jointControlFsmMap["r2/right_leg/gripper/joint0"]->getFaults(status);
01042     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::WARN, status.level);
01043     jointControlFsmMap["r2/left_arm/joint0"]->getFaults(status);
01044     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
01045     jointControlFsmMap["r2/neck/joint0"]->getFaults(status);
01046     EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level);
01047 
01048 
01049 }
01050 
01051 int main(int argc, char** argv)
01052 {
01053     testing::InitGoogleTest(&argc, argv);
01054     return RUN_ALL_TESTS();
01055 }

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