robot_instance: NodeRegisterManager

Go to the documentation of this file.
00001 #include <iostream>
00002 #include <gtest/gtest.h>
00003 #include <boost/shared_ptr.hpp>
00004 #include <boost/make_shared.hpp>
00005 #include "robot_instance/RobotInstanceFactory.h"
00006 #include "robot_instance/NodeRegisterManager.h"
00007 #include <ros/package.h>
00008 
00009 using namespace std;
00010 
00011 class NodeRegisterManagerTest : public ::testing::Test
00012 {
00013 protected:
00014     virtual void SetUp()
00015     {
00016         packagePath      = ros::package::getPath("robot_instance");
00017         configPath       = packagePath + "/test/config";
00018         configSafetyPath = packagePath + "/test/configSafety";
00019         fileName         = "TestRobotInstance.xml";
00020         instance         = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName);
00021     }
00022 
00023     virtual void TearDown()
00024     {
00025     }
00026 
00027     string packagePath, configPath, configSafetyPath, fileName;
00028     NodeRegisterManager nodeRegisterManager;
00029     RobotInstancePtr    instance;
00030 };
00031 
00032 TEST_F(NodeRegisterManagerTest, AddNode)
00033 {
00034     nodeRegisterManager.addNode("r2/left_leg/joint0", instance->configurationSafetyBasePath + RobotInstance::REGISTER_PATH + instance->mechanismRegisterMap["r2/left_leg/joint0"]);
00035 
00036     EXPECT_EQ(1, nodeRegisterManager.nodeRegisterMap.size());
00037     EXPECT_EQ(1, nodeRegisterManager.nodeRegisterMap.count("r2/left_leg/joint0"));
00038     EXPECT_EQ(4, nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].control.size());
00039     EXPECT_EQ(1, nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].control.count("r2/left_leg/joint0/CtrlReg1"));
00040     EXPECT_EQ(1, nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].control.count("r2/left_leg/joint0/CtrlReg2"));
00041     EXPECT_EQ(3, nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].status.size());
00042     EXPECT_EQ(1, nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].status.count("r2/left_leg/joint0/StatReg1"));
00043     EXPECT_EQ(1, nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].status.count("r2/left_leg/joint0/StatReg2"));
00044 
00045     EXPECT_EQ(0xA000, nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].control["r2/left_leg/joint0/CtrlReg1"].registerValue);
00046     EXPECT_EQ(0x5000, nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].control["r2/left_leg/joint0/CtrlReg2"].registerValue);
00047     EXPECT_EQ(0x0,    nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue);
00048     EXPECT_EQ(0x0,    nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].status["r2/left_leg/joint0/StatReg1"].registerValue);
00049 
00050     EXPECT_TRUE(nodeRegisterManager.nodeRegisterMap["r2/left_leg/joint0"].nodeType.compare("SeriesElastic") == 0);
00051 }
00052 
00053 TEST_F(NodeRegisterManagerTest, getNodeCount)
00054 {
00055     EXPECT_EQ(0, nodeRegisterManager.getNodeCount());
00056     nodeRegisterManager.addNode("r2/left_leg/joint0", instance->configurationSafetyBasePath + RobotInstance::REGISTER_PATH + instance->mechanismRegisterMap["r2/left_leg/joint0"]);
00057     EXPECT_EQ(1, nodeRegisterManager.getNodeCount());
00058 }
00059 
00060 TEST_F(NodeRegisterManagerTest, getRegisterInfo)
00061 {
00062     nodeRegisterManager.addNode("r2/left_leg/joint0", instance->configurationSafetyBasePath + RobotInstance::REGISTER_PATH + instance->mechanismRegisterMap["r2/left_leg/joint0"]);
00063 
00064     EXPECT_EQ(0xA000, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg1").registerValue);
00065     EXPECT_EQ(0x5000, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg2").registerValue);
00066     EXPECT_EQ(0x0,    nodeRegisterManager.getStatusInfo("r2/left_leg/joint0",  "StatReg1").registerValue);
00067     EXPECT_EQ(0x0,    nodeRegisterManager.getStatusInfo("r2/left_leg/joint0",  "StatReg2").registerValue);
00068 
00069     EXPECT_THROW(nodeRegisterManager.getControlInfo("r2/left_leg/point0", "CtrlReg1"),         std::runtime_error);
00070     EXPECT_THROW(nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "ControlRegister1"), std::runtime_error);
00071     EXPECT_THROW(nodeRegisterManager.getStatusInfo("r2/left_leg/point0",  "StatReg1"),         std::runtime_error);
00072     EXPECT_THROW(nodeRegisterManager.getStatusInfo("r2/left_leg/joint0",  "StatusRegister1"),  std::runtime_error);
00073 }
00074 
00075 TEST_F(NodeRegisterManagerTest, hasProperty)
00076 {
00077     nodeRegisterManager.addNode("r2/left_leg/joint0", instance->configurationSafetyBasePath + RobotInstance::REGISTER_PATH + instance->mechanismRegisterMap["r2/left_leg/joint0"]);
00078 
00079     EXPECT_TRUE(nodeRegisterManager.hasControlProperty("r2/left_leg/joint0", "MotorEnable"));
00080     EXPECT_TRUE(nodeRegisterManager.hasControlProperty("r2/left_leg/joint0", "ControlMode"));
00081     EXPECT_FALSE(nodeRegisterManager.hasControlProperty("r2/left_leg/joint0", "ASDF"));
00082 
00083     EXPECT_TRUE(nodeRegisterManager.hasStatusProperty("r2/left_leg/joint0", "MotorEnable"));
00084     EXPECT_TRUE(nodeRegisterManager.hasStatusProperty("r2/left_leg/joint0", "JointFault"));
00085     EXPECT_FALSE(nodeRegisterManager.hasStatusProperty("r2/left_leg/joint0", "ASDF"));
00086 
00087     EXPECT_THROW(nodeRegisterManager.hasControlProperty("r2/left_leg/point0", "MotorEnable"), std::runtime_error);
00088     EXPECT_THROW(nodeRegisterManager.hasStatusProperty("r2/left_leg/point0", "JointFault"), std::runtime_error);
00089 }
00090 
00091 TEST_F(NodeRegisterManagerTest, getSetControlValue)
00092 {
00093     nodeRegisterManager.addNode("r2/left_leg/joint0", instance->configurationSafetyBasePath + RobotInstance::REGISTER_PATH + instance->mechanismRegisterMap["r2/left_leg/joint0"]);
00094 
00095     EXPECT_EQ(0xA000, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg1").registerValue);
00096     EXPECT_EQ(0,      nodeRegisterManager.getControlValue("r2/left_leg/joint0", "MotorEnable"));
00097     nodeRegisterManager.setControlValue("r2/left_leg/joint0", "MotorEnable", 1);
00098     EXPECT_EQ(1,      nodeRegisterManager.getControlValue("r2/left_leg/joint0", "MotorEnable"));
00099     EXPECT_EQ(0xA001, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg1").registerValue);
00100 
00101     EXPECT_EQ(0x5000, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg2").registerValue);
00102     EXPECT_EQ(0, nodeRegisterManager.getControlValue("r2/left_leg/joint0", "ControlMode"));
00103     nodeRegisterManager.setControlValue("r2/left_leg/joint0", "ControlMode", 3);
00104     EXPECT_EQ(3, nodeRegisterManager.getControlValue("r2/left_leg/joint0", "ControlMode"));
00105     EXPECT_EQ(0x5006, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg2").registerValue);
00106 
00107     EXPECT_THROW(nodeRegisterManager.getControlValue("r2/left_leg/point0", "MotorEnable"), std::runtime_error);
00108     EXPECT_THROW(nodeRegisterManager.getControlValue("r2/left_leg/joint0", "ASDF"), std::runtime_error);
00109     EXPECT_THROW(nodeRegisterManager.setControlValue("r2/left_leg/point0", "MotorEnable", 0), std::runtime_error);
00110     EXPECT_THROW(nodeRegisterManager.setControlValue("r2/left_leg/joint0", "ASDF", 0), std::runtime_error);
00111 }
00112 
00113 TEST_F(NodeRegisterManagerTest, resetControlValues)
00114 {
00115     nodeRegisterManager.addNode("r2/left_leg/joint0", instance->configurationSafetyBasePath + RobotInstance::REGISTER_PATH + instance->mechanismRegisterMap["r2/left_leg/joint0"]);
00116 
00117     EXPECT_EQ(0xA000, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg1").registerValue);
00118     EXPECT_EQ(0x5000, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg2").registerValue);
00119 
00120     nodeRegisterManager.setControlValue("r2/left_leg/joint0", "MotorEnable", 1);
00121     nodeRegisterManager.setControlValue("r2/left_leg/joint0", "ControlMode", 3);
00122 
00123     EXPECT_EQ(0xA001, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg1").registerValue);
00124     EXPECT_EQ(0x5006, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg2").registerValue);
00125 
00126     nodeRegisterManager.resetControlValues("r2/left_leg/joint0");
00127 
00128     EXPECT_EQ(0xA001, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg1").registerValue);
00129     EXPECT_EQ(0x5000, nodeRegisterManager.getControlInfo("r2/left_leg/joint0", "CtrlReg2").registerValue);
00130 }
00131 
00132 TEST_F(NodeRegisterManagerTest, Show)
00133 {
00134     nodeRegisterManager.addNode("r2/left_leg/joint0", instance->configurationSafetyBasePath + RobotInstance::REGISTER_PATH + instance->mechanismRegisterMap["r2/left_leg/joint0"]);
00135 
00136     nodeRegisterManager.show("r2/left_leg/joint0");
00137 }
00138 
00139 int main(int argc, char** argv)
00140 {
00141     testing::InitGoogleTest(&argc, argv);
00142     return RUN_ALL_TESTS();
00143 }