00001 #include <gtest/gtest.h> 00002 #include <boost/shared_ptr.hpp> 00003 #include <boost/make_shared.hpp> 00004 #include "robot_instance/RobotInstanceFactory.h" 00005 #include "robot_instance/CoeffMapLoader.h" 00006 #include "robodyn_mechanisms/JointCommandRigid.h" 00007 #include <ros/package.h> 00008 00009 using namespace std; 00010 00011 std::map<std::string, float> testFloats; 00012 std::map<std::string, uint32_t> testUInt32s; 00013 std::map<std::string, uint16_t> testUInt16s; 00014 CoeffMapPtr motorCoeffMap; 00015 CoeffMapPtr brainstemCoeffMap; 00016 RobotInstancePtr instance; 00017 00018 float getFloat(std::string item) 00019 { 00020 return testFloats[item]; 00021 } 00022 00023 void setFloat(std::string item, float value) 00024 { 00025 testFloats[item] = value; 00026 } 00027 00028 float getUInt16(std::string item) 00029 { 00030 return testUInt16s[item]; 00031 } 00032 00033 void setUInt16(std::string item, uint16_t value) 00034 { 00035 testUInt16s[item] = value; 00036 } 00037 00038 uint32_t getUInt32(std::string item) 00039 { 00040 return testUInt32s[item]; 00041 } 00042 00043 void setUInt32(std::string item, uint32_t value) 00044 { 00045 testUInt32s[item] = value; 00046 } 00047 00048 float getMotorCoeff(std::string item) 00049 { 00050 return motorCoeffMap->operator[](item); 00051 } 00052 00053 bool hasBrainstemCoeff(std::string item) 00054 { 00055 if (brainstemCoeffMap->find(item) != brainstemCoeffMap->end()) 00056 { 00057 return true; 00058 } 00059 00060 return false; 00061 } 00062 00063 float getBrainstemCoeff(std::string item) 00064 { 00065 return brainstemCoeffMap->operator[](item); 00066 } 00067 00068 std::string getCommandFile(std::string mechanism) 00069 { 00070 return instance->configurationSafetyBasePath + instance->APIMAP_PATH + instance->mechanismCommandMap[mechanism]; 00071 } 00072 00073 class JointCommandRigidTest : public ::testing::Test 00074 { 00075 protected: 00076 virtual void SetUp() 00077 { 00078 packagePath = ros::package::getPath("robot_instance"); 00079 configPath = packagePath + "/test/config"; 00080 configSafetyPath = packagePath + "/test/configSafety"; 00081 fileName = "TestRobotInstance.xml"; 00082 instance = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName); 00083 00084 motorCoeffMap = boost::make_shared<CoeffMap>(); 00085 brainstemCoeffMap = boost::make_shared<CoeffMap>(); 00086 CoeffMapLoader::loadElements(instance, motorCoeffMap, brainstemCoeffMap); 00087 00088 testFloats.clear(); 00089 testUInt32s.clear(); 00090 00091 io.getFloat = getFloat; 00092 io.setFloat = setFloat; 00093 io.setUInt16 = setUInt16; 00094 io.getUInt32 = getUInt32; 00095 io.getMotorCoeff = getMotorCoeff; 00096 io.hasBrainstemCoeff = hasBrainstemCoeff; 00097 io.getBrainstemCoeff = getBrainstemCoeff; 00098 io.getCommandFile = getCommandFile; 00099 00100 ros::Time::init(); 00101 } 00102 00103 virtual void TearDown() 00104 { 00105 } 00106 00107 string packagePath, configPath, configSafetyPath, fileName; 00108 JointCommandInterface::IoFunctions io; 00109 }; 00110 00111 TEST_F(JointCommandRigidTest, Constructor) 00112 { 00113 ASSERT_NO_THROW(JointCommandRigid rigidJoint("r2/neck/joint0", io)); 00114 00116 io.getFloat = 0; 00117 ASSERT_THROW(JointCommandRigid rigidJoint("r2/neck/joint0", io), std::invalid_argument); 00118 } 00119 00120 TEST_F(JointCommandRigidTest, LoadCoeffs) 00121 { 00122 JointCommandRigid rigidJoint("r2/neck/joint0", io); 00123 00124 EXPECT_NO_THROW(rigidJoint.loadCoeffs()); 00125 00126 r2_msgs::JointControlData controlMsg; 00127 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED; 00128 EXPECT_NO_THROW(rigidJoint.updateMeasuredState(controlMsg)); 00129 00130 r2_msgs::JointCommand msg; 00131 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK; 00132 00133 msg.header.stamp = ros::Time::now(); 00134 00135 msg.name.resize(1); 00136 msg.desiredPosition.resize(1); 00137 msg.desiredPositionVelocityLimit.resize(1); 00138 msg.feedForwardTorque.resize(1); 00139 msg.proportionalGain.resize(1); 00140 msg.derivativeGain.resize(1); 00141 msg.integralGain.resize(1); 00142 msg.positionLoopTorqueLimit.resize(1); 00143 msg.positionLoopWindupLimit.resize(1); 00144 msg.torqueLoopVelocityLimit.resize(1); 00145 00146 msg.name[0] = "r2/neck/joint0"; 00147 msg.desiredPosition[0] = 1; 00148 msg.desiredPositionVelocityLimit[0] = 2; 00149 msg.feedForwardTorque[0] = 3; 00150 msg.proportionalGain[0] = 4; 00151 msg.derivativeGain[0] = 5; 00152 msg.integralGain[0] = 6; 00153 msg.positionLoopTorqueLimit[0] = 7; 00154 msg.positionLoopWindupLimit[0] = 8; 00155 msg.torqueLoopVelocityLimit[0] = 9; 00156 00157 EXPECT_NO_THROW(rigidJoint.setCommand(msg, controlMsg)); 00158 00159 EXPECT_NO_THROW(rigidJoint.getCommandedState()); 00160 00161 EXPECT_NO_THROW(rigidJoint.getCapability()); 00162 } 00163 00164 TEST_F(JointCommandRigidTest, getSimpleMeasuredState) 00165 { 00166 JointCommandRigid rigidJoint("r2/neck/joint0", io); 00167 00168 setFloat("r2/neck/joint0/APS2Unfiltered", 1); 00169 setFloat("r2/neck/joint0/JointVelocity", 2); 00170 setFloat("r2/neck/joint0/EncPos", 5); 00171 setFloat("r2/neck/joint0/EncVel", 6); 00172 00173 r2_msgs::JointControlData controlMsg; 00174 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED; 00175 rigidJoint.updateMeasuredState(controlMsg); 00176 sensor_msgs::JointState msg = rigidJoint.getSimpleMeasuredState(); 00177 00178 EXPECT_EQ(1, msg.name.size()); 00179 EXPECT_EQ(1, msg.position.size()); 00180 EXPECT_EQ(1, msg.velocity.size()); 00181 EXPECT_EQ(1, msg.effort.size()); 00182 00183 EXPECT_STREQ("r2/neck/joint0", msg.name[0].c_str()); 00184 EXPECT_FLOAT_EQ(1, msg.position[0]); 00185 EXPECT_FLOAT_EQ(2, msg.velocity[0]); 00186 EXPECT_FLOAT_EQ(0, msg.effort[0]); 00187 } 00188 00189 TEST_F(JointCommandRigidTest, getCommandedState) 00190 { 00191 JointCommandRigid rigidJoint("r2/neck/joint0", io); 00192 00193 setFloat("r2/neck/joint0/PosCom", 1); 00194 setFloat("r2/neck/joint0/PosComVelocity", 2); 00195 00196 r2_msgs::JointCommand msg = rigidJoint.getCommandedState(); 00197 00198 EXPECT_EQ(1, msg.name.size()); 00199 EXPECT_EQ(1, msg.desiredPosition.size()); 00200 EXPECT_EQ(1, msg.desiredPositionVelocityLimit.size()); 00201 EXPECT_EQ(1, msg.feedForwardTorque.size()); 00202 EXPECT_EQ(1, msg.proportionalGain.size()); 00203 EXPECT_EQ(1, msg.derivativeGain.size()); 00204 EXPECT_EQ(1, msg.integralGain.size()); 00205 EXPECT_EQ(1, msg.positionLoopTorqueLimit.size()); 00206 EXPECT_EQ(1, msg.positionLoopWindupLimit.size()); 00207 EXPECT_EQ(1, msg.torqueLoopVelocityLimit.size()); 00208 00209 EXPECT_STREQ("r2/neck/joint0", msg.name[0].c_str()); 00210 EXPECT_FLOAT_EQ(1, msg.desiredPosition[0]); 00211 EXPECT_FLOAT_EQ(2, msg.desiredPositionVelocityLimit[0]); 00212 EXPECT_FLOAT_EQ(0, msg.feedForwardTorque[0]); 00213 EXPECT_FLOAT_EQ(0, msg.proportionalGain[0]); 00214 EXPECT_FLOAT_EQ(0, msg.derivativeGain[0]); 00215 EXPECT_FLOAT_EQ(0, msg.integralGain[0]); 00216 EXPECT_FLOAT_EQ(0, msg.positionLoopTorqueLimit[0]); 00217 EXPECT_FLOAT_EQ(0, msg.positionLoopWindupLimit[0]); 00218 EXPECT_FLOAT_EQ(0, msg.torqueLoopVelocityLimit[0]); 00219 } 00220 00221 TEST_F(JointCommandRigidTest, setCommand) 00222 { 00223 JointCommandRigid rigidJoint("r2/neck/joint0", io); 00224 00225 r2_msgs::JointCommand msg; 00226 r2_msgs::JointControlData controlMsg; 00227 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK; 00228 00229 msg.header.stamp = ros::Time::now(); 00230 00231 msg.name.resize(1); 00232 msg.desiredPosition.resize(1); 00233 msg.desiredPositionVelocityLimit.resize(1); 00234 msg.feedForwardTorque.resize(1); 00235 msg.proportionalGain.resize(1); 00236 msg.derivativeGain.resize(1); 00237 msg.integralGain.resize(1); 00238 msg.positionLoopTorqueLimit.resize(1); 00239 msg.positionLoopWindupLimit.resize(1); 00240 msg.torqueLoopVelocityLimit.resize(1); 00241 00242 msg.name[0] = "r2/neck/joint0"; 00243 msg.desiredPosition[0] = 1; 00244 msg.desiredPositionVelocityLimit[0] = 2; 00245 00246 rigidJoint.setCommand(msg, controlMsg); 00247 00248 EXPECT_FLOAT_EQ(1, getFloat("r2/neck/joint0/PosCom")); 00249 EXPECT_FLOAT_EQ(2, getFloat("r2/neck/joint0/PosComVelocity")); 00250 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/TorqueCom")); 00251 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/JointStiffness")); 00252 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/JointDamping")); 00253 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/PLKi")); 00254 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/TorqueLim")); 00255 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/PLWindupLim")); 00256 EXPECT_FLOAT_EQ(0, getFloat("/neck/joint0/TLVelLimit")); 00257 00258 msg.desiredPositionVelocityLimit.resize(0); 00259 msg.proportionalGain.resize(0); 00260 msg.integralGain.resize(0); 00261 msg.positionLoopWindupLimit.resize(0); 00262 00263 msg.desiredPosition[0] = 10; 00264 00265 rigidJoint.setCommand(msg, controlMsg); 00266 00267 EXPECT_FLOAT_EQ(10, getFloat("r2/neck/joint0/PosCom")); 00268 EXPECT_FLOAT_EQ(2, getFloat("r2/neck/joint0/PosComVelocity")); 00269 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/TorqueCom")); 00270 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/JointStiffness")); 00271 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/JointDamping")); 00272 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/PLKi")); 00273 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/TorqueLim")); 00274 EXPECT_FLOAT_EQ(0, getFloat("r2/neck/joint0/PLWindupLim")); 00275 EXPECT_FLOAT_EQ(0, getFloat("/neck/joint7/TLVelLimit")); 00276 } 00277 00278 TEST_F(JointCommandRigidTest, setCommand_BadMsg) 00279 { 00280 JointCommandRigid rigidJoint("r2/neck/joint0", io); 00281 00282 r2_msgs::JointCommand msg; 00283 r2_msgs::JointControlData controlMsg; 00284 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK; 00285 00287 EXPECT_THROW(rigidJoint.setCommand(msg, controlMsg), std::runtime_error); 00288 00289 msg.header.stamp = ros::Time::now(); 00290 00291 msg.name.resize(1); 00292 msg.desiredPosition.resize(1); 00293 msg.desiredPositionVelocityLimit.resize(1); 00294 msg.feedForwardTorque.resize(1); 00295 msg.proportionalGain.resize(1); 00296 msg.derivativeGain.resize(1); 00297 msg.integralGain.resize(1); 00298 msg.positionLoopTorqueLimit.resize(1); 00299 msg.positionLoopWindupLimit.resize(1); 00300 msg.torqueLoopVelocityLimit.resize(1); 00301 00302 msg.name[0] = "r2/neck/joint1"; 00303 msg.desiredPosition[0] = 1; 00304 msg.desiredPositionVelocityLimit[0] = 2; 00305 msg.feedForwardTorque[0] = 3; 00306 msg.proportionalGain[0] = 4; 00307 msg.derivativeGain[0] = 5; 00308 msg.integralGain[0] = 6; 00309 msg.positionLoopTorqueLimit[0] = 7; 00310 msg.positionLoopWindupLimit[0] = 8; 00311 msg.torqueLoopVelocityLimit[0] = 9; 00312 00314 EXPECT_THROW(rigidJoint.setCommand(msg, controlMsg), std::runtime_error); 00315 00316 msg.name[0] = "r2/neck/joint0"; 00317 msg.desiredPosition.push_back(11); 00318 00320 EXPECT_THROW(rigidJoint.setCommand(msg, controlMsg), std::runtime_error); 00321 } 00322 00323 TEST_F(JointCommandRigidTest, getCapability) 00324 { 00325 JointCommandRigid rigidJoint("r2/neck/joint0", io); 00326 00327 r2_msgs::JointCapability msg = rigidJoint.getCapability(); 00328 00329 EXPECT_EQ(1, msg.name.size()); 00330 EXPECT_EQ(1, msg.positionLimitMax.size()); 00331 EXPECT_EQ(1, msg.positionLimitMin.size()); 00332 EXPECT_EQ(1, msg.torqueLimit.size()); 00333 00334 EXPECT_STREQ("r2/neck/joint0", msg.name[0].c_str()); 00335 EXPECT_FLOAT_EQ(0.03, msg.positionLimitMax[0]); 00336 EXPECT_FLOAT_EQ(-1.1, msg.positionLimitMin[0]); 00337 EXPECT_FLOAT_EQ(0, msg.torqueLimit[0]); 00338 } 00339 00340 int main(int argc, char** argv) 00341 { 00342 testing::InitGoogleTest(&argc, argv); 00343 return RUN_ALL_TESTS(); 00344 }