virtual_shadowhand.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_hand/hand/virtual_shadowhand.h"
00028 
00029 #include <time.h>
00030 #include <ros/ros.h>
00031 
00032 #include <boost/algorithm/string.hpp>
00033 
00034 #ifdef GAZEBO
00035 #include <std_msgs/Float64.h>
00036 #endif
00037 
00038 #include <urdf/model.h>
00039 #include <sr_utilities/sr_math_utils.hpp>
00040 
00041 namespace shadowrobot
00042 {
00043   VirtualShadowhand::VirtualShadowhand() :
00044     SRArticulatedRobot(), n_tilde("~")
00045   {
00046 #ifdef GAZEBO
00047     ROS_INFO("This ROS interface is built for Gazebo.");
00048     //initialises the subscriber to the Gazebo joint_states messages
00049     std::string prefix;
00050     std::string searched_param;
00051 
00052     n_tilde.searchParam("gazebo_joint_states_prefix", searched_param);
00053     n_tilde.param(searched_param, prefix, std::string());
00054     std::string full_topic = prefix + "joint_states";
00055     gazebo_subscriber = node.subscribe(full_topic, 2, &VirtualShadowhand::gazeboCallback, this);
00056 #else
00057     ROS_INFO("This ROS interface is not built for Gazebo.");
00058 #endif
00059 
00060     srand(time(NULL));
00061     initializeMap();
00062   }
00063 
00064   VirtualShadowhand::~VirtualShadowhand()
00065   {
00066   }
00067 
00068   void VirtualShadowhand::initializeMap()
00069   {
00070     joints_map_mutex.lock();
00071     parameters_map_mutex.lock();
00072     controllers_map_mutex.lock();
00073 
00074 
00075     JointData tmpData;
00076     JointData tmpDataZero;
00077     JointControllerData tmpController;
00078     tmpDataZero.isJointZero = 1;
00079     tmpDataZero.max = 180.0;
00080 
00081 #ifdef GAZEBO
00082     std::string topic_prefix = "/sh_";
00083     std::string topic_suffix = "_mixed_position_velocity_controller/command";
00084     std::string full_topic = "";
00085 #endif
00086 
00087     //Get the urdf model from the parameter server
00088     std::string robot_desc_string;
00089     node.param("hand_description", robot_desc_string, std::string());
00090     urdf::Model robot_model;
00091     if (!robot_model.initString(robot_desc_string)){
00092       ROS_ERROR("Failed to parse urdf file");
00093       return;
00094     }
00095 
00096     std::map<std::string, boost::shared_ptr<urdf::Joint> > all_joints = robot_model.joints_;
00097     std::map<std::string, boost::shared_ptr<urdf::Joint> >::const_iterator iter = all_joints.begin();
00098     ROS_DEBUG("All the Hand joints: ");
00099 
00100 #ifdef GAZEBO
00101     //index of the publisher to the Gazebo controller
00102     int tmp_index = 0;
00103 #endif
00104 
00105     for (; iter != all_joints.end(); ++iter )
00106     {
00107       if( iter->second->type == urdf::Joint::REVOLUTE)
00108       {
00109         std::string current_joint_name = iter->first;
00110 
00111         ROS_DEBUG_STREAM(" joint: " << current_joint_name << '\t' << iter->second );
00112 
00113         //check if we need to create a joint 0
00114         // create them when we have the joint 2
00115 
00116 #ifdef GAZEBO
00117         //The joint 1 is not controlled directly in Gazebo (the controller
00118         // controls joint 0)
00119         bool no_controller = false;
00120 #endif
00121 
00122         bool create_joint_zero = false;
00123         boost::algorithm::to_lower(current_joint_name);
00124         if( current_joint_name == "ffj2" || current_joint_name == "mfj2"
00125             || current_joint_name == "rfj2" || current_joint_name == "lfj2" )
00126         {
00127           create_joint_zero = true;
00128 
00129 #ifdef GAZEBO
00130           no_controller = true;
00131 #endif
00132         }
00133 
00134 #ifdef GAZEBO
00135         if( current_joint_name == "ffj1" || current_joint_name == "mfj1"
00136             || current_joint_name == "rfj1" || current_joint_name == "lfj1" )
00137         {
00138           no_controller = true;
00139         }
00140 #endif
00141 
00142         if( create_joint_zero )
00143         {
00144 #ifdef GAZEBO
00145           full_topic = topic_prefix + std::string(1, current_joint_name[0]) + "fj0" + topic_suffix;
00146           gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00147           tmpDataZero.publisher_index = tmp_index;
00148 
00149           ++tmp_index;
00150 #endif
00151           boost::algorithm::to_upper(current_joint_name);
00152           joints_map[std::string(1, current_joint_name[0]) + "FJ0"] = tmpDataZero;
00153           controllers_map[std::string(1, current_joint_name[0]) + "FJ0"] = tmpController;
00154 
00155           tmpData.min = sr_math_utils::to_degrees(iter->second->limits->lower);
00156           tmpData.max = sr_math_utils::to_degrees(iter->second->limits->upper);
00157           joints_map[current_joint_name] = tmpData;
00158           controllers_map[current_joint_name] = tmpController;
00159         }
00160         else
00161         {
00162 #ifdef GAZEBO
00163           if( !no_controller )
00164           {
00165             boost::algorithm::to_lower(current_joint_name);
00166             full_topic = topic_prefix + current_joint_name + topic_suffix;
00167             gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00168             tmpData.publisher_index = tmp_index;
00169             ++tmp_index;
00170           }
00171 #endif
00172           tmpData.min = sr_math_utils::to_degrees(iter->second->limits->lower);
00173           tmpData.max = sr_math_utils::to_degrees(iter->second->limits->upper);
00174           boost::algorithm::to_upper(current_joint_name);
00175           joints_map[ current_joint_name ] = tmpData;
00176           controllers_map[ current_joint_name ] = tmpController;
00177         }
00178       }
00179     }
00180 
00181     parameters_map["d"] = PARAM_d;
00182     parameters_map["i"] = PARAM_i;
00183     parameters_map["p"] = PARAM_p;
00184     parameters_map["target"] = PARAM_target;
00185     parameters_map["sensor"] = PARAM_sensor;
00186 
00187     parameters_map["valve"] = PARAM_valve;
00188     parameters_map["dead"] = PARAM_deadband;
00189     parameters_map["deadband"] = PARAM_deadband;
00190     parameters_map["imax"] = PARAM_imax;
00191     parameters_map["offset"] = PARAM_output_offset;
00192     parameters_map["shift"] = PARAM_shift;
00193     parameters_map["max"] = PARAM_output_max;
00194 
00196     parameters_map["motor_maxforce"] = PARAM_motor_maxforce;
00197     parameters_map["motor_safeforce"] = PARAM_motor_safeforce;
00198 
00199     parameters_map["force_p"] = PARAM_force_p;
00200     parameters_map["force_i"] = PARAM_force_i;
00201     parameters_map["force_d"] = PARAM_force_d;
00202 
00203     parameters_map["force_imax"] = PARAM_force_imax;
00204     parameters_map["force_out_shift"] = PARAM_force_out_shift;
00205     parameters_map["force_deadband"] = PARAM_force_deadband;
00206     parameters_map["force_offset"] = PARAM_force_offset;
00207 
00208     parameters_map["sensor_imax"] = PARAM_sensor_imax;
00209     parameters_map["sensor_out_shift"] = PARAM_sensor_out_shift;
00210     parameters_map["sensor_deadband"] = PARAM_sensor_deadband;
00211     parameters_map["sensor_offset"] = PARAM_sensor_offset;
00212     parameters_map["max_temp"] = PARAM_max_temperature;
00213     parameters_map["max_temperature"] = PARAM_max_temperature;
00214     parameters_map["max_current"] = PARAM_max_current;
00215 
00216     controllers_map_mutex.unlock();
00217     parameters_map_mutex.unlock();
00218     joints_map_mutex.unlock();
00219   }
00220 
00221   short VirtualShadowhand::sendupdate( std::string joint_name, double target )
00222   {
00223     joints_map_mutex.lock();
00224 
00225     JointsMap::iterator iter = joints_map.find(joint_name);
00226 #ifdef GAZEBO
00227     std_msgs::Float64 target_msg;
00228 #endif
00229 
00230     //not found
00231     if( iter == joints_map.end() )
00232     {
00233       ROS_DEBUG("Joint %s not found", joint_name.c_str());
00234 
00235       joints_map_mutex.unlock();
00236       return -1;
00237     }
00238     JointData joint_0_data = JointData(iter->second);
00239 
00240     //in total simulation:
00241     //if joint 0, send 1/2 of the target to joint 1 and other half to
00242     //2;
00243     // if using gazebo, we just send the target to the joint 0 controller
00244     // which is then controlling both joints.
00245     if( iter->second.isJointZero == 1 )
00246     {
00247       //push target and position to the given target for Joint 0
00248       JointData tmpData0 = JointData(iter->second);
00249       if( target < tmpData0.min )
00250         target = tmpData0.min;
00251       if( target > tmpData0.max )
00252         target = tmpData0.max;
00253 
00254 #ifndef GAZEBO
00255       tmpData0.position = target;
00256 #endif
00257       tmpData0.target = target;
00258 
00259       joints_map[joint_name] = tmpData0;
00260 
00261       ++iter;
00262       JointData tmpData1 = JointData(iter->second);
00263 #ifdef GAZEBO
00264       //gazebo targets are in radians
00265       target_msg.data = toRad( target );
00266       gazebo_publishers[joint_0_data.publisher_index].publish(target_msg);
00267 #else
00268       tmpData1.position = target / 2.0;
00269 #endif
00270       tmpData1.target = target / 2.0;
00271 
00272       joints_map[iter->first] = tmpData1;
00273 
00274       ++iter;
00275       JointData tmpData2 = JointData(iter->second);
00276 #ifdef GAZEBO
00277       //we've already send the data to the joint 0 controller
00278 #else
00279       tmpData2.position = target / 2.0;
00280 #endif
00281       tmpData2.target = target / 2.0;
00282 
00283       joints_map[iter->first] = tmpData2;
00284 
00285       joints_map_mutex.unlock();
00286       return 0;
00287     }
00288 
00289     //joint found
00290     JointData tmpData(iter->second);
00291 
00292     if( target < tmpData.min )
00293       target = tmpData.min;
00294     if( target > tmpData.max )
00295       target = tmpData.max;
00296 
00297 #ifdef GAZEBO
00298     //gazebo targets are in radians
00299     target_msg.data = toRad(target);
00300     gazebo_publishers[tmpData.publisher_index].publish(target_msg);
00301 #else
00302     tmpData.position = target;
00303 #endif
00304     tmpData.target = target;
00305 
00306     joints_map[joint_name] = tmpData;
00307 
00308     joints_map_mutex.unlock();
00309     return 0;
00310   }
00311 
00312   JointData VirtualShadowhand::getJointData( std::string joint_name )
00313   {
00314     joints_map_mutex.lock();
00315 
00316     JointsMap::iterator iter = joints_map.find(joint_name);
00317 
00318     //joint found
00319     if( iter != joints_map.end() )
00320     {
00321       //return the position
00322       iter->second.temperature = ((double)(rand() % 100) / 100.0);
00323       iter->second.current = ((double)(rand() % 100) / 100.0);
00324 #ifndef GAZEBO
00325       iter->second.force = ((double)(rand() % 100) / 100.0);
00326 #endif
00327 
00328       JointData tmp = JointData(iter->second);
00329 
00330       joints_map_mutex.unlock();
00331       return tmp;
00332     }
00333 
00334     ROS_ERROR("Joint %s not found.", joint_name.c_str());
00335     JointData noData;
00336     joints_map_mutex.unlock();
00337     return noData;
00338   }
00339 
00340   SRArticulatedRobot::JointsMap VirtualShadowhand::getAllJointsData()
00341   {
00342     joints_map_mutex.lock();
00343 
00344     for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00345     {
00346       JointData tmpData = it->second;
00347       tmpData.temperature = ((double)(rand() % 100) / 100.0);
00348       tmpData.current = ((double)(rand() % 100) / 100.0);
00349 #ifndef GAZEBO
00350       tmpData.force = ((double)(rand() % 100) / 100.0);
00351 #endif
00352       tmpData.jointIndex = 0;
00353       tmpData.flags = "";
00354 
00355       joints_map[it->first] = tmpData;
00356     }
00357 
00358     JointsMap tmp_map = JointsMap(joints_map);
00359     joints_map_mutex.unlock();
00360     return tmp_map;
00361   }
00362 
00363   short VirtualShadowhand::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
00364   {
00365     controllers_map_mutex.lock();
00366 
00367     ControllersMap::iterator iter = controllers_map.find(contrlr_name);
00368 
00369     //joint found
00370     if( iter != controllers_map.end() )
00371     {
00372       controllers_map[iter->first] = ctrlr_data;
00373     }
00374     else
00375     {
00376       ROS_ERROR("Controller %s not found", contrlr_name.c_str());
00377     }
00378 
00379     controllers_map_mutex.unlock();
00380     return 0;
00381   }
00382 
00383   JointControllerData VirtualShadowhand::getContrl( std::string contrlr_name )
00384   {
00385     controllers_map_mutex.lock();
00386     ControllersMap::iterator iter = controllers_map.find(contrlr_name);
00387 
00388     //joint found
00389     if( iter != controllers_map.end() )
00390     {
00391       JointControllerData tmp = JointControllerData(iter->second);
00392       controllers_map_mutex.unlock();
00393       return tmp;
00394     }
00395 
00396     ROS_ERROR("Controller %s not found", contrlr_name.c_str() );
00397     JointControllerData no_result;
00398     controllers_map_mutex.unlock();
00399     return no_result;
00400   }
00401 
00402   short VirtualShadowhand::setConfig( std::vector<std::string> myConfig )
00403   {
00404     ROS_WARN("The set config function is not implemented in the virtual shadowhand.");
00405     return 0;
00406   }
00407 
00408   void VirtualShadowhand::getConfig( std::string joint_name )
00409   {
00410     ROS_WARN("The get config function is not implemented in the virtual shadowhand.");
00411   }
00412 
00413   std::vector<DiagnosticData> VirtualShadowhand::getDiagnostics()
00414   {
00415     joints_map_mutex.lock();
00416     std::vector<DiagnosticData> returnVect;
00417 
00418     for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00419     {
00420       DiagnosticData tmpDiag;
00421       tmpDiag.joint_name = it->first;
00422       tmpDiag.level = 0;
00423       tmpDiag.flags = "";
00424       tmpDiag.target_sensor_num = 0;
00425       tmpDiag.target = it->second.target;
00426       tmpDiag.position_sensor_num = 0;
00427       tmpDiag.position = it-> second.position;
00428 
00429       returnVect.push_back(tmpDiag);
00430     }
00431 
00432     joints_map_mutex.unlock();
00433     return returnVect;
00434   }
00435 
00436 #ifdef GAZEBO
00437   void VirtualShadowhand::gazeboCallback(const sensor_msgs::JointStateConstPtr& msg)
00438   {
00439     joints_map_mutex.lock();
00440 
00441     //loop on all the names in the joint_states message
00442     for(unsigned int index = 0; index < msg->name.size(); ++index)
00443     {
00444       std::string joint_name = msg->name[index];
00445       JointsMap::iterator iter = joints_map.find(joint_name);
00446       //not found => can be a joint from the arm / hand
00447       if(iter == joints_map.end())
00448         continue;
00449 
00450       //joint found
00451       JointData tmpData(iter->second);
00452 
00453       tmpData.position = toDegrees(msg->position[index]);
00454       tmpData.force = msg->effort[index];
00455 
00456       joints_map[joint_name] = tmpData;
00457     }
00458 
00459     //push the sum of J1+J2 to the J0s
00460     for(JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it)
00461     {
00462       JointData tmpData = it->second;
00463       if( tmpData.isJointZero == 1 )
00464       {
00465         std::string joint_name = it->first;
00466         double position = 0.0;
00467 
00468         //get the position from joint 1
00469         ++it;
00470         JointData tmpData1 = JointData(it->second);
00471         position += tmpData1.position;
00472 
00473         //get the position from joint 2
00474         ++it;
00475         JointData tmpData2 = JointData(it->second);
00476         position += tmpData2.position;
00477 
00478         tmpData.position = position;
00479 
00480         joints_map[joint_name] = tmpData;
00481       }
00482     }
00483 
00484     joints_map_mutex.unlock();
00485   }
00486 
00487 #endif
00488 } //end namespace
00489 
00490 
00491 /* For the emacs weenies in the crowd.
00492 Local Variables:
00493    c-basic-offset: 2
00494 End:
00495 */


sr_hand
Author(s): Ugo Cupcic / ugo@shadowrobot.com, contact@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:44:02