00001
00027 #include "sr_hand/hand/virtual_shadowhand.h"
00028
00029 #include <time.h>
00030 #include <ros/ros.h>
00031
00032 #ifdef GAZEBO
00033 #include <std_msgs/Float64.h>
00034 #endif
00035
00036 namespace shadowrobot
00037 {
00038 VirtualShadowhand::VirtualShadowhand() :
00039 SRArticulatedRobot()
00040 {
00041 #ifdef GAZEBO
00042 ROS_INFO("This ROS interface is built for Gazebo.");
00043
00044 std::string prefix;
00045 std::string searched_param;
00046 n_tilde = ros::NodeHandle("~");
00047
00048 n_tilde.searchParam("gazebo_joint_states_prefix", searched_param);
00049 n_tilde.param(searched_param, prefix, std::string());
00050 std::string full_topic = prefix + "joint_states";
00051 gazebo_subscriber = node.subscribe(full_topic, 2, &VirtualShadowhand::gazeboCallback, this);
00052 #else
00053 ROS_INFO("This ROS interface is not built for Gazebo.");
00054 #endif
00055
00056 srand(time(NULL));
00057 initializeMap();
00058 }
00059
00060 VirtualShadowhand::~VirtualShadowhand()
00061 {
00062 }
00063
00064 void VirtualShadowhand::initializeMap()
00065 {
00066 joints_map_mutex.lock();
00067 parameters_map_mutex.lock();
00068 controllers_map_mutex.lock();
00069
00070 JointData tmpData;
00071 JointData tmpDataZero;
00072 JointControllerData tmpController;
00073 tmpDataZero.isJointZero = 1;
00074 tmpDataZero.max = 180.0;
00075
00076 #ifdef GAZEBO
00077 std::string topic_prefix = "/";
00078 std::string topic_suffix = "/command";
00079 std::string full_topic = "";
00080 #endif
00081
00082 joints_map["FFJ0"] = tmpDataZero;
00083 controllers_map["FFJ0"] = tmpController;
00084 #ifdef GAZEBO
00085 full_topic = topic_prefix + "ffdistal_controller" + topic_suffix;
00086 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00087 int tmp_index = 0;
00088 tmpData.publisher_index = tmp_index;
00089 #endif
00090 joints_map["FFJ1"] = tmpData;
00091 controllers_map["FFJ1"] = tmpController;
00092 #ifdef GAZEBO
00093 full_topic = topic_prefix + "ffmiddle_controller" + topic_suffix;
00094 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00095 tmp_index ++;
00096 tmpData.publisher_index = tmp_index;
00097 #endif
00098 joints_map["FFJ2"] = tmpData;
00099 controllers_map["FFJ2"] = tmpController;
00100 #ifdef GAZEBO
00101 full_topic = topic_prefix + "ffproximal_controller" + topic_suffix;
00102 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00103 tmp_index ++;
00104 tmpData.publisher_index = tmp_index;
00105 #endif
00106 joints_map["FFJ3"] = tmpData;
00107 controllers_map["FFJ3"] = tmpController;
00108 tmpData.min = -25.0;
00109 tmpData.max = 25.0;
00110 #ifdef GAZEBO
00111 full_topic = topic_prefix + "ffknuckle_controller" + topic_suffix;
00112 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00113 tmp_index ++;
00114 tmpData.publisher_index = tmp_index;
00115 #endif
00116 joints_map["FFJ4"] = tmpData;
00117 controllers_map["FFJ4"] = tmpController;
00118
00119 joints_map["MFJ0"] = tmpDataZero;
00120 controllers_map["MFJ0"] = tmpController;
00121 tmpData.min = 0.0;
00122 tmpData.max = 90.0;
00123 #ifdef GAZEBO
00124 full_topic = topic_prefix + "mfdistal_controller" + topic_suffix;
00125 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00126 tmp_index ++;
00127 tmpData.publisher_index = tmp_index;
00128 #endif
00129 joints_map["MFJ1"] = tmpData;
00130 controllers_map["MFJ1"] = tmpController;
00131 #ifdef GAZEBO
00132 full_topic = topic_prefix + "mfmiddle_controller" + topic_suffix;
00133 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00134 tmp_index ++;
00135 tmpData.publisher_index = tmp_index;
00136 #endif
00137 joints_map["MFJ2"] = tmpData;
00138 controllers_map["MFJ2"] = tmpController;
00139 #ifdef GAZEBO
00140 full_topic = topic_prefix + "mfproximal_controller" + topic_suffix;
00141 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00142 tmp_index ++;
00143 tmpData.publisher_index = tmp_index;
00144 #endif
00145 joints_map["MFJ3"] = tmpData;
00146 controllers_map["MFJ3"] = tmpController;
00147 tmpData.min = -25.0;
00148 tmpData.max = 25.0;
00149 #ifdef GAZEBO
00150 full_topic = topic_prefix + "mfknuckle_controller" + topic_suffix;
00151 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00152 tmp_index ++;
00153 tmpData.publisher_index = tmp_index;
00154 #endif
00155 joints_map["MFJ4"] = tmpData;
00156 controllers_map["MFJ4"] = tmpController;
00157
00158 joints_map["RFJ0"] = tmpDataZero;
00159 controllers_map["RFJ0"] = tmpController;
00160 tmpData.min = 0.0;
00161 tmpData.max = 90.0;
00162 #ifdef GAZEBO
00163 full_topic = topic_prefix + "rfdistal_controller" + topic_suffix;
00164 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00165 tmp_index ++;
00166 tmpData.publisher_index = tmp_index;
00167 #endif
00168 joints_map["RFJ1"] = tmpData;
00169 controllers_map["RFJ1"] = tmpController;
00170 #ifdef GAZEBO
00171 full_topic = topic_prefix + "rfmiddle_controller" + topic_suffix;
00172 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00173 tmp_index ++;
00174 tmpData.publisher_index = tmp_index;
00175 #endif
00176 joints_map["RFJ2"] = tmpData;
00177 controllers_map["RFJ2"] = tmpController;
00178 #ifdef GAZEBO
00179 full_topic = topic_prefix + "rfproximal_controller" + topic_suffix;
00180 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00181 tmp_index ++;
00182 tmpData.publisher_index = tmp_index;
00183 #endif
00184 joints_map["RFJ3"] = tmpData;
00185 controllers_map["RFJ3"] = tmpController;
00186 tmpData.min = -25.0;
00187 tmpData.max = 25.0;
00188 #ifdef GAZEBO
00189 full_topic = topic_prefix + "rfknuckle_controller" + topic_suffix;
00190 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00191 tmp_index ++;
00192 tmpData.publisher_index = tmp_index;
00193 #endif
00194 joints_map["RFJ4"] = tmpData;
00195 controllers_map["RFJ4"] = tmpController;
00196
00197 joints_map["LFJ0"] = tmpDataZero;
00198 controllers_map["LFJ0"] = tmpController;
00199 tmpData.min = 0.0;
00200 tmpData.max = 90.0;
00201 #ifdef GAZEBO
00202 full_topic = topic_prefix + "lfdistal_controller" + topic_suffix;
00203 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00204 tmp_index ++;
00205 tmpData.publisher_index = tmp_index;
00206 #endif
00207 joints_map["LFJ1"] = tmpData;
00208 controllers_map["LFJ1"] = tmpController;
00209 #ifdef GAZEBO
00210 full_topic = topic_prefix + "lfmiddle_controller" + topic_suffix;
00211 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00212 tmp_index ++;
00213 tmpData.publisher_index = tmp_index;
00214 #endif
00215 joints_map["LFJ2"] = tmpData;
00216 controllers_map["LFJ2"] = tmpController;
00217 #ifdef GAZEBO
00218 full_topic = topic_prefix + "lfproximal_controller" + topic_suffix;
00219 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00220 tmp_index ++;
00221 tmpData.publisher_index = tmp_index;
00222 #endif
00223 joints_map["LFJ3"] = tmpData;
00224 controllers_map["LFJ3"] = tmpController;
00225 tmpData.min = -25.0;
00226 tmpData.max = 25.0;
00227 #ifdef GAZEBO
00228 full_topic = topic_prefix + "lfknuckle_controller" + topic_suffix;
00229 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00230 tmp_index ++;
00231 tmpData.publisher_index = tmp_index;
00232 #endif
00233 joints_map["LFJ4"] = tmpData;
00234 controllers_map["LFJ4"] = tmpController;
00235 tmpData.min = 0.0;
00236 tmpData.max = 45.0;
00237 #ifdef GAZEBO
00238 full_topic = topic_prefix + "lfmetacarpal_controller" + topic_suffix;
00239 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00240 tmp_index ++;
00241 tmpData.publisher_index = tmp_index;
00242 #endif
00243 joints_map["LFJ5"] = tmpData;
00244 controllers_map["LFJ5"] = tmpController;
00245
00246 tmpData.min = 0.0;
00247 tmpData.max = 90.0;
00248 #ifdef GAZEBO
00249 full_topic = topic_prefix + "thdistal_controller" + topic_suffix;
00250 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00251 tmp_index ++;
00252 tmpData.publisher_index = tmp_index;
00253 #endif
00254 joints_map["THJ1"] = tmpData;
00255 controllers_map["THJ1"] = tmpController;
00256 tmpData.min = -30.0;
00257 tmpData.max = 30.0;
00258 #ifdef GAZEBO
00259 full_topic = topic_prefix + "thmiddle_controller" + topic_suffix;
00260 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00261 tmp_index ++;
00262 tmpData.publisher_index = tmp_index;
00263 #endif
00264 joints_map["THJ2"] = tmpData;
00265 controllers_map["THJ2"] = tmpController;
00266 tmpData.min = -15.0;
00267 tmpData.max = 15.0;
00268 #ifdef GAZEBO
00269 full_topic = topic_prefix + "thhub_controller" + topic_suffix;
00270 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00271 tmp_index ++;
00272 tmpData.publisher_index = tmp_index;
00273 #endif
00274 joints_map["THJ3"] = tmpData;
00275 controllers_map["THJ3"] = tmpController;
00276 tmpData.min = 0.0;
00277 tmpData.max = 75.0;
00278 #ifdef GAZEBO
00279 full_topic = topic_prefix + "thproximal_controller" + topic_suffix;
00280 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00281 tmp_index ++;
00282 tmpData.publisher_index = tmp_index;
00283 #endif
00284 joints_map["THJ4"] = tmpData;
00285 controllers_map["THJ4"] = tmpController;
00286 tmpData.min = -60.0;
00287 tmpData.max = 60.0;
00288 #ifdef GAZEBO
00289 full_topic = topic_prefix + "thbase_controller" + topic_suffix;
00290 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00291 tmp_index ++;
00292 tmpData.publisher_index = tmp_index;
00293 #endif
00294 joints_map["THJ5"] = tmpData;
00295 controllers_map["THJ5"] = tmpController;
00296
00297 tmpData.min = -30.0;
00298 tmpData.max = 40.0;
00299 #ifdef GAZEBO
00300 full_topic = topic_prefix + "palm_controller" + topic_suffix;
00301 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00302 tmp_index ++;
00303 tmpData.publisher_index = tmp_index;
00304 #endif
00305 joints_map["WRJ1"] = tmpData;
00306 controllers_map["WRJ1"] = tmpController;
00307 tmpData.min = -30.0;
00308 tmpData.max = 10.0;
00309 #ifdef GAZEBO
00310 full_topic = topic_prefix + "wrist_controller" + topic_suffix;
00311 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00312 tmp_index ++;
00313 tmpData.publisher_index = tmp_index;
00314 #endif
00315 joints_map["WRJ2"] = tmpData;
00316 controllers_map["WRJ2"] = tmpController;
00317
00318 parameters_map["d"] = PARAM_d;
00319 parameters_map["i"] = PARAM_i;
00320 parameters_map["p"] = PARAM_p;
00321 parameters_map["target"] = PARAM_target;
00322 parameters_map["sensor"] = PARAM_sensor;
00323
00324 parameters_map["valve"] = PARAM_valve;
00325 parameters_map["dead"] = PARAM_deadband;
00326 parameters_map["deadband"] = PARAM_deadband;
00327 parameters_map["imax"] = PARAM_imax;
00328 parameters_map["offset"] = PARAM_output_offset;
00329 parameters_map["shift"] = PARAM_shift;
00330 parameters_map["max"] = PARAM_output_max;
00331
00333 parameters_map["motor_maxforce"] = PARAM_motor_maxforce;
00334 parameters_map["motor_safeforce"] = PARAM_motor_safeforce;
00335
00336 parameters_map["force_p"] = PARAM_force_p;
00337 parameters_map["force_i"] = PARAM_force_i;
00338 parameters_map["force_d"] = PARAM_force_d;
00339
00340 parameters_map["force_imax"] = PARAM_force_imax;
00341 parameters_map["force_out_shift"] = PARAM_force_out_shift;
00342 parameters_map["force_deadband"] = PARAM_force_deadband;
00343 parameters_map["force_offset"] = PARAM_force_offset;
00344
00345 parameters_map["sensor_imax"] = PARAM_sensor_imax;
00346 parameters_map["sensor_out_shift"] = PARAM_sensor_out_shift;
00347 parameters_map["sensor_deadband"] = PARAM_sensor_deadband;
00348 parameters_map["sensor_offset"] = PARAM_sensor_offset;
00349 parameters_map["max_temp"] = PARAM_max_temperature;
00350 parameters_map["max_temperature"] = PARAM_max_temperature;
00351 parameters_map["max_current"] = PARAM_max_current;
00352
00353 controllers_map_mutex.unlock();
00354 parameters_map_mutex.unlock();
00355 joints_map_mutex.unlock();
00356 }
00357
00358 short VirtualShadowhand::sendupdate( std::string joint_name, double target )
00359 {
00360 joints_map_mutex.lock();
00361
00362 JointsMap::iterator iter = joints_map.find(joint_name);
00363 #ifdef GAZEBO
00364 std_msgs::Float64 target_msg;
00365 #endif
00366
00367
00368 if( iter == joints_map.end() )
00369 {
00370 ROS_DEBUG("Joint %s not found", joint_name.c_str());
00371
00372 joints_map_mutex.unlock();
00373 return -1;
00374 }
00375
00376
00377
00378 if( iter->second.isJointZero == 1 )
00379 {
00380
00381 JointData tmpData0 = JointData(iter->second);
00382 if( target < tmpData0.min )
00383 target = tmpData0.min;
00384 if( target > tmpData0.max )
00385 target = tmpData0.max;
00386
00387 #ifndef GAZEBO
00388 tmpData0.position = target;
00389 #endif
00390 tmpData0.target = target;
00391
00392 joints_map[joint_name] = tmpData0;
00393
00394 ++iter;
00395 JointData tmpData1 = JointData(iter->second);
00396 #ifdef GAZEBO
00397
00398 target_msg.data = toRad( target / 2.0 );
00399 gazebo_publishers[tmpData1.publisher_index].publish(target_msg);
00400 ros::spinOnce();
00401 #else
00402 tmpData1.position = target / 2.0;
00403 #endif
00404 tmpData1.target = target / 2.0;
00405
00406 joints_map[iter->first] = tmpData1;
00407
00408 ++iter;
00409 JointData tmpData2 = JointData(iter->second);
00410 #ifdef GAZEBO
00411
00412 target_msg.data = toRad( target / 2.0 );
00413 gazebo_publishers[tmpData2.publisher_index].publish(target_msg);
00414 ros::spinOnce();
00415 #else
00416 tmpData2.position = target / 2.0;
00417 #endif
00418 tmpData2.target = target / 2.0;
00419
00420 joints_map[iter->first] = tmpData2;
00421
00422 joints_map_mutex.unlock();
00423 return 0;
00424 }
00425
00426
00427 JointData tmpData(iter->second);
00428
00429 if( target < tmpData.min )
00430 target = tmpData.min;
00431 if( target > tmpData.max )
00432 target = tmpData.max;
00433
00434 #ifdef GAZEBO
00435
00436 target_msg.data = toRad(target);
00437 gazebo_publishers[tmpData.publisher_index].publish(target_msg);
00438 ros::spinOnce();
00439 #else
00440 tmpData.position = target;
00441 #endif
00442 tmpData.target = target;
00443
00444 joints_map[joint_name] = tmpData;
00445
00446 joints_map_mutex.unlock();
00447 return 0;
00448 }
00449
00450 JointData VirtualShadowhand::getJointData( std::string joint_name )
00451 {
00452 joints_map_mutex.lock();
00453
00454 JointsMap::iterator iter = joints_map.find(joint_name);
00455
00456
00457 if( iter != joints_map.end() )
00458 {
00459
00460 iter->second.temperature = ((double)(rand() % 100) / 100.0);
00461 iter->second.current = ((double)(rand() % 100) / 100.0);
00462 iter->second.force = ((double)(rand() % 100) / 100.0);
00463
00464 JointData tmp = JointData(iter->second);
00465
00466 joints_map_mutex.unlock();
00467 return tmp;
00468 }
00469
00470 ROS_ERROR("Joint %s not found.", joint_name.c_str());
00471 JointData noData;
00472 joints_map_mutex.unlock();
00473 return noData;
00474 }
00475
00476 SRArticulatedRobot::JointsMap VirtualShadowhand::getAllJointsData()
00477 {
00478 joints_map_mutex.lock();
00479
00480 for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00481 {
00482 JointData tmpData = it->second;
00483 tmpData.temperature = ((double)(rand() % 100) / 100.0);
00484 tmpData.current = ((double)(rand() % 100) / 100.0);
00485 tmpData.force = ((double)(rand() % 100) / 100.0);
00486 tmpData.jointIndex = 0;
00487 tmpData.flags = "";
00488
00489 joints_map[it->first] = tmpData;
00490 }
00491
00492 JointsMap tmp_map = JointsMap(joints_map);
00493 joints_map_mutex.unlock();
00494 return tmp_map;
00495 }
00496
00497 short VirtualShadowhand::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
00498 {
00499 controllers_map_mutex.lock();
00500
00501 ControllersMap::iterator iter = controllers_map.find(contrlr_name);
00502
00503
00504 if( iter != controllers_map.end() )
00505 {
00506 controllers_map[iter->first] = ctrlr_data;
00507 }
00508 else
00509 {
00510 ROS_ERROR("Controller %s not found", contrlr_name.c_str());
00511 }
00512
00513 controllers_map_mutex.unlock();
00514 return 0;
00515 }
00516
00517 JointControllerData VirtualShadowhand::getContrl( std::string contrlr_name )
00518 {
00519 controllers_map_mutex.lock();
00520 ControllersMap::iterator iter = controllers_map.find(contrlr_name);
00521
00522
00523 if( iter != controllers_map.end() )
00524 {
00525 JointControllerData tmp = JointControllerData(iter->second);
00526 controllers_map_mutex.unlock();
00527 return tmp;
00528 }
00529
00530 ROS_ERROR("Controller %s not found", contrlr_name.c_str() );
00531 JointControllerData no_result;
00532 controllers_map_mutex.unlock();
00533 return no_result;
00534 }
00535
00536 short VirtualShadowhand::setConfig( std::vector<std::string> myConfig )
00537 {
00538 ROS_WARN("The set config function is not implemented in the virtual shadowhand.");
00539 return 0;
00540 }
00541
00542 void VirtualShadowhand::getConfig( std::string joint_name )
00543 {
00544 ROS_WARN("The get config function is not implemented in the virtual shadowhand.");
00545 }
00546
00547 std::vector<DiagnosticData> VirtualShadowhand::getDiagnostics()
00548 {
00549 joints_map_mutex.lock();
00550 std::vector<DiagnosticData> returnVect;
00551
00552 for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00553 {
00554 DiagnosticData tmpDiag;
00555 tmpDiag.joint_name = it->first;
00556 tmpDiag.level = 0;
00557 tmpDiag.flags = "";
00558 tmpDiag.target_sensor_num = 0;
00559 tmpDiag.target = it->second.target;
00560 tmpDiag.position_sensor_num = 0;
00561 tmpDiag.position = it-> second.position;
00562
00563 returnVect.push_back(tmpDiag);
00564 }
00565
00566 joints_map_mutex.unlock();
00567 return returnVect;
00568 }
00569
00570 #ifdef GAZEBO
00571 void VirtualShadowhand::gazeboCallback(const sensor_msgs::JointStateConstPtr& msg)
00572 {
00573 joints_map_mutex.lock();
00574
00575
00576 for(unsigned int index = 0; index < msg->name.size(); ++index)
00577 {
00578 std::string joint_name = msg->name[index];
00579 JointsMap::iterator iter = joints_map.find(joint_name);
00580
00581 if(iter == joints_map.end())
00582 continue;
00583
00584
00585 JointData tmpData(iter->second);
00586
00587 tmpData.position = toDegrees(msg->position[index]);
00588 tmpData.force = msg->effort[index];
00589
00590 joints_map[joint_name] = tmpData;
00591 }
00592
00593
00594 for(JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it)
00595 {
00596 JointData tmpData = it->second;
00597 if( tmpData.isJointZero == 1 )
00598 {
00599 std::string joint_name = it->first;
00600 double position = 0.0;
00601
00602
00603 ++it;
00604 JointData tmpData1 = JointData(it->second);
00605 position += tmpData1.position;
00606
00607
00608 ++it;
00609 JointData tmpData2 = JointData(it->second);
00610 position += tmpData2.position;
00611
00612 tmpData.position = position;
00613
00614 joints_map[joint_name] = tmpData;
00615 }
00616 }
00617
00618 joints_map_mutex.unlock();
00619 }
00620 #endif
00621 }