00001
00029 #include "sr_robot_lib/sr_hand_lib.hpp"
00030 #include <algorithm>
00031 #include <boost/foreach.hpp>
00032 #include <boost/algorithm/string.hpp>
00033
00034 #include <sr_utilities/sr_math_utils.hpp>
00035
00036 #include "sr_robot_lib/shadow_PSTs.hpp"
00037
00038 namespace shadow_robot
00039 {
00040 const int SrHandLib::nb_motor_data = 14;
00041 const char* SrHandLib::human_readable_motor_data_types[nb_motor_data] = {"sgl", "sgr", "pwm", "flags", "current",
00042 "voltage", "temperature", "can_num_received",
00043 "can_num_transmitted", "slow_data",
00044 "can_error_counters",
00045 "pterm", "iterm", "dterm"};
00046
00047 const int32u SrHandLib::motor_data_types[nb_motor_data] = {MOTOR_DATA_SGL, MOTOR_DATA_SGR,
00048 MOTOR_DATA_PWM, MOTOR_DATA_FLAGS,
00049 MOTOR_DATA_CURRENT, MOTOR_DATA_VOLTAGE,
00050 MOTOR_DATA_TEMPERATURE, MOTOR_DATA_CAN_NUM_RECEIVED,
00051 MOTOR_DATA_CAN_NUM_TRANSMITTED, MOTOR_DATA_SLOW_MISC,
00052 MOTOR_DATA_CAN_ERROR_COUNTERS,
00053 MOTOR_DATA_PTERM, MOTOR_DATA_ITERM,
00054 MOTOR_DATA_DTERM};
00055
00056 const int SrHandLib::nb_sensor_data = 31;
00057 const char* SrHandLib::human_readable_sensor_data_types[nb_sensor_data] = {"TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ",
00058 "TACTILE_SENSOR_TYPE_MANUFACTURER",
00059 "TACTILE_SENSOR_TYPE_SERIAL_NUMBER",
00060 "TACTILE_SENSOR_TYPE_SOFTWARE_VERSION",
00061 "TACTILE_SENSOR_TYPE_PCB_VERSION",
00062 "TACTILE_SENSOR_TYPE_WHICH_SENSORS",
00063 "TACTILE_SENSOR_TYPE_PST3_PRESSURE_TEMPERATURE",
00064 "TACTILE_SENSOR_TYPE_PST3_PRESSURE_RAW_ZERO_TRACKING",
00065 "TACTILE_SENSOR_TYPE_PST3_DAC_VALUE",
00066 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_1",
00067 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_2",
00068 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_3",
00069 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_4",
00070 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_5",
00071 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_6",
00072 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7",
00073 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_8",
00074 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_9",
00075 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_10",
00076 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_11",
00077 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_12",
00078 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_13",
00079 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_14",
00080 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_15",
00081 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_16",
00082 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_17",
00083 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_18",
00084 "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_19",
00085 "TACTILE_SENSOR_TYPE_BIOTAC_PDC",
00086 "TACTILE_SENSOR_TYPE_BIOTAC_TAC",
00087 "TACTILE_SENSOR_TYPE_BIOTAC_TDC"
00088 };
00089
00090 const int32u SrHandLib::sensor_data_types[nb_sensor_data] = {TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ,
00091 TACTILE_SENSOR_TYPE_MANUFACTURER,
00092 TACTILE_SENSOR_TYPE_SERIAL_NUMBER,
00093 TACTILE_SENSOR_TYPE_SOFTWARE_VERSION,
00094 TACTILE_SENSOR_TYPE_PCB_VERSION,
00095 TACTILE_SENSOR_TYPE_WHICH_SENSORS,
00096 TACTILE_SENSOR_TYPE_PST3_PRESSURE_TEMPERATURE,
00097 TACTILE_SENSOR_TYPE_PST3_PRESSURE_RAW_ZERO_TRACKING,
00098 TACTILE_SENSOR_TYPE_PST3_DAC_VALUE,
00099 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_1,
00100 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_2,
00101 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_3,
00102 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_4,
00103 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_5,
00104 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_6,
00105 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7,
00106 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_8,
00107 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_9,
00108 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_10,
00109 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_11,
00110 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_12,
00111 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_13,
00112 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_14,
00113 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_15,
00114 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_16,
00115 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_17,
00116 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_18,
00117 TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_19,
00118 TACTILE_SENSOR_TYPE_BIOTAC_PDC,
00119 TACTILE_SENSOR_TYPE_BIOTAC_TAC,
00120 TACTILE_SENSOR_TYPE_BIOTAC_TDC
00121 };
00122
00123 SrHandLib::SrHandLib(pr2_hardware_interface::HardwareInterface *hw) :
00124 SrRobotLib(hw)
00125 {
00126
00127 motor_update_rate_configs_vector = read_update_rate_configs("motor_data_update_rate/", nb_motor_data, human_readable_motor_data_types, motor_data_types);
00128 motor_updater_ = boost::shared_ptr<generic_updater::MotorUpdater>(new generic_updater::MotorUpdater(motor_update_rate_configs_vector, operation_mode::device_update_state::INITIALIZATION));
00129
00130
00131
00132 generic_sensor_update_rate_configs_vector = read_update_rate_configs("generic_sensor_data_update_rate/", nb_sensor_data, human_readable_sensor_data_types, sensor_data_types);
00133 tactiles_init = boost::shared_ptr<tactiles::GenericTactiles>( new tactiles::GenericTactiles(generic_sensor_update_rate_configs_vector, operation_mode::device_update_state::INITIALIZATION) );
00134
00135
00136 pst3_sensor_update_rate_configs_vector = read_update_rate_configs("pst3_sensor_data_update_rate/", nb_sensor_data, human_readable_sensor_data_types, sensor_data_types);
00137
00138 biotac_sensor_update_rate_configs_vector = read_update_rate_configs("biotac_sensor_data_update_rate/", nb_sensor_data, human_readable_sensor_data_types, sensor_data_types);
00139
00140
00141 std::vector<shadow_joints::JointToSensor > joint_to_sensor_vect = read_joint_to_sensor_mapping();
00142
00143
00144 std::vector<std::string> joint_names_tmp;
00145 std::vector<int> motor_ids = read_joint_to_motor_mapping();
00146 std::vector<shadow_joints::JointToSensor > joints_to_sensors;
00147 std::vector<sr_actuator::SrActuator*> actuators;
00148
00149 ROS_ASSERT(motor_ids.size() == JOINTS_NUM_0220);
00150 ROS_ASSERT(joint_to_sensor_vect.size() == JOINTS_NUM_0220);
00151
00152 for(unsigned int i=0; i< JOINTS_NUM_0220; ++i)
00153 {
00154 joint_names_tmp.push_back(std::string(joint_names[i]));
00155 shadow_joints::JointToSensor tmp_jts = joint_to_sensor_vect[i];
00156 joints_to_sensors.push_back(tmp_jts);
00157
00158
00159 sr_actuator::SrActuator* actuator = new sr_actuator::SrActuator(joint_names[i]);
00160 ROS_INFO_STREAM("adding actuator: "<<joint_names[i]);
00161 actuators.push_back( actuator );
00162
00163 if(hw)
00164 {
00165 if(!hw->addActuator(actuator) )
00166 {
00167 ROS_FATAL("An actuator of the name '%s' already exists.", actuator->name_.c_str());
00168 }
00169 }
00170 }
00171 initialize(joint_names_tmp, motor_ids, joint_to_sensor_vect, actuators);
00172
00173
00174 motor_data_checker = boost::shared_ptr<generic_updater::MotorDataChecker>(new generic_updater::MotorDataChecker(joints_vector, motor_updater_->initialization_configs_vector));
00175
00176
00177 this->calibration_map = read_joint_calibration();
00178
00179 #ifdef DEBUG_PUBLISHER
00180
00181 debug_service = nh_tilde.advertiseService( "set_debug_publishers", &SrHandLib::set_debug_data_to_publish, this);
00182 #endif
00183 }
00184
00185 SrHandLib::~SrHandLib()
00186 {
00187 boost::ptr_vector<shadow_joints::Joint>::iterator joint = joints_vector.begin();
00188 for(;joint != joints_vector.end(); ++joint)
00189 {
00190 delete joint->motor->actuator;
00191 }
00192 }
00193
00194 void SrHandLib::initialize(std::vector<std::string> joint_names,
00195 std::vector<int> motor_ids,
00196 std::vector<shadow_joints::JointToSensor> joint_to_sensors,
00197 std::vector<sr_actuator::SrActuator*> actuators)
00198 {
00199 for(unsigned int index = 0; index < joint_names.size(); ++index)
00200 {
00201
00202 joints_vector.push_back( new shadow_joints::Joint() );
00203
00204
00205 boost::ptr_vector<shadow_joints::Joint>::reverse_iterator joint = joints_vector.rbegin();
00206
00207
00208 joint->joint_name = joint_names[index];
00209 joint->joint_to_sensor = joint_to_sensors[index];
00210
00211 if(motor_ids[index] == -1)
00212 joint->has_motor = false;
00213 else
00214 joint->has_motor = true;
00215
00216 joint->motor = boost::shared_ptr<shadow_joints::Motor>( new shadow_joints::Motor() );
00217 joint->motor->motor_id = motor_ids[index];
00218 joint->motor->actuator = actuators[index];
00219
00220 std::stringstream ss;
00221 ss << "change_force_PID_" << joint_names[index];
00222
00223 joint->motor->force_pid_service = nh_tilde.advertiseService<sr_robot_msgs::ForceController::Request,sr_robot_msgs::ForceController::Response>( ss.str().c_str(),
00224 boost::bind( &SrHandLib::force_pid_callback, this, _1, _2, joint->motor->motor_id) );
00225
00226 ss.str("");
00227 ss << "reset_motor_" << joint_names[index];
00228
00229 joint->motor->reset_motor_service = nh_tilde.advertiseService<std_srvs::Empty::Request,std_srvs::Empty::Response>( ss.str().c_str(),
00230 boost::bind( &SrHandLib::reset_motor_callback, this, _1, _2, std::pair<int,std::string>(joint->motor->motor_id, joint->joint_name) ) );
00231
00232 }
00233 }
00234
00235 bool SrHandLib::reset_motor_callback(std_srvs::Empty::Request& request,
00236 std_srvs::Empty::Response& response,
00237 std::pair<int,std::string> joint)
00238 {
00239 ROS_INFO_STREAM(" resetting " << joint.second << " ("<< joint.first <<")");
00240
00241 reset_motors_queue.push(joint.first);
00242
00243
00244 std::string joint_name = joint.second;
00245
00246
00247
00248
00249
00250 pid_timers[ joint_name ] = nh_tilde.createTimer( ros::Duration(3.0),
00251 boost::bind(&SrHandLib::resend_pids, this, joint_name, joint.first),
00252 true );
00253
00254
00255 return true;
00256 }
00257
00258
00259 void SrHandLib::resend_pids(std::string joint_name, int motor_index)
00260 {
00261
00262
00263 std::stringstream full_param;
00264
00265 int f, p, i, d, imax, max_pwm, sg_left, sg_right, deadband, sign;
00266 std::string act_name = boost::to_lower_copy(joint_name);
00267
00268 full_param << "/" << act_name << "/pid/f";
00269 nodehandle_.param<int>(full_param.str(), f, 0);
00270 full_param.str("");
00271 full_param << "/" << act_name << "/pid/p";
00272 nodehandle_.param<int>(full_param.str(), p, 0);
00273 full_param.str("");
00274 full_param << "/" << act_name << "/pid/i";
00275 nodehandle_.param<int>(full_param.str(), i, 0);
00276 full_param.str("");
00277 full_param << "/" << act_name << "/pid/d";
00278 nodehandle_.param<int>(full_param.str(), d, 0);
00279 full_param.str("");
00280 full_param << "/" << act_name << "/pid/imax";
00281 nodehandle_.param<int>(full_param.str(), imax, 0);
00282 full_param.str("");
00283 full_param << "/" << act_name << "/pid/max_pwm";
00284 nodehandle_.param<int>(full_param.str(), max_pwm, 0);
00285 full_param.str("");
00286 full_param << "/" << act_name << "/pid/sgleftref";
00287 nodehandle_.param<int>(full_param.str(), sg_left, 0);
00288 full_param.str("");
00289 full_param << "/" << act_name << "/pid/sgrightref";
00290 nodehandle_.param<int>(full_param.str(), sg_right, 0);
00291 full_param.str("");
00292 full_param << "/" << act_name << "/pid/deadband";
00293 nodehandle_.param<int>(full_param.str(), deadband, 0);
00294 full_param.str("");
00295 full_param << "/" << act_name << "/pid/sign";
00296 nodehandle_.param<int>(full_param.str(), sign, 0);
00297 full_param.str("");
00298
00299 sr_robot_msgs::ForceController::Request pid_request;
00300 pid_request.maxpwm = max_pwm;
00301 pid_request.sgleftref = sg_left;
00302 pid_request.sgrightref = sg_right;
00303 pid_request.f = f;
00304 pid_request.p = p;
00305 pid_request.i = i;
00306 pid_request.d = d;
00307 pid_request.imax = imax;
00308 pid_request.deadband = deadband;
00309 pid_request.sign = sign;
00310 sr_robot_msgs::ForceController::Response pid_response;
00311 bool pid_success = force_pid_callback(pid_request, pid_response, motor_index );
00312
00313
00314 bool backlash_compensation;
00315 full_param << "/" << act_name << "/backlash_compensation";
00316 nodehandle_.param<bool>(full_param.str(), backlash_compensation, true);
00317 full_param.str("");
00318 sr_robot_msgs::ChangeMotorSystemControls::Request backlash_request;
00319 sr_robot_msgs::MotorSystemControls motor_sys_ctrl;
00320 motor_sys_ctrl.motor_id = motor_index;
00321 motor_sys_ctrl.enable_backlash_compensation = backlash_compensation;
00322
00323 if( !backlash_compensation)
00324 ROS_INFO_STREAM( "Setting backlash compensation to OFF for joint " << act_name );
00325
00326 backlash_request.motor_system_controls.push_back(motor_sys_ctrl);
00327 sr_robot_msgs::ChangeMotorSystemControls::Response backlash_response;
00328 bool backlash_success = motor_system_controls_callback_( backlash_request, backlash_response );
00329
00330 if( !pid_success )
00331 ROS_WARN_STREAM( "Didn't load the force pid settings for the motor in joint " << act_name );
00332 if( !backlash_success )
00333 ROS_WARN_STREAM( "Didn't set the backlash compensation correctly for the motor in joint " << act_name );
00334 }
00335
00336
00337 bool SrHandLib::force_pid_callback(sr_robot_msgs::ForceController::Request& request,
00338 sr_robot_msgs::ForceController::Response& response,
00339 int motor_index)
00340 {
00341 ROS_INFO_STREAM("Received new force PID parameters for motor " << motor_index);
00342
00343
00344 if( motor_index > 20 )
00345 {
00346 ROS_WARN_STREAM(" Wrong motor index specified: " << motor_index);
00347 response.configured = false;;
00348 return false;
00349 }
00350
00351 if( !( (request.maxpwm >= MOTOR_DEMAND_PWM_RANGE_MIN) &&
00352 (request.maxpwm <= MOTOR_DEMAND_PWM_RANGE_MAX) )
00353 )
00354 {
00355 ROS_WARN_STREAM(" pid parameter maxpwm is out of range : " << request.maxpwm << " -> not in [" <<
00356 MOTOR_DEMAND_PWM_RANGE_MIN << " ; " << MOTOR_DEMAND_PWM_RANGE_MAX << "]");
00357 response.configured = false;
00358 return false;
00359 }
00360
00361 if( !( (request.f >= MOTOR_CONFIG_F_RANGE_MIN) &&
00362 (request.maxpwm <= MOTOR_CONFIG_F_RANGE_MAX) )
00363 )
00364 {
00365 ROS_WARN_STREAM(" pid parameter f is out of range : " << request.f << " -> not in [" <<
00366 MOTOR_CONFIG_F_RANGE_MIN << " ; " << MOTOR_CONFIG_F_RANGE_MAX << "]");
00367 response.configured = false;
00368 return false;
00369 }
00370
00371 if( !( (request.p >= MOTOR_CONFIG_P_RANGE_MIN) &&
00372 (request.p <= MOTOR_CONFIG_P_RANGE_MAX) )
00373 )
00374 {
00375 ROS_WARN_STREAM(" pid parameter p is out of range : " << request.p << " -> not in [" <<
00376 MOTOR_CONFIG_P_RANGE_MIN << " ; " << MOTOR_CONFIG_P_RANGE_MAX << "]");
00377 response.configured = false;
00378 return false;
00379 }
00380
00381 if( !( (request.i >= MOTOR_CONFIG_I_RANGE_MIN) &&
00382 (request.i <= MOTOR_CONFIG_I_RANGE_MAX) )
00383 )
00384 {
00385 ROS_WARN_STREAM(" pid parameter i is out of range : " << request.i << " -> not in [" <<
00386 MOTOR_CONFIG_I_RANGE_MIN << " ; " << MOTOR_CONFIG_I_RANGE_MAX << "]");
00387 response.configured = false;
00388 return false;
00389 }
00390
00391 if( !( (request.d >= MOTOR_CONFIG_D_RANGE_MIN) &&
00392 (request.d <= MOTOR_CONFIG_D_RANGE_MAX) )
00393 )
00394 {
00395 ROS_WARN_STREAM(" pid parameter d is out of range : " << request.d << " -> not in [" <<
00396 MOTOR_CONFIG_D_RANGE_MIN << " ; " << MOTOR_CONFIG_D_RANGE_MAX << "]");
00397 response.configured = false;
00398 return false;
00399 }
00400
00401 if( !( (request.imax >= MOTOR_CONFIG_IMAX_RANGE_MIN) &&
00402 (request.imax <= MOTOR_CONFIG_IMAX_RANGE_MAX) )
00403 )
00404 {
00405 ROS_WARN_STREAM(" pid parameter imax is out of range : " << request.imax << " -> not in [" <<
00406 MOTOR_CONFIG_IMAX_RANGE_MIN << " ; " << MOTOR_CONFIG_IMAX_RANGE_MAX << "]");
00407 response.configured = false;
00408 return false;
00409 }
00410
00411 if( !( (request.deadband >= MOTOR_CONFIG_DEADBAND_RANGE_MIN) &&
00412 (request.deadband <= MOTOR_CONFIG_DEADBAND_RANGE_MAX) )
00413 )
00414 {
00415 ROS_WARN_STREAM(" pid parameter deadband is out of range : " << request.deadband << " -> not in [" <<
00416 MOTOR_CONFIG_DEADBAND_RANGE_MIN << " ; " << MOTOR_CONFIG_DEADBAND_RANGE_MAX << "]");
00417 response.configured = false;
00418 return false;
00419 }
00420
00421 if( !( (request.sign >= MOTOR_CONFIG_SIGN_RANGE_MIN) &&
00422 (request.sign <= MOTOR_CONFIG_SIGN_RANGE_MAX) )
00423 )
00424 {
00425 ROS_WARN_STREAM(" pid parameter sign is out of range : " << request.sign << " -> not in [" <<
00426 MOTOR_CONFIG_SIGN_RANGE_MIN << " ; " << MOTOR_CONFIG_SIGN_RANGE_MAX << "]");
00427 response.configured = false;
00428 return false;
00429 }
00430
00431
00432 generate_force_control_config( motor_index, request.maxpwm, request.sgleftref,
00433 request.sgrightref, request.f, request.p, request.i,
00434 request.d, request.imax, request.deadband, request.sign );
00435
00436 update_force_control_in_param_server( find_joint_name(motor_index), request.maxpwm, request.sgleftref,
00437 request.sgrightref, request.f, request.p, request.i,
00438 request.d, request.imax, request.deadband, request.sign);
00439 response.configured = true;
00440
00441
00442 reinitialize_motors();
00443
00444 return true;
00445 }
00446
00447 std::string SrHandLib::find_joint_name(int motor_index)
00448 {
00449 for( boost::ptr_vector<shadow_joints::Joint>::iterator joint = joints_vector.begin();
00450 joint != joints_vector.end(); ++joint )
00451 {
00452 if( !boost::is_null(joint) )
00453 {
00454 if(joint->motor->motor_id == motor_index)
00455 return joint->joint_name;
00456 }
00457 }
00458 ROS_ERROR("Could not find joint name for motor index: %d", motor_index);
00459 return "";
00460 }
00461
00462 void SrHandLib::update_force_control_in_param_server(std::string joint_name, int max_pwm, int sg_left, int sg_right, int f, int p,
00463 int i, int d, int imax, int deadband, int sign)
00464 {
00465 if(joint_name != "")
00466 {
00467 std::stringstream full_param;
00468 std::string act_name = boost::to_lower_copy(joint_name);
00469
00470 full_param << "/" << act_name << "/pid/f";
00471 nodehandle_.setParam(full_param.str(), f);
00472 full_param.str("");
00473 full_param << "/" << act_name << "/pid/p";
00474 nodehandle_.setParam(full_param.str(), p);
00475 full_param.str("");
00476 full_param << "/" << act_name << "/pid/i";
00477 nodehandle_.setParam(full_param.str(), i);
00478 full_param.str("");
00479 full_param << "/" << act_name << "/pid/d";
00480 nodehandle_.setParam(full_param.str(), d);
00481 full_param.str("");
00482 full_param << "/" << act_name << "/pid/imax";
00483 nodehandle_.setParam(full_param.str(), imax);
00484 full_param.str("");
00485 full_param << "/" << act_name << "/pid/max_pwm";
00486 nodehandle_.setParam(full_param.str(), max_pwm);
00487 full_param.str("");
00488 full_param << "/" << act_name << "/pid/sgleftref";
00489 nodehandle_.setParam(full_param.str(), sg_left);
00490 full_param.str("");
00491 full_param << "/" << act_name << "/pid/sgrightref";
00492 nodehandle_.setParam(full_param.str(), sg_right);
00493 full_param.str("");
00494 full_param << "/" << act_name << "/pid/deadband";
00495 nodehandle_.setParam(full_param.str(), deadband);
00496 full_param.str("");
00497 full_param << "/" << act_name << "/pid/sign";
00498 nodehandle_.setParam(full_param.str(), sign);
00499 }
00500 }
00501
00502
00503 std::vector<shadow_joints::JointToSensor> SrHandLib::read_joint_to_sensor_mapping()
00504 {
00505 std::vector<shadow_joints::JointToSensor> joint_to_sensor_vect;
00506
00507 std::map<std::string, int> sensors_map;
00508 for(unsigned int i=0; i < SENSORS_NUM_0220; ++i)
00509 {
00510 sensors_map[ sensor_names[i] ] = i;
00511 }
00512
00513 XmlRpc::XmlRpcValue joint_to_sensor_mapping;
00514 nodehandle_.getParam("joint_to_sensor_mapping", joint_to_sensor_mapping);
00515 ROS_ASSERT(joint_to_sensor_mapping.getType() == XmlRpc::XmlRpcValue::TypeArray);
00516 for (int32_t i = 0; i < joint_to_sensor_mapping.size(); ++i)
00517 {
00518 shadow_joints::JointToSensor tmp_vect;
00519
00520 XmlRpc::XmlRpcValue map_one_joint = joint_to_sensor_mapping[i];
00521
00522
00523
00524
00525 int param_index = 0;
00526
00527 if(map_one_joint[param_index].getType() == XmlRpc::XmlRpcValue::TypeInt)
00528 {
00529 if(1 == static_cast<int>(map_one_joint[0]) )
00530 tmp_vect.calibrate_after_combining_sensors = true;
00531 else
00532 tmp_vect.calibrate_after_combining_sensors = false;
00533
00534 param_index ++;
00535 }
00536 else
00537 tmp_vect.calibrate_after_combining_sensors = false;
00538
00539 ROS_ASSERT(map_one_joint.getType() == XmlRpc::XmlRpcValue::TypeArray);
00540 for (int32_t i = param_index; i < map_one_joint.size(); ++i)
00541 {
00542 ROS_ASSERT(map_one_joint[i].getType() == XmlRpc::XmlRpcValue::TypeArray);
00543 shadow_joints::PartialJointToSensor tmp_joint_to_sensor;
00544
00545 ROS_ASSERT(map_one_joint[i][0].getType() == XmlRpc::XmlRpcValue::TypeString);
00546 tmp_vect.sensor_names.push_back( static_cast<std::string>(map_one_joint[i][0]) );
00547 tmp_joint_to_sensor.sensor_id = sensors_map[ static_cast<std::string>(map_one_joint[i][0]) ];
00548
00549 ROS_ASSERT(map_one_joint[i][1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00550 tmp_joint_to_sensor.coeff = static_cast<double> (map_one_joint[i][1]);
00551 tmp_vect.joint_to_sensor_vector.push_back(tmp_joint_to_sensor);
00552 }
00553 joint_to_sensor_vect.push_back(tmp_vect);
00554 }
00555
00556 return joint_to_sensor_vect;
00557 }
00558
00559
00560
00561 shadow_joints::CalibrationMap SrHandLib::read_joint_calibration()
00562 {
00563 shadow_joints::CalibrationMap joint_calibration;
00564 std::string param_name = "sr_calibrations";
00565
00566 XmlRpc::XmlRpcValue calib;
00567 nodehandle_.getParam(param_name, calib);
00568 ROS_ASSERT(calib.getType() == XmlRpc::XmlRpcValue::TypeArray);
00569
00570 for(int32_t index_cal = 0; index_cal < calib.size(); ++index_cal)
00571 {
00572
00573
00574 ROS_ASSERT(calib[index_cal][0].getType() == XmlRpc::XmlRpcValue::TypeString);
00575 ROS_ASSERT(calib[index_cal][1].getType() == XmlRpc::XmlRpcValue::TypeArray);
00576
00577 std::string joint_name = static_cast<std::string> (calib[index_cal][0]);
00578 std::vector<joint_calibration::Point> calib_table_tmp;
00579
00580
00581 for(int32_t index_table=0; index_table < calib[index_cal][1].size(); ++index_table)
00582 {
00583 ROS_ASSERT(calib[index_cal][1][index_table].getType() == XmlRpc::XmlRpcValue::TypeArray);
00584
00585 ROS_ASSERT(calib[index_cal][1][index_table].size() == 2);
00586 ROS_ASSERT(calib[index_cal][1][index_table][0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00587 ROS_ASSERT(calib[index_cal][1][index_table][1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00588
00589
00590 joint_calibration::Point point_tmp;
00591 point_tmp.raw_value = static_cast<double> (calib[index_cal][1][index_table][0]);
00592 point_tmp.calibrated_value = sr_math_utils::to_rad( static_cast<double> (calib[index_cal][1][index_table][1]) );
00593 calib_table_tmp.push_back(point_tmp);
00594 }
00595
00596 joint_calibration.insert(joint_name, boost::shared_ptr<shadow_robot::JointCalibration>(new shadow_robot::JointCalibration(calib_table_tmp)) );
00597 }
00598
00599 return joint_calibration;
00600 }
00601
00602
00603 std::vector<int> SrHandLib::read_joint_to_motor_mapping()
00604 {
00605 std::vector<int> motor_ids;
00606 std::string param_name = "joint_to_motor_mapping";
00607
00608 XmlRpc::XmlRpcValue mapping;
00609 nodehandle_.getParam(param_name, mapping);
00610 ROS_ASSERT(mapping.getType() == XmlRpc::XmlRpcValue::TypeArray);
00611
00612 for(int32_t i = 0; i < mapping.size(); ++i)
00613 {
00614 ROS_ASSERT(mapping[i].getType() == XmlRpc::XmlRpcValue::TypeInt);
00615 motor_ids.push_back(static_cast<int>(mapping[i]));
00616 }
00617
00618 return motor_ids;
00619 }
00620
00621 std::vector<generic_updater::UpdateConfig> SrHandLib::read_update_rate_configs(std::string base_param, int nb_data_defined, const char* human_readable_data_types[], const int32u data_types[])
00622 {
00623 std::vector<generic_updater::UpdateConfig> update_rate_configs_vector;
00624 typedef std::pair<std::string, int32u> ConfPair;
00625 std::vector<ConfPair> config;
00626
00627 for(int i=0; i<nb_data_defined; ++i)
00628 {
00629 ConfPair tmp;
00630
00631 ROS_DEBUG_STREAM(" read " << base_param << " config [" << i<< "] = " << human_readable_data_types[i]);
00632
00633 tmp.first = base_param + human_readable_data_types[i];
00634 tmp.second = data_types[i];
00635 config.push_back(tmp);
00636 }
00637
00638 for( unsigned int i = 0; i < config.size(); ++i )
00639 {
00640 double rate;
00641 if (nodehandle_.getParam(config[i].first, rate))
00642 {
00643 generic_updater::UpdateConfig config_tmp;
00644
00645 config_tmp.when_to_update = rate;
00646 config_tmp.what_to_update = config[i].second;
00647 update_rate_configs_vector.push_back(config_tmp);
00648
00649 ROS_DEBUG_STREAM(" read " << base_param <<" config [" << i<< "] = " << "what: "<< config_tmp.what_to_update << " when: " << config_tmp.when_to_update);
00650 }
00651 }
00652
00653 return update_rate_configs_vector;
00654 }
00655
00656 #ifdef DEBUG_PUBLISHER
00657 bool SrHandLib::set_debug_data_to_publish(sr_robot_msgs::SetDebugData::Request& request,
00658 sr_robot_msgs::SetDebugData::Response& response)
00659 {
00660
00661 if( request.publisher_index < nb_debug_publishers_const )
00662 {
00663 if( request.motor_index > NUM_MOTORS )
00664 {
00665 response.success = false;
00666 return false;
00667 }
00668 if( request.motor_data_type > 0 )
00669 {
00670 if( (request.motor_data_type < MOTOR_DATA_SGL) ||
00671 (request.motor_data_type > MOTOR_DATA_DTERM) )
00672 {
00673 response.success = false;
00674 return false;
00675 }
00676 }
00677 if(!debug_mutex.timed_lock(boost::posix_time::microseconds(debug_mutex_lock_wait_time)))
00678 {
00679 response.success = false;
00680 return false;
00681 }
00682
00683 debug_motor_indexes_and_data[request.publisher_index] = boost::shared_ptr<std::pair<int, int> >(new std::pair<int, int>());
00684
00685 debug_motor_indexes_and_data[request.publisher_index]->first = request.motor_index;
00686 debug_motor_indexes_and_data[request.publisher_index]->second = request.motor_data_type;
00687 debug_mutex.unlock();
00688 }
00689 else
00690 {
00691 response.success = false;
00692 return false;
00693 }
00694
00695 response.success = true;
00696 return true;
00697 }
00698 #endif
00699
00700 }
00701
00702
00703
00704
00705
00706