hand_commander.cpp
Go to the documentation of this file.
00001 
00030 #include <sr_hand/hand_commander.hpp>
00031 #include <controller_manager_msgs/ListControllers.h>
00032 #include <sr_robot_msgs/sendupdate.h>
00033 #include <std_msgs/Float64.h>
00034 #include <boost/algorithm/string.hpp>
00035 
00036 namespace shadowrobot
00037 {
00038 
00039   const double HandCommander::TIMEOUT_TO_DETECT_CONTROLLER_MANAGER = 3.0;
00040 
00041   HandCommander::HandCommander(const std::string& ns):
00042     node_(ns),
00043     hand_type(shadowhandRosLib::UNKNOWN),
00044     ethercat_controllers_found(false)
00045   {
00046     //Get the urdf model from the parameter server
00047     // this is used for returning min and max for joints for example.
00048     std::string robot_desc_string;
00049     node_.param("sh_description", robot_desc_string, std::string());
00050     urdf::Model robot_model;
00051     if (!robot_model.initString(robot_desc_string))
00052     {
00053       ROS_WARN("Failed to parse urdf file - trying with robot_description instead of sh_description.");
00054 
00055       node_.param("robot_description", robot_desc_string, std::string());
00056       if (!robot_model.initString(robot_desc_string))
00057       {
00058         ROS_ERROR_STREAM("Couldn't parse the urdf file on sh_description or on robot_description (namespace=" << node_.getNamespace() << ")");
00059         return;
00060       }
00061     }
00062 
00063     all_joints = robot_model.joints_;
00064 
00065     //We use the presence of the controller_manager/list_controllers service to detect that the hand is ethercat
00066     //We look for the manager in the robots namespace (that of node_ not the process).
00067     if(ros::service::waitForService(node_.getNamespace() + "/controller_manager/list_controllers", ros::Duration(TIMEOUT_TO_DETECT_CONTROLLER_MANAGER)))
00068     {
00069       hand_type = shadowhandRosLib::ETHERCAT;
00070       initializeEthercatHand();
00071       ROS_INFO_STREAM("HandCommander library: ETHERCAT hand detected in " << node_.getNamespace());
00072     }
00073     else
00074     {
00075       hand_type = shadowhandRosLib::CAN;
00076       sr_hand_target_pub = node_.advertise<sr_robot_msgs::sendupdate>("srh/sendupdate", 2);
00077       ROS_INFO_STREAM("HandCommander library: CAN hand detected in " << node_.getNamespace());
00078     }
00079   }
00080 
00081   HandCommander::~HandCommander()
00082   {
00083   }
00084 
00085   void HandCommander::initializeEthercatHand()
00086   {
00087     sr_hand_target_pub_map.clear();
00088 
00089     ros::ServiceClient controller_list_client = node_.serviceClient<controller_manager_msgs::ListControllers>("controller_manager/list_controllers");
00090 
00091     controller_manager_msgs::ListControllers controller_list;
00092     std::string controlled_joint_name;
00093 
00094     controller_list_client.call(controller_list);
00095     for (size_t i=0;i<controller_list.response.controller.size() ;i++ )
00096     {
00097       if(controller_list.response.controller[i].state=="running")
00098       {
00099         std::string controller = node_.resolveName(controller_list.response.controller[i].name);
00100         if (node_.getParam(controller+"/joint", controlled_joint_name))
00101         {
00102           ROS_DEBUG("controller %d:%s controls joint %s\n",
00103                     (int)i,controller.c_str(),controlled_joint_name.c_str());
00104           sr_hand_target_pub_map[controlled_joint_name]
00105             = node_.advertise<std_msgs::Float64>(controller+"/command", 2);
00106           ethercat_controllers_found = true;
00107           sr_hand_sub_topics[controlled_joint_name] = controller+"/state";
00108         }
00109       }
00110     }
00111   }
00112 
00113   void HandCommander::sendCommands(std::vector<sr_robot_msgs::joint> joint_vector)
00114   {
00115     if(hand_type == shadowhandRosLib::ETHERCAT)
00116     {
00117       if(!ethercat_controllers_found)
00118       {
00119         initializeEthercatHand();
00120         // No controllers we found so bail out
00121         if (!ethercat_controllers_found)
00122           return;
00123       }
00124       for(size_t i = 0; i < joint_vector.size(); ++i)
00125       {
00126         boost::algorithm::to_upper(joint_vector.at(i).joint_name);
00127         std_msgs::Float64 target;
00128         target.data = joint_vector.at(i).joint_target * M_PI/180.0;
00129         sr_hand_target_pub_map[joint_vector.at(i).joint_name].publish(target);
00130       }
00131     }
00132     else
00133     {
00134       sr_robot_msgs::sendupdate sendupdate_msg;
00135       sendupdate_msg.sendupdate_length = joint_vector.size();
00136       sendupdate_msg.sendupdate_list = joint_vector;
00137 
00138       sr_hand_target_pub.publish(sendupdate_msg);
00139     }
00140   }
00141 
00142   std::pair<double, double> HandCommander::get_min_max(std::string joint_name)
00143   {
00144     //needs to get min max for J1 and J2 if J0
00145     std::vector<std::string> joint_names, split_name;
00146     boost::split( split_name, joint_name, boost::is_any_of("0") );
00147     if( split_name.size() == 1)
00148     {
00149       //not a J0
00150       joint_names.push_back(joint_name);
00151     }
00152     else
00153     {
00154       //this is a J0, push J1 and J2
00155       joint_names.push_back(split_name[0] + "1");
00156       joint_names.push_back(split_name[0] + "2");
00157     }
00158 
00159 
00160     std::pair<double, double> min_max;
00161     for( size_t i = 0; i < joint_names.size(); ++i)
00162     {
00163       std::string jn = joint_names[i];
00164 
00165       //urdf names are upper case
00166       boost::algorithm::to_upper(jn);
00167       std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = all_joints.find(jn);
00168 
00169       if( it != all_joints.end() )
00170       {
00171         min_max.first += it->second->limits->lower;
00172         min_max.second += it->second->limits->upper;
00173       }
00174       else
00175       {
00176         ROS_ERROR_STREAM("Joint " << jn << " not found in the urdf description.");
00177       }
00178     }
00179 
00180     return min_max;
00181   }
00182 
00183   std::vector<std::string> HandCommander::get_all_joints()
00184   {
00185     std::vector<std::string> all_joints_names;
00186     std::map<std::string, std::string>::iterator it;
00187 
00188     for( it = sr_hand_sub_topics.begin(); it != sr_hand_sub_topics.end(); ++it )
00189     {
00190       // all Hand joint names have a length of 4...
00191       //The other way would be to check if the name is in a list
00192       // of possible names. Not sure what's best.
00193       if(it->first.size() == 4)
00194         all_joints_names.push_back(it->first);
00195     }
00196 
00197     return all_joints_names;
00198   }
00199 
00200   std::string HandCommander::get_controller_state_topic(std::string joint_name)
00201   {
00202     std::string topic;
00203 
00204     if(hand_type == shadowhandRosLib::ETHERCAT)
00205     {
00206       //urdf names are upper case
00207       boost::algorithm::to_upper(joint_name);
00208       std::map<std::string, std::string>::iterator it = sr_hand_sub_topics.find(joint_name);
00209       if( it != sr_hand_sub_topics.end() )
00210       {
00211         topic = it->second;
00212       }
00213       else
00214       {
00215         ROS_ERROR_STREAM(" Controller for joint " << joint_name << " not found.");
00216       }
00217     }
00218     else
00219     {
00220       topic = "shadowhand_data";
00221     }
00222 
00223     return topic;
00224   }
00225 }
00226 
00227 /* For the emacs weenies in the crowd.
00228 Local Variables:
00229    c-basic-offset: 2
00230 End:
00231 */


sr_hand
Author(s): Ugo Cupcic
autogenerated on Fri Aug 21 2015 12:24:55