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 <ros/package.h> 00007 00008 using namespace std; 00009 00010 class RobotInstanceTest : public ::testing::Test 00011 { 00012 protected: 00013 virtual void SetUp() 00014 { 00015 packagePath = ros::package::getPath("robot_instance"); 00016 configPath = packagePath + "/test/config"; 00017 configSafetyPath = packagePath + "/test/configSafety"; 00018 fileName = "TestRobotInstance.xml"; 00019 } 00020 00021 virtual void TearDown() 00022 { 00023 } 00024 00025 string packagePath, configPath, configSafetyPath, fileName; 00026 RobotInstancePtr instance; 00027 }; 00028 00029 TEST_F(RobotInstanceTest, Factory) 00030 { 00031 instance = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName); 00032 00033 EXPECT_STREQ("TestRobotInstance", instance->robotInstance.c_str()); 00034 EXPECT_STREQ("TestMachineName", instance->machineName.c_str()); 00035 EXPECT_EQ(2, instance->crew.size()); 00036 00037 EXPECT_STREQ("TestKinematicModel.urdf.xml", instance->kinematicModelFile.c_str()); 00038 EXPECT_STREQ("TestDynamicModel.urdf.xml", instance->dynamicModelFile.c_str()); 00039 00040 EXPECT_STREQ("TestMasterTokens.xml", instance->masterTokenFile.c_str()); 00041 00042 EXPECT_EQ(10, instance->mechanisms.size()); 00043 EXPECT_EQ(2, instance->devices.size()); 00044 EXPECT_EQ(17, instance->commandableJoints.size()); 00045 00046 EXPECT_EQ(9, instance->nodeTokenMap.size()); 00047 EXPECT_EQ(10, instance->mechanismRegisterMap.size()); 00048 EXPECT_EQ(10, instance->mechanismClassFileMap.size()); 00049 EXPECT_EQ(10, instance->mechanismJointFileMap.size()); 00050 EXPECT_EQ(10, instance->mechanismClassSafetyFileMap.size()); 00051 EXPECT_EQ(10, instance->mechanismJointSafetyFileMap.size()); 00052 EXPECT_EQ(10, instance->mechanismCommandMap.size()); 00053 EXPECT_EQ(10, instance->mechanismControlMap.size()); 00054 EXPECT_EQ(6, instance->mechanismScheduleMap.size()); 00055 EXPECT_EQ(1, instance->mechanismSetpointMap.size()); 00056 EXPECT_EQ(6, instance->mechanismNodeMap.size()); 00057 EXPECT_EQ(10, instance->mechanismJointsMap.size()); 00058 EXPECT_EQ(4, instance->mechanismActuatorsMap.size()); 00059 00060 EXPECT_EQ(16, instance->jointScheduleNodeMap.size()); 00061 EXPECT_EQ(14, instance->actuatorScheduleNodeMap.size()); 00062 EXPECT_EQ(2, instance->deviceNodeMap.size()); 00063 EXPECT_EQ(2, instance->deviceScheduleMap.size()); 00064 EXPECT_EQ(1, instance->deviceRegisterMap.size()); 00065 00066 EXPECT_EQ(10, instance->mechanismTypeMap.size()); 00067 EXPECT_EQ(22, instance->jointTypeMap.size()); 00068 EXPECT_EQ(22, instance->jointCommandPathMap.size()); 00069 00070 EXPECT_EQ(1, instance->componentCpfsMap.size()); 00071 EXPECT_STREQ("RobotInstance.cpf", instance->componentCpfsMap["RobotInstance"][0].first.c_str()); 00072 EXPECT_STREQ("safety", instance->componentCpfsMap["RobotInstance"][0].second.c_str()); 00073 EXPECT_STREQ("Additional.cpf", instance->componentCpfsMap["RobotInstance"][1].first.c_str()); 00074 EXPECT_STREQ("nonSafety", instance->componentCpfsMap["RobotInstance"][1].second.c_str()); 00075 00077 EXPECT_STREQ("TestTokens.xml", instance->nodeTokenMap["/left_leg/node0"].c_str()); 00078 EXPECT_STREQ("TestTokens.xml", instance->nodeTokenMap["/left_leg/node1"].c_str()); 00079 EXPECT_STREQ("TestTokens.xml", instance->nodeTokenMap["/right_leg/node7"].c_str()); 00080 EXPECT_STREQ("TestTokens.xml", instance->nodeTokenMap["/left_arm/node0"].c_str()); 00081 EXPECT_STREQ("TestTokens.xml", instance->nodeTokenMap["/right_arm/forearm"].c_str()); 00082 EXPECT_STREQ("TestTokens.xml", instance->nodeTokenMap["/right_arm/palm"].c_str()); 00083 EXPECT_STREQ("TestTokens.xml", instance->nodeTokenMap["/body/pdmc"].c_str()); 00084 00086 EXPECT_STREQ("r2/left_leg/joint0", instance->mechanisms[0].c_str()); 00087 EXPECT_STREQ("simple", instance->mechanismTypeMap["r2/left_leg/joint0"].c_str()); 00088 EXPECT_STREQ("/left_leg/node0", instance->mechanismNodeMap["r2/left_leg/joint0"].c_str()); 00089 EXPECT_STREQ("TestTurboRegisters.xml", instance->mechanismRegisterMap["r2/left_leg/joint0"].c_str()); 00090 EXPECT_STREQ("TestClass.xml", instance->mechanismClassFileMap["r2/left_leg/joint0"].c_str()); 00091 EXPECT_STREQ("TestJoint.xml", instance->mechanismJointFileMap["r2/left_leg/joint0"].c_str()); 00092 EXPECT_STREQ("TestSafety.xml", instance->mechanismClassSafetyFileMap["r2/left_leg/joint0"].c_str()); 00093 EXPECT_STREQ("TestJointSafety.xml", instance->mechanismJointSafetyFileMap["r2/left_leg/joint0"].c_str()); 00094 EXPECT_STREQ("TestSeriesElasticCommandApiMap.xml", instance->mechanismCommandMap["r2/left_leg/joint0"].c_str()); 00095 EXPECT_STREQ("TestControlApiMap.xml", instance->mechanismControlMap["r2/left_leg/joint0"].c_str()); 00096 EXPECT_EQ(1, instance->mechanismJointsMap["r2/left_leg/joint0"].size()); 00097 EXPECT_EQ(0, instance->jointScheduleNodeMap["r2/left_leg/joint0"].size()); 00098 EXPECT_STREQ("active", instance->jointTypeMap["r2/left_leg/joint0"].c_str()); 00099 EXPECT_EQ(0, instance->mechanismActuatorsMap["r2/left_leg/joint0"].size()); 00100 EXPECT_EQ(0, instance->actuatorScheduleNodeMap["r2/left_leg/joint0"].size()); 00101 EXPECT_STREQ("robodyn", instance->jointCommandPathMap["r2/left_leg/joint0"].c_str()); 00102 00104 EXPECT_STREQ("r2/left_leg/joint1", instance->mechanisms[1].c_str()); 00105 EXPECT_STREQ("simple", instance->mechanismTypeMap["r2/left_leg/joint1"].c_str()); 00106 EXPECT_STREQ("/left_leg/node1", instance->mechanismNodeMap["r2/left_leg/joint1"].c_str()); 00107 EXPECT_STREQ("TestTurboRegisters.xml", instance->mechanismRegisterMap["r2/left_leg/joint1"].c_str()); 00108 EXPECT_STREQ("TestClass.xml", instance->mechanismClassFileMap["r2/left_leg/joint1"].c_str()); 00109 EXPECT_STREQ("TestJoint.xml", instance->mechanismJointFileMap["r2/left_leg/joint1"].c_str()); 00110 EXPECT_STREQ("TestSafety.xml", instance->mechanismClassSafetyFileMap["r2/left_leg/joint1"].c_str()); 00111 EXPECT_STREQ("TestJointSafety.xml", instance->mechanismJointSafetyFileMap["r2/left_leg/joint1"].c_str()); 00112 EXPECT_STREQ("TestSeriesElasticCommandApiMap.xml", instance->mechanismCommandMap["r2/left_leg/joint1"].c_str()); 00113 EXPECT_STREQ("TestControlApiMap.xml", instance->mechanismControlMap["r2/left_leg/joint0"].c_str()); 00114 EXPECT_EQ(1, instance->mechanismJointsMap["r2/left_leg/joint1"].size()); 00115 EXPECT_EQ(0, instance->jointScheduleNodeMap["r2/left_leg/joint1"].size()); 00116 EXPECT_STREQ("active", instance->jointTypeMap["r2/left_leg/joint1"].c_str()); 00117 EXPECT_EQ(0, instance->mechanismActuatorsMap["r2/left_leg/joint1"].size()); 00118 EXPECT_EQ(0, instance->actuatorScheduleNodeMap["r2/left_leg/joint1"].size()); 00119 EXPECT_STREQ("robodyn", instance->jointCommandPathMap["r2/left_leg/joint1"].c_str()); 00120 00122 EXPECT_STREQ("r2/right_leg/gripper/joint0", instance->mechanisms[2].c_str()); 00123 EXPECT_STREQ("simple", instance->mechanismTypeMap["r2/right_leg/gripper/joint0"].c_str()); 00124 EXPECT_STREQ("/right_leg/node7", instance->mechanismNodeMap["r2/right_leg/gripper/joint0"].c_str()); 00125 EXPECT_STREQ("TestGripperRegisters.xml", instance->mechanismRegisterMap["r2/right_leg/gripper/joint0"].c_str()); 00126 EXPECT_STREQ("TestClass.xml", instance->mechanismClassFileMap["r2/right_leg/gripper/joint0"].c_str()); 00127 EXPECT_STREQ("TestJoint.xml", instance->mechanismJointFileMap["r2/right_leg/gripper/joint0"].c_str()); 00128 EXPECT_STREQ("TestSafety.xml", instance->mechanismClassSafetyFileMap["r2/right_leg/gripper/joint0"].c_str()); 00129 EXPECT_STREQ("TestJointSafety.xml", instance->mechanismJointSafetyFileMap["r2/right_leg/gripper/joint0"].c_str()); 00130 EXPECT_STREQ("TestGripperCommandApiMap.xml", instance->mechanismCommandMap["r2/right_leg/gripper/joint0"].c_str()); 00131 EXPECT_STREQ("TestControlApiMap.xml", instance->mechanismControlMap["r2/left_leg/joint0"].c_str()); 00132 EXPECT_STREQ("TestSetpoint.xml", instance->mechanismSetpointMap["r2/right_leg/gripper/joint0"].c_str()); 00133 EXPECT_EQ(1, instance->mechanismJointsMap["r2/right_leg/gripper/joint0"].size()); 00134 EXPECT_EQ(0, instance->jointScheduleNodeMap["r2/right_leg/gripper/joint0"].size()); 00135 EXPECT_STREQ("active", instance->jointTypeMap["r2/right_leg/gripper/joint0"].c_str()); 00136 EXPECT_EQ(0, instance->mechanismActuatorsMap["r2/right_leg/gripper/joint0"].size()); 00137 EXPECT_EQ(0, instance->actuatorScheduleNodeMap["r2/right_leg/gripper/joint0"].size()); 00138 EXPECT_STREQ("api", instance->jointCommandPathMap["r2/right_leg/gripper/joint0"].c_str()); 00139 00141 EXPECT_STREQ("r2/left_arm/joint0", instance->mechanisms[3].c_str()); 00142 EXPECT_STREQ("simple", instance->mechanismTypeMap["r2/left_arm/joint0"].c_str()); 00143 EXPECT_STREQ("/left_arm/node0", instance->mechanismNodeMap["r2/left_arm/joint0"].c_str()); 00144 EXPECT_STREQ("TestSuperRegisters.xml", instance->mechanismRegisterMap["r2/left_arm/joint0"].c_str()); 00145 EXPECT_STREQ("TestClass.xml", instance->mechanismClassFileMap["r2/left_arm/joint0"].c_str()); 00146 EXPECT_STREQ("TestJoint.xml", instance->mechanismJointFileMap["r2/left_arm/joint0"].c_str()); 00147 EXPECT_STREQ("TestSafety.xml", instance->mechanismClassSafetyFileMap["r2/left_arm/joint0"].c_str()); 00148 EXPECT_STREQ("TestJointSafety.xml", instance->mechanismJointSafetyFileMap["r2/left_arm/joint0"].c_str()); 00149 EXPECT_STREQ("TestSeriesElasticCommandApiMap.xml", instance->mechanismCommandMap["r2/left_arm/joint0"].c_str()); 00150 EXPECT_STREQ("TestControlApiMap.xml", instance->mechanismControlMap["r2/left_leg/joint0"].c_str()); 00151 EXPECT_EQ(1, instance->mechanismJointsMap["r2/left_arm/joint0"].size()); 00152 EXPECT_EQ(0, instance->jointScheduleNodeMap["r2/left_arm/joint0"].size()); 00153 EXPECT_STREQ("active", instance->jointTypeMap["r2/left_arm/joint0"].c_str()); 00154 EXPECT_EQ(0, instance->mechanismActuatorsMap["r2/left_arm/joint0"].size()); 00155 EXPECT_EQ(0, instance->actuatorScheduleNodeMap["r2/left_arm/joint0"].size()); 00156 EXPECT_STREQ("robodyn", instance->jointCommandPathMap["r2/left_arm/joint1"].c_str()); 00157 00159 EXPECT_STREQ("r2/right_arm/hand/thumb", instance->mechanisms[5].c_str()); 00160 EXPECT_STREQ("complex", instance->mechanismTypeMap["r2/right_arm/hand/thumb"].c_str()); 00161 EXPECT_TRUE(instance->mechanismNodeMap.find("r2/right_arm/hand/thumb") == instance->mechanismNodeMap.end()); 00162 EXPECT_STREQ("TestThumbControl.xml", instance->mechanismRegisterMap["r2/right_arm/hand/thumb"].c_str()); 00163 EXPECT_STREQ("TestThumbClass.xml", instance->mechanismClassFileMap["r2/right_arm/hand/thumb"].c_str()); 00164 EXPECT_STREQ("TestJoint.xml", instance->mechanismJointFileMap["r2/right_arm/hand/thumb"].c_str()); 00165 EXPECT_STREQ("TestSafety.xml", instance->mechanismClassSafetyFileMap["r2/right_arm/hand/thumb"].c_str()); 00166 EXPECT_STREQ("TestJointSafety.xml", instance->mechanismJointSafetyFileMap["r2/right_arm/hand/thumb"].c_str()); 00167 EXPECT_STREQ("TestThumbCommandApiMap.xml", instance->mechanismCommandMap["r2/right_arm/hand/thumb"].c_str()); 00168 EXPECT_STREQ("TestControlApiMap.xml", instance->mechanismControlMap["r2/left_leg/joint0"].c_str()); 00169 EXPECT_EQ(4, instance->mechanismJointsMap["r2/right_arm/hand/thumb"].size()); 00170 EXPECT_STREQ("r2/right_arm/hand/thumb/yaw", instance->mechanismJointsMap["r2/right_arm/hand/thumb"][0].c_str()); 00171 EXPECT_EQ(1, instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/yaw"].size()); 00172 EXPECT_STREQ("TestSchedule.xml", instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/yaw"][0].first.c_str()); 00173 EXPECT_STREQ("/right_arm/palm", instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/yaw"][0].second.c_str()); 00174 EXPECT_STREQ("active", instance->jointTypeMap["r2/right_arm/hand/thumb/yaw"].c_str()); 00175 EXPECT_STREQ("robodyn", instance->jointCommandPathMap["r2/right_arm/hand/thumb/yaw"].c_str()); 00176 EXPECT_STREQ("r2/right_arm/hand/thumb/proximal", instance->mechanismJointsMap["r2/right_arm/hand/thumb"][1].c_str()); 00177 EXPECT_EQ(1, instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/proximal"].size()); 00178 EXPECT_STREQ("TestSchedule.xml", instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/proximal"][0].first.c_str()); 00179 EXPECT_STREQ("/right_arm/palm", instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/proximal"][0].second.c_str()); 00180 EXPECT_STREQ("active", instance->jointTypeMap["r2/right_arm/hand/thumb/proximal"].c_str()); 00181 EXPECT_STREQ("robodyn", instance->jointCommandPathMap["r2/right_arm/hand/thumb/proximal"].c_str()); 00182 EXPECT_STREQ("r2/right_arm/hand/thumb/medial", instance->mechanismJointsMap["r2/right_arm/hand/thumb"][2].c_str()); 00183 EXPECT_EQ(1, instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/medial"].size()); 00184 EXPECT_STREQ("TestSchedule.xml", instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/medial"][0].first.c_str()); 00185 EXPECT_STREQ("/right_arm/palm", instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/medial"][0].second.c_str()); 00186 EXPECT_STREQ("active", instance->jointTypeMap["r2/right_arm/hand/thumb/medial"].c_str()); 00187 EXPECT_STREQ("robodyn", instance->jointCommandPathMap["r2/right_arm/hand/thumb/medial"].c_str()); 00188 EXPECT_STREQ("r2/right_arm/hand/thumb/distal", instance->mechanismJointsMap["r2/right_arm/hand/thumb"][3].c_str()); 00189 EXPECT_EQ(1, instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/distal"].size()); 00190 EXPECT_STREQ("TestSchedule.xml", instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/distal"][0].first.c_str()); 00191 EXPECT_STREQ("/right_arm/palm", instance->jointScheduleNodeMap["r2/right_arm/hand/thumb/distal"][0].second.c_str()); 00192 EXPECT_STREQ("active", instance->jointTypeMap["r2/right_arm/hand/thumb/distal"].c_str()); 00193 EXPECT_STREQ("robodyn", instance->jointCommandPathMap["r2/right_arm/hand/thumb/distal"].c_str()); 00194 EXPECT_EQ(5, instance->mechanismActuatorsMap["r2/right_arm/hand/thumb"].size()); 00195 EXPECT_STREQ("r2/right_arm/hand/thumb/proximal_extensor", instance->mechanismActuatorsMap["r2/right_arm/hand/thumb"][0].c_str()); 00196 EXPECT_EQ(2, instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/proximal_extensor"].size()); 00197 EXPECT_STREQ("TestScheduleForearm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/proximal_extensor"][0].first.c_str()); 00198 EXPECT_STREQ("/right_arm/forearm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/proximal_extensor"][0].second.c_str()); 00199 EXPECT_STREQ("TestSchedulePalm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/proximal_extensor"][1].first.c_str()); 00200 EXPECT_STREQ("/right_arm/palm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/proximal_extensor"][1].second.c_str()); 00201 EXPECT_STREQ("r2/right_arm/hand/thumb/medial_extensor", instance->mechanismActuatorsMap["r2/right_arm/hand/thumb"][1].c_str()); 00202 EXPECT_EQ(2, instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_extensor"].size()); 00203 EXPECT_STREQ("TestScheduleForearm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_extensor"][0].first.c_str()); 00204 EXPECT_STREQ("/right_arm/forearm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_extensor"][0].second.c_str()); 00205 EXPECT_STREQ("TestSchedulePalm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_extensor"][1].first.c_str()); 00206 EXPECT_STREQ("/right_arm/palm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_extensor"][1].second.c_str()); 00207 EXPECT_STREQ("r2/right_arm/hand/thumb/medial_flexor", instance->mechanismActuatorsMap["r2/right_arm/hand/thumb"][2].c_str()); 00208 EXPECT_EQ(2, instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_flexor"].size()); 00209 EXPECT_STREQ("TestScheduleForearm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_flexor"][0].first.c_str()); 00210 EXPECT_STREQ("/right_arm/forearm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_flexor"][0].second.c_str()); 00211 EXPECT_STREQ("TestSchedulePalm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_flexor"][1].first.c_str()); 00212 EXPECT_STREQ("/right_arm/palm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/medial_flexor"][1].second.c_str()); 00213 EXPECT_STREQ("r2/right_arm/hand/thumb/distal_extensor", instance->mechanismActuatorsMap["r2/right_arm/hand/thumb"][3].c_str()); 00214 EXPECT_EQ(2, instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_extensor"].size()); 00215 EXPECT_STREQ("TestScheduleForearm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_extensor"][0].first.c_str()); 00216 EXPECT_STREQ("/right_arm/forearm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_extensor"][0].second.c_str()); 00217 EXPECT_STREQ("TestSchedulePalm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_extensor"][1].first.c_str()); 00218 EXPECT_STREQ("/right_arm/palm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_extensor"][1].second.c_str()); 00219 EXPECT_STREQ("r2/right_arm/hand/thumb/distal_flexor", instance->mechanismActuatorsMap["r2/right_arm/hand/thumb"][4].c_str()); 00220 EXPECT_EQ(2, instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_flexor"].size()); 00221 EXPECT_STREQ("TestScheduleForearm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_flexor"][0].first.c_str()); 00222 EXPECT_STREQ("/right_arm/forearm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_flexor"][0].second.c_str()); 00223 EXPECT_STREQ("TestSchedulePalm.xml", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_flexor"][1].first.c_str()); 00224 EXPECT_STREQ("/right_arm/palm", instance->actuatorScheduleNodeMap["r2/right_arm/hand/thumb/distal_flexor"][1].second.c_str()); 00225 00227 00229 00231 EXPECT_STREQ("r2/right_arm/wrist", instance->mechanisms[8].c_str()); 00232 EXPECT_STREQ("complex", instance->mechanismTypeMap["r2/right_arm/wrist"].c_str()); 00233 EXPECT_TRUE(instance->mechanismNodeMap.find("r2/right_arm/wrist") == instance->mechanismNodeMap.end()); 00234 EXPECT_STREQ("TestWristControl.xml", instance->mechanismRegisterMap["r2/right_arm/wrist"].c_str()); 00235 EXPECT_STREQ("TestRightWristClass.xml", instance->mechanismClassFileMap["r2/right_arm/wrist"].c_str()); 00236 EXPECT_STREQ("TestJoint.xml", instance->mechanismJointFileMap["r2/right_arm/wrist"].c_str()); 00237 EXPECT_STREQ("TestSafety.xml", instance->mechanismClassSafetyFileMap["r2/right_arm/wrist"].c_str()); 00238 EXPECT_STREQ("TestJointSafety.xml", instance->mechanismJointSafetyFileMap["r2/right_arm/wrist"].c_str()); 00239 EXPECT_STREQ("TestWristCommandApiMap.xml", instance->mechanismCommandMap["r2/right_arm/wrist"].c_str()); 00240 EXPECT_STREQ("TestControlApiMap.xml", instance->mechanismControlMap["r2/left_leg/joint0"].c_str()); 00241 EXPECT_EQ(2, instance->mechanismJointsMap["r2/right_arm/wrist"].size()); 00242 EXPECT_STREQ("r2/right_arm/wrist/pitch", instance->mechanismJointsMap["r2/right_arm/wrist"][0].c_str()); 00243 EXPECT_EQ(1, instance->jointScheduleNodeMap["r2/right_arm/wrist/pitch"].size()); 00244 EXPECT_STREQ("TestSchedule.xml", instance->jointScheduleNodeMap["r2/right_arm/wrist/pitch"][0].first.c_str()); 00245 EXPECT_STREQ("/right_arm/forearm", instance->jointScheduleNodeMap["r2/right_arm/wrist/pitch"][0].second.c_str()); 00246 EXPECT_STREQ("active", instance->jointTypeMap["r2/right_arm/wrist/pitch"].c_str()); 00247 EXPECT_STREQ("robodyn", instance->jointCommandPathMap["r2/right_arm/wrist/pitch"].c_str()); 00248 EXPECT_STREQ("r2/right_arm/wrist/yaw", instance->mechanismJointsMap["r2/right_arm/wrist"][1].c_str()); 00249 EXPECT_EQ(1, instance->jointScheduleNodeMap["r2/right_arm/wrist/yaw"].size()); 00250 EXPECT_STREQ("TestSchedule.xml", instance->jointScheduleNodeMap["r2/right_arm/wrist/yaw"][0].first.c_str()); 00251 EXPECT_STREQ("/right_arm/forearm", instance->jointScheduleNodeMap["r2/right_arm/wrist/yaw"][0].second.c_str()); 00252 EXPECT_STREQ("active", instance->jointTypeMap["r2/right_arm/wrist/yaw"].c_str()); 00253 EXPECT_STREQ("robodyn", instance->jointCommandPathMap["r2/right_arm/wrist/yaw"].c_str()); 00254 EXPECT_EQ(2, instance->mechanismActuatorsMap["r2/right_arm/wrist"].size()); 00255 EXPECT_STREQ("r2/right_arm/wrist/thumbside", instance->mechanismActuatorsMap["r2/right_arm/wrist"][0].c_str()); 00256 EXPECT_EQ(1, instance->actuatorScheduleNodeMap["r2/right_arm/wrist/littleside"].size()); 00257 EXPECT_STREQ("TestSchedule.xml", instance->actuatorScheduleNodeMap["r2/right_arm/wrist/littleside"][0].first.c_str()); 00258 EXPECT_STREQ("/right_arm/forearm", instance->actuatorScheduleNodeMap["r2/right_arm/wrist/littleside"][0].second.c_str()); 00259 EXPECT_STREQ("r2/right_arm/wrist/littleside", instance->mechanismActuatorsMap["r2/right_arm/wrist"][1].c_str()); 00260 EXPECT_EQ(1, instance->actuatorScheduleNodeMap["r2/right_arm/wrist/thumbside"].size()); 00261 EXPECT_STREQ("TestSchedule.xml", instance->actuatorScheduleNodeMap["r2/right_arm/wrist/thumbside"][0].first.c_str()); 00262 EXPECT_STREQ("/right_arm/forearm", instance->actuatorScheduleNodeMap["r2/right_arm/wrist/thumbside"][0].second.c_str()); 00263 00264 EXPECT_STREQ("r2/body/pdmc", instance->devices[0].c_str()); 00265 EXPECT_STREQ("/body/pdmc", instance->deviceNodeMap["r2/body/pdmc"].c_str()); 00266 EXPECT_STREQ("TestSchedule.xml", instance->deviceScheduleMap["r2/body/pdmc"].c_str()); 00267 EXPECT_STREQ("TestPdmcRegisters.xml", instance->deviceRegisterMap["r2/body/pdmc"].c_str()); 00268 EXPECT_STREQ("r2/right_arm/medusa", instance->devices[1].c_str()); 00269 EXPECT_STREQ("/right_arm/forearm", instance->deviceNodeMap["r2/right_arm/medusa"].c_str()); 00270 EXPECT_STREQ("TestSchedule.xml", instance->deviceScheduleMap["r2/right_arm/medusa"].c_str()); 00271 EXPECT_TRUE(instance->deviceRegisterMap.find("r2/right_arm/medusa") == instance->deviceRegisterMap.end()); 00272 } 00273 00274 TEST_F(RobotInstanceTest, LiveCoeffs) 00275 { 00276 instance = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName); 00277 00278 EXPECT_TRUE(instance->liveCoeffs.find("asdf") == instance->liveCoeffs.end()); 00279 EXPECT_THROW(instance->liveCoeffs.at("asdf"), std::out_of_range); 00280 instance->liveCoeffs["asdf"] = 42.1; 00281 EXPECT_TRUE(instance->liveCoeffs.find("asdf") != instance->liveCoeffs.end()); 00282 EXPECT_NO_THROW(instance->liveCoeffs.at("asdf")); 00283 EXPECT_FLOAT_EQ(42.1, instance->liveCoeffs["asdf"]); 00284 00285 EXPECT_TRUE(instance->liveCoeffs.find("qwer") == instance->liveCoeffs.end()); 00286 EXPECT_THROW(instance->liveCoeffs.at("qwer"), std::out_of_range); 00287 instance->liveCoeffs["qwer"] = 12.4; 00288 EXPECT_TRUE(instance->liveCoeffs.find("qwer") != instance->liveCoeffs.end()); 00289 EXPECT_NO_THROW(instance->liveCoeffs.at("qwer")); 00290 EXPECT_FLOAT_EQ(12.4, instance->liveCoeffs["qwer"]); 00291 } 00292 00293 TEST_F(RobotInstanceTest, Minimal) 00294 { 00295 fileName = "TestRobotInstanceMinimal.xml"; 00296 EXPECT_NO_THROW(instance = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName)); 00297 } 00298 00299 int main(int argc, char** argv) 00300 { 00301 testing::InitGoogleTest(&argc, argv); 00302 return RUN_ALL_TESTS(); 00303 }