test_robot_lib.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_robot_lib/sr_motor_hand_lib.hpp"
00028 #include <gtest/gtest.h>
00029 #include <ros/ros.h>
00030 
00031 #define error_flag_names palm_0200_edc_error_flag_names
00032 #define STATUS_TYPE ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS
00033 #define COMMAND_TYPE ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND
00034 
00035 class HandLibTestProtected : public shadow_robot::SrMotorHandLib<STATUS_TYPE, COMMAND_TYPE>
00036 {
00037 public:
00038   HandLibTestProtected(pr2_hardware_interface::HardwareInterface *hw)
00039     : shadow_robot::SrMotorHandLib<STATUS_TYPE, COMMAND_TYPE>(hw)
00040   {};
00041 
00042   ~HandLibTestProtected()
00043   {};
00044 
00045 public: using shadow_robot::SrMotorHandLib<STATUS_TYPE, COMMAND_TYPE>::joints_vector;
00046 };
00047 
00048 class HandLibTest
00049 {
00050 public:
00051   pr2_hardware_interface::HardwareInterface *hw;
00052   boost::shared_ptr<HandLibTestProtected> sr_hand_lib;
00053   sr_actuator::SrActuator* actuator;
00054 
00055   HandLibTest()
00056   {
00057     hw = new pr2_hardware_interface::HardwareInterface();
00058     sr_hand_lib= boost::shared_ptr<HandLibTestProtected>( new HandLibTestProtected(hw) );
00059   }
00060 
00061   ~HandLibTest()
00062   {
00063     delete hw;
00064   }
00065 
00066   void check_hw_actuator(std::string name, int motor_id, int id_in_enum, double expected_pos)
00067   {
00068     sr_actuator::SrActuatorState state;
00069 
00070     actuator = static_cast<sr_actuator::SrActuator*>(hw->getActuator(name));
00071     state = (actuator->state_);
00072 
00073     EXPECT_EQ(state.device_id_ , motor_id);
00074     //EXPECT_EQ(state.position_ , expected_pos);
00075   }
00076 
00077 };
00078 
00082 TEST(SrRobotLib, Initialization)
00083 {
00084   boost::shared_ptr< HandLibTest > lib_test = boost::shared_ptr< HandLibTest >( new HandLibTest() );
00085 
00086 //  pr2_hardware_interface::HardwareInterface *hw;
00087 //  hw = new pr2_hardware_interface::HardwareInterface();
00088 //  boost::shared_ptr< shadow_robot::SrMotorHandLib<STATUS_TYPE, COMMAND_TYPE> > lib_test = boost::shared_ptr< shadow_robot::SrMotorHandLib<STATUS_TYPE, COMMAND_TYPE> >( new shadow_robot::SrMotorHandLib<STATUS_TYPE, COMMAND_TYPE>(hw) );
00089 
00090   EXPECT_TRUE(true);
00091 }
00092 
00096 TEST(SrRobotLib, UpdateMotor)
00097 {
00098   boost::shared_ptr< HandLibTest > lib_test = boost::shared_ptr< HandLibTest >( new HandLibTest() );
00099 
00100   STATUS_TYPE* status_data = new STATUS_TYPE();
00101   //add growing sensors values
00102   for(unsigned int i=1 ; i < SENSORS_NUM_0220 + 2; ++i)
00103   {
00104     //position = id in joint enum
00105     status_data->sensors[i] = i;
00106   }
00107 
00108   status_data->motor_data_type = MOTOR_DATA_SGR;
00109 
00110   //even motors
00111   status_data->which_motors = 0;
00112 
00113   //all motor data arrived with no errors
00114   status_data->which_motor_data_arrived = 0x00055555;
00115   status_data->which_motor_data_had_errors = 0;
00116 
00117   //add growing motor data packet values
00118   for(unsigned int i=0 ; i < 10; ++i)
00119   {
00120     status_data->motor_data_packet[i].torque = 4;
00121     status_data->motor_data_packet[i].misc = 2*i;
00122   }
00123   //filling the status data with known values
00124   status_data->idle_time_us = 1;
00125 
00126   //update the library
00127   lib_test->sr_hand_lib->update(status_data);
00128 
00129   //check the data we read back are correct.
00130   EXPECT_EQ(lib_test->sr_hand_lib->main_pic_idle_time, 1);
00131   EXPECT_EQ(lib_test->sr_hand_lib->main_pic_idle_time_min, 1);
00132 
00133   //check the sensors etc..
00134   boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp = lib_test->sr_hand_lib->joints_vector.begin();
00135   for(;joint_tmp != lib_test->sr_hand_lib->joints_vector.end(); ++joint_tmp)
00136   {
00137     if(joint_tmp->has_actuator)
00138     {
00139       boost::shared_ptr<shadow_joints::MotorWrapper> motor_wrapper = boost::static_pointer_cast<shadow_joints::MotorWrapper>(joint_tmp->actuator_wrapper);
00140       //we updated the even motors
00141       if(motor_wrapper->motor_id % 2 == 0)
00142       {
00143         const sr_actuator::SrActuator* sr_actuator = static_cast<sr_actuator::SrActuator*>(motor_wrapper->actuator);
00144 
00145         ROS_ERROR_STREAM("last measured effort: " << sr_actuator->state_.last_measured_effort_ << " actuator: " << motor_wrapper->actuator);
00146 
00147         EXPECT_FLOAT_EQ(sr_actuator->state_.force_unfiltered_ , 4.0);//(double)motor_wrapper->motor_id/2.0);
00148         EXPECT_EQ(sr_actuator->state_.strain_gauge_right_, motor_wrapper->motor_id);
00149       }
00150     }
00151   }
00152 }
00153 
00159 TEST(SrRobotLib, UpdateActuators)
00160 {
00161   boost::shared_ptr< HandLibTest > lib_test = boost::shared_ptr< HandLibTest >( new HandLibTest() );
00162 
00163   STATUS_TYPE* status_data = new STATUS_TYPE();
00164   //add growing sensors values
00165   for(unsigned int i=0 ; i < SENSORS_NUM_0220 + 1; ++i)
00166   {
00167     //position = id in joint enum
00168     status_data->sensors[i] = i+1;
00169   }
00170 
00171   status_data->motor_data_type = MOTOR_DATA_VOLTAGE;
00172 
00173   //even motors
00174   status_data->which_motors = 0;
00175 
00176   //all motor data arrived with no errors
00177   status_data->which_motor_data_arrived = 0x00055555;
00178   status_data->which_motor_data_had_errors = 0;
00179 
00180   //add growing motor data packet values
00181   for(unsigned int i=0 ; i < 10; ++i)
00182   {
00183     status_data->motor_data_packet[i].torque = i;
00184     status_data->motor_data_packet[i].misc = 10*i;
00185   }
00186   //filling the status data with known values
00187   status_data->idle_time_us = 1;
00188 
00189   //update the library
00190   lib_test->sr_hand_lib->update(status_data);
00191 
00192   // name, motor_id, id_in_enum, expected_pos
00193   lib_test->check_hw_actuator("FFJ4", 7, 3, 4.0);
00194 
00195   //cleanup
00196   delete status_data;
00197 }
00198 
00205 class TestHandLib
00206   : public HandLibTestProtected
00207 {
00208 public:
00209   TestHandLib(pr2_hardware_interface::HardwareInterface* hw)
00210     : HandLibTestProtected(hw)
00211   {}
00212 
00213   using HandLibTestProtected::calibrate_joint;
00214 
00215   //using HandLibTestProtected::status_data;
00216 
00217   //using HandLibTestProtected::actuator;
00218 
00219   using HandLibTestProtected::humanize_flags;
00220 };
00221 
00222 
00228 // TEST(SrRobotLib, CalibrationOneMotor)
00229 // {
00230 
00231 //   pr2_hardware_interface::HardwareInterface *hw;
00232 //   boost::shared_ptr<TestHandLib> sr_hand_lib = boost::shared_ptr<TestHandLib>( new TestHandLib(hw) );
00233 
00234 //   STATUS_TYPE status_data;
00235 
00236 //   //set all the sensors to 0
00237 //   for(unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
00238 //     status_data.sensors[i] = 0;
00239 
00240 //   sr_hand_lib->status_data = &status_data;
00241 //   for(unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
00242 //     EXPECT_EQ(sr_hand_lib->status_data->sensors[i], 0);
00243 
00244 //   //let's find FFJ3 in the vector
00245 //   boost::ptr_vector<shadow_joints::Joint>::iterator ffj3 = sr_hand_lib->joints_vector.begin();
00246 //   std::string name_tmp = ffj3->joint_name;
00247 //   bool ffj3_found = false;
00248 //   int index_ffj3 = 0;
00249 
00250 //   for(; ffj3 != sr_hand_lib->joints_vector.end(); ++ffj3)
00251 //   {
00252 //     name_tmp = ffj3->joint_name;
00253 
00254 //     if( name_tmp.compare("FFJ3") == 0)
00255 //     {
00256 //       ffj3_found = true;
00257 //       break;
00258 //     }
00259 
00260 //     ++index_ffj3;
00261 //   }
00262 
00263 //   EXPECT_TRUE(ffj3_found);
00264 //   EXPECT_EQ(index_ffj3, 3);
00265 
00266 //   sr_hand_lib->actuator = (ffj3->motor->actuator);
00267 
00268 //   sr_hand_lib->calibrate_joint(ffj3);
00269 //   //all the sensors at 0 -> should be 0
00270 //   EXPECT_EQ( ffj3->motor->actuator->state_.position_unfiltered_ , 0.0);
00271 
00272 //   for(unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
00273 //   sr_hand_lib->status_data->sensors[i] = 1;
00274 
00275 //   //now ffj3 position should be 1
00276 //   sr_hand_lib->calibrate_joint(ffj3);
00277 //   //all the sensors at 1 -> should be 1
00278 //   EXPECT_EQ( ffj3->motor->actuator->state_.position_unfiltered_ , 1.0);
00279 
00280 //   delete hw;
00281 // }
00282 
00283 
00284 // /**
00285 //  * Testing the calibration procedure for
00286 //  * a joint having calibrating the sensors first
00287 //  * and then combining them (FFJ0)
00288 //  *
00289 //  */
00290 // TEST(SrRobotLib, CalibrationFFJ0)
00291 // {
00292 
00293 //   pr2_hardware_interface::HardwareInterface *hw;
00294 //   boost::shared_ptr<TestHandLib> sr_hand_lib = boost::shared_ptr<TestHandLib>( new TestHandLib(hw) );
00295 
00296 //   STATUS_TYPE status_data;
00297 
00298 //   //set all the sensors to 0
00299 //   for(unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
00300 //     status_data.sensors[i] = 0;
00301 
00302 //   sr_hand_lib->status_data = &status_data;
00303 //   for(unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
00304 //     EXPECT_EQ(sr_hand_lib->status_data->sensors[i], 0);
00305 
00306 //   //let's find FFJ0 in the vector
00307 //   boost::ptr_vector<shadow_joints::Joint>::iterator ffj0 = sr_hand_lib->joints_vector.begin();
00308 //   std::string name_tmp = ffj0->joint_name;
00309 //   bool ffj0_found = false;
00310 //   int index_ffj0 = 0;
00311 
00312 //   for(; ffj0 != sr_hand_lib->joints_vector.end(); ++ffj0)
00313 //   {
00314 //     name_tmp = ffj0->joint_name;
00315 
00316 //     if( name_tmp.compare("FFJ0") == 0)
00317 //     {
00318 //       ffj0_found = true;
00319 //       break;
00320 //     }
00321 
00322 //     ++index_ffj0;
00323 //   }
00324 
00325 //   EXPECT_TRUE(ffj0_found);
00326 //   EXPECT_EQ(index_ffj0, 0);
00327 
00328 //   sr_hand_lib->actuator = (ffj0->motor->actuator);
00329 
00330 //   sr_hand_lib->calibrate_joint(ffj0);
00331 //   //all the sensors at 0 -> should be 0
00332 //   EXPECT_EQ( ffj0->motor->actuator->state_.position_unfiltered_ , 0.0);
00333 
00334 //   for(unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
00335 //     sr_hand_lib->status_data->sensors[i] = 1;
00336 
00337 //   //now ffj0 position should be 1
00338 //   sr_hand_lib->calibrate_joint(ffj0);
00339 //   //all the sensors at 1 -> should be 2
00340 //   EXPECT_EQ( ffj0->motor->actuator->state_.position_unfiltered_ , 2.0);
00341 
00342 //   delete hw;
00343 // }
00344 
00345 // /**
00346 //  * Testing the calibration procedure for
00347 //  * a compound joint combining the sensors
00348 //  * and then calibrating the total (THJ5)
00349 //  *
00350 //  */
00351 // TEST(SrRobotLib, CalibrationTHJ5)
00352 // {
00353 
00354 //   pr2_hardware_interface::HardwareInterface *hw;
00355 //   boost::shared_ptr<TestHandLib> sr_hand_lib = boost::shared_ptr<TestHandLib>( new TestHandLib(hw) );
00356 
00357 //   STATUS_TYPE status_data;
00358 
00359 //   //set all the sensors to 0
00360 //   for(unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
00361 //     status_data.sensors[i] = 0;
00362 
00363 //   sr_hand_lib->status_data = &status_data;
00364 //   for(unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
00365 //     EXPECT_EQ(sr_hand_lib->status_data->sensors[i], 0);
00366 
00367 //   //let's find THJ5 in the vector
00368 //   boost::ptr_vector<shadow_joints::Joint>::iterator thj5 = sr_hand_lib->joints_vector.begin();
00369 //   std::string name_tmp = thj5->joint_name;
00370 //   bool thj5_found = false;
00371 //   int index_thj5 = 0;
00372 
00373 //   for(; thj5 != sr_hand_lib->joints_vector.end(); ++thj5)
00374 //   {
00375 //     name_tmp = thj5->joint_name;
00376 
00377 //     if( name_tmp.compare("THJ5") == 0)
00378 //     {
00379 //       thj5_found = true;
00380 //       break;
00381 //     }
00382 
00383 //     ++index_thj5;
00384 //   }
00385 
00386 //   EXPECT_TRUE(thj5_found);
00387 //   EXPECT_EQ(index_thj5, 25);
00388 
00389 //   sr_hand_lib->actuator = (thj5->motor->actuator);
00390 
00391 //   sr_hand_lib->calibrate_joint(thj5);
00392 //   //all the sensors at 0 -> should be 0
00393 //   EXPECT_EQ( thj5->motor->actuator->state_.position_unfiltered_ , 0.0);
00394 
00395 //   for(unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
00396 //     sr_hand_lib->status_data->sensors[i] = 1;
00397 
00398 //   //now thj5 position should be 1
00399 //   sr_hand_lib->calibrate_joint(thj5);
00400 //   //all the sensors at 1 -> should be 1 (THJ5 = .5 THJ5A + .5 THJ5B)
00401 //   EXPECT_EQ( thj5->motor->actuator->state_.position_unfiltered_ , 1.0);
00402 
00403 //   delete hw;
00404 // }
00405 
00410 TEST(SrRobotLib, HumanizeFlags)
00411 {
00412   pr2_hardware_interface::HardwareInterface *hw;
00413   boost::shared_ptr<TestHandLib> sr_hand_lib = boost::shared_ptr<TestHandLib>( new TestHandLib(hw) );
00414 
00415   std::vector<std::pair<std::string, bool> > flags;
00416   //all flags set
00417   flags = sr_hand_lib->humanize_flags(0xFFFF);
00418 
00419   EXPECT_EQ(flags.size(), 16);
00420 
00421   for(unsigned int i=0; i < 16; ++i)
00422     EXPECT_EQ(flags[i].first.compare(error_flag_names[i]) , 0);
00423 
00424   //The last three flags are serious
00425   EXPECT_TRUE(flags[13].second);
00426   EXPECT_TRUE(flags[14].second);
00427   EXPECT_TRUE(flags[15].second);
00428 }
00429 
00431 //     MAIN       //
00433 
00434 int main(int argc, char **argv)
00435 {
00436   ros::init(argc, argv, "calibration_test");
00437 
00438   testing::InitGoogleTest(&argc, argv);
00439   return RUN_ALL_TESTS();
00440 }
00441 /* For the emacs weenies in the crowd.
00442    Local Variables:
00443    c-basic-offset: 2
00444    End:
00445 */
00446 


sr_robot_lib
Author(s): Ugo Cupcic / ugo@shadowrobot.com , software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:37:50