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
00075 }
00076
00077 };
00078
00082 TEST(SrRobotLib, Initialization)
00083 {
00084 boost::shared_ptr< HandLibTest > lib_test = boost::shared_ptr< HandLibTest >( new HandLibTest() );
00085
00086
00087
00088
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
00102 for(unsigned int i=1 ; i < SENSORS_NUM_0220 + 2; ++i)
00103 {
00104
00105 status_data->sensors[i] = i;
00106 }
00107
00108 status_data->motor_data_type = MOTOR_DATA_SGR;
00109
00110
00111 status_data->which_motors = 0;
00112
00113
00114 status_data->which_motor_data_arrived = 0x00055555;
00115 status_data->which_motor_data_had_errors = 0;
00116
00117
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
00124 status_data->idle_time_us = 1;
00125
00126
00127 lib_test->sr_hand_lib->update(status_data);
00128
00129
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
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
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);
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
00165 for(unsigned int i=0 ; i < SENSORS_NUM_0220 + 1; ++i)
00166 {
00167
00168 status_data->sensors[i] = i+1;
00169 }
00170
00171 status_data->motor_data_type = MOTOR_DATA_VOLTAGE;
00172
00173
00174 status_data->which_motors = 0;
00175
00176
00177 status_data->which_motor_data_arrived = 0x00055555;
00178 status_data->which_motor_data_had_errors = 0;
00179
00180
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
00187 status_data->idle_time_us = 1;
00188
00189
00190 lib_test->sr_hand_lib->update(status_data);
00191
00192
00193 lib_test->check_hw_actuator("FFJ4", 7, 3, 4.0);
00194
00195
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
00216
00217
00218
00219 using HandLibTestProtected::humanize_flags;
00220 };
00221
00222
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
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
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
00425 EXPECT_TRUE(flags[13].second);
00426 EXPECT_TRUE(flags[14].second);
00427 EXPECT_TRUE(flags[15].second);
00428 }
00429
00431
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
00442
00443
00444
00445
00446