etherCAT_compatibility_hand.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_hand/hand/etherCAT_compatibility_hand.hpp"
00028 #include <sr_utilities/sr_math_utils.hpp>
00029 
00030 #include <time.h>
00031 #include <ros/ros.h>
00032 #include <ros/topic.h>
00033 #include <std_msgs/Float64.h>
00034 #include <pr2_controllers_msgs/JointControllerState.h>
00035 #include <sr_robot_msgs/JointControllerState.h>
00036 
00037 namespace shadowrobot
00038 {
00039   ROS_DEPRECATED EtherCATCompatibilityHand::EtherCATCompatibilityHand() :
00040     SRArticulatedRobot(), n_tilde("~")
00041   {
00042     ROS_WARN("This interface is deprecated, you should probably use the interface provided by the etherCAT driver directly.");
00043 
00044     initializeMap();
00045 
00046     joint_state_subscriber = node.subscribe("/joint_states", 2, &EtherCATCompatibilityHand::joint_states_callback, this);
00047 
00048   }
00049 
00050   EtherCATCompatibilityHand::~EtherCATCompatibilityHand()
00051   {
00052   }
00053 
00054   void EtherCATCompatibilityHand::initializeMap()
00055   {
00056     joints_map_mutex.lock();
00057     JointData tmpData;
00058     JointData tmpDataZero;
00059     tmpDataZero.isJointZero = 1;
00060     tmpDataZero.max = 180.0;
00061     tmpData.max=90.0;
00062 
00063     std::string full_topic = "";
00064 
00065     full_topic = findControllerTopicName("ffj0");
00066     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00067     int tmp_index = 0;
00068     tmpDataZero.publisher_index = tmp_index;
00069     joints_map["FFJ0"] = tmpDataZero;
00070     joints_map["FFJ1"] = tmpData;
00071     joints_map["FFJ2"] = tmpData;
00072 
00073     full_topic = findControllerTopicName("ffj3");
00074     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00075     tmp_index ++;
00076     tmpData.publisher_index = tmp_index;
00077     joints_map["FFJ3"] = tmpData;
00078 
00079     tmpData.min = -20.0;
00080     tmpData.max = 20.0;
00081     full_topic = findControllerTopicName("ffj4");
00082     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00083     tmp_index ++;
00084     tmpData.publisher_index = tmp_index;
00085     joints_map["FFJ4"] = tmpData;
00086 
00087     full_topic = findControllerTopicName("mfj0");
00088     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00089     tmp_index ++;
00090     tmpDataZero.publisher_index = tmp_index;
00091     joints_map["MFJ0"] = tmpDataZero;
00092 
00093     tmpData.min = 0.0;
00094     tmpData.max = 90.0;
00095     joints_map["MFJ1"] = tmpData;
00096     joints_map["MFJ2"] = tmpData;
00097 
00098     full_topic = findControllerTopicName("mfj3");
00099     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00100     tmp_index ++;
00101     tmpData.publisher_index = tmp_index;
00102     joints_map["MFJ3"] = tmpData;
00103 
00104     tmpData.min = -20.0;
00105     tmpData.max = 20.0;
00106     full_topic = findControllerTopicName("mfj4");
00107     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00108     tmp_index ++;
00109     tmpData.publisher_index = tmp_index;
00110     joints_map["MFJ4"] = tmpData;
00111 
00112     full_topic = findControllerTopicName("rfj0");
00113     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00114     tmp_index ++;
00115     tmpDataZero.publisher_index = tmp_index;
00116     joints_map["RFJ0"] = tmpDataZero;
00117 
00118     tmpData.min = 0.0;
00119     tmpData.max = 90.0;
00120     joints_map["RFJ1"] = tmpData;
00121     joints_map["RFJ2"] = tmpData;
00122     full_topic = findControllerTopicName("rfj3");
00123     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00124     tmp_index ++;
00125     tmpData.publisher_index = tmp_index;
00126     joints_map["RFJ3"] = tmpData;
00127 
00128     tmpData.min = -20.0;
00129     tmpData.max = 20.0;
00130     full_topic = findControllerTopicName("rfj4");
00131     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00132     tmp_index ++;
00133     tmpData.publisher_index = tmp_index;
00134     joints_map["RFJ4"] = tmpData;
00135 
00136     full_topic = findControllerTopicName("lfj0");
00137     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00138     tmp_index ++;
00139     tmpDataZero.publisher_index = tmp_index;
00140     joints_map["LFJ0"] = tmpDataZero;
00141 
00142     tmpData.min = 0.0;
00143     tmpData.max = 90.0;
00144     joints_map["LFJ1"] = tmpData;
00145     joints_map["LFJ2"] = tmpData;
00146 
00147     full_topic = findControllerTopicName("lfj3");
00148     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00149     tmp_index ++;
00150     tmpData.publisher_index = tmp_index;
00151     joints_map["LFJ3"] = tmpData;
00152 
00153     tmpData.min = -20.0;
00154     tmpData.max = 20.0;
00155     full_topic = findControllerTopicName("lfj4");
00156     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00157     tmp_index ++;
00158     tmpData.publisher_index = tmp_index;
00159     joints_map["LFJ4"] = tmpData;
00160 
00161     tmpData.min = 0.0;
00162     tmpData.max = 45.0;
00163     full_topic = findControllerTopicName("lfj5");
00164     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00165     tmp_index ++;
00166     tmpData.publisher_index = tmp_index;
00167     joints_map["LFJ5"] = tmpData;
00168 
00169     tmpData.min = 0.0;
00170     tmpData.max = 90.0;
00171     full_topic = findControllerTopicName("thj1");
00172     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00173     tmp_index ++;
00174     tmpData.publisher_index = tmp_index;
00175     joints_map["THJ1"] = tmpData;
00176 
00177     tmpData.min = -40.0;
00178     tmpData.max = 40.0;
00179     full_topic = findControllerTopicName("thj2");
00180     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00181     tmp_index ++;
00182     tmpData.publisher_index = tmp_index;
00183     joints_map["THJ2"] = tmpData;
00184 
00185     tmpData.min = -15.0;
00186     tmpData.max = 15.0;
00187     full_topic = findControllerTopicName("thj3");
00188     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00189     tmp_index ++;
00190     tmpData.publisher_index = tmp_index;
00191     joints_map["THJ3"] = tmpData;
00192 
00193     tmpData.min = 0.0;
00194     tmpData.max = 75.0;
00195     full_topic = findControllerTopicName("thj4");
00196     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00197     tmp_index ++;
00198     tmpData.publisher_index = tmp_index;
00199     joints_map["THJ4"] = tmpData;
00200 
00201     tmpData.min = -60.0;
00202     tmpData.max = 60.0;
00203     full_topic = findControllerTopicName("thj5");
00204     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00205     tmp_index ++;
00206     tmpData.publisher_index = tmp_index;
00207     joints_map["THJ5"] = tmpData;
00208 
00209     tmpData.min = -30.0;
00210     tmpData.max = 45.0;
00211     full_topic = findControllerTopicName("wrj1");
00212     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00213     tmp_index ++;
00214     tmpData.publisher_index = tmp_index;
00215     joints_map["WRJ1"] = tmpData;
00216 
00217     tmpData.min = -30.0;
00218     tmpData.max = 10.0;
00219     full_topic = findControllerTopicName("wrj2");
00220     etherCAT_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00221     tmp_index ++;
00222     tmpData.publisher_index = tmp_index;
00223     joints_map["WRJ2"] = tmpData;
00224 
00225     joints_map_mutex.unlock();
00226   }
00227 
00228   std::string EtherCATCompatibilityHand::findControllerTopicName( std::string joint_name)
00229   {
00230     ros::Duration max_wait(0.8);
00231     std::string controller_suffix = "mixed_position_velocity_controller";
00232     std::string topic = "/sh_"+ joint_name + "_" + controller_suffix + "/state";
00233     bool success = true;
00234     sr_robot_msgs::JointControllerState::ConstPtr msg_received = ros::topic::waitForMessage<sr_robot_msgs::JointControllerState>(topic, max_wait);
00235     if(!msg_received)
00236     {
00237       ROS_WARN("Mixed controller state not received for joint: %s", joint_name.c_str());
00238       controller_suffix = "position_controller";
00239       topic = "/sh_"+ joint_name + "_" + controller_suffix + "/state";
00240       pr2_controllers_msgs::JointControllerState::ConstPtr msg_received_2 = ros::topic::waitForMessage<pr2_controllers_msgs::JointControllerState>(topic, max_wait);
00241       if(!msg_received_2)
00242       {
00243         ROS_WARN("Position controller state not received for joint: %s", joint_name.c_str());
00244         success = false;
00245       }
00246     }
00247 
00248     if(!success)
00249     {
00250       std::string searched_param;
00251       n_tilde.searchParam("controller_suffix", searched_param);
00252       n_tilde.param(searched_param, controller_suffix, std::string("mixed_position_velocity_controller"));
00253     }
00254 
00255     std::string topic_prefix = "/sh_";
00256     std::string topic_suffix = "_" + controller_suffix + "/command";
00257     std::string full_topic = topic_prefix + joint_name + topic_suffix;
00258 
00259     return full_topic;
00260   }
00261 
00262   short EtherCATCompatibilityHand::sendupdate( std::string joint_name, double target )
00263   {
00264     if( !joints_map_mutex.try_lock() )
00265       return -1;
00266     JointsMap::iterator iter = joints_map.find(joint_name);
00267     std_msgs::Float64 target_msg;
00268 
00269     //not found
00270     if( iter == joints_map.end() )
00271       {
00272         ROS_DEBUG("Joint %s not found", joint_name.c_str());
00273 
00274         joints_map_mutex.unlock();
00275         return -1;
00276       }
00277 
00278     //joint found
00279     JointData tmpData(iter->second);
00280 
00281     if( target < tmpData.min )
00282       target = tmpData.min;
00283     if( target > tmpData.max )
00284       target = tmpData.max;
00285 
00286     tmpData.target = target;
00287 
00288     joints_map[joint_name] = tmpData;
00289     //the targets are in radians
00290     target_msg.data = sr_math_utils::to_rad( target );
00291 
00292     //  ROS_ERROR("Joint %s ,pub index %d, pub name %s", joint_name.c_str(),tmpData.publisher_index,etherCAT_publishers[tmpData.publisher_index].getTopic().c_str());
00293     etherCAT_publishers[tmpData.publisher_index].publish(target_msg);
00294 
00295     joints_map_mutex.unlock();
00296     return 0;
00297   }
00298 
00299   JointData EtherCATCompatibilityHand::getJointData( std::string joint_name )
00300   {
00301     JointData noData;
00302     if( !joints_map_mutex.try_lock() )
00303       return noData;
00304 
00305     JointsMap::iterator iter = joints_map.find(joint_name);
00306 
00307     //joint found
00308     if( iter != joints_map.end() )
00309       {
00310         JointData tmp = JointData(iter->second);
00311 
00312         joints_map_mutex.unlock();
00313         return tmp;
00314       }
00315 
00316     ROS_ERROR("Joint %s not found.", joint_name.c_str());
00317     joints_map_mutex.unlock();
00318     return noData;
00319   }
00320 
00321   SRArticulatedRobot::JointsMap EtherCATCompatibilityHand::getAllJointsData()
00322   {
00323     return joints_map;
00324   }
00325 
00326   short EtherCATCompatibilityHand::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
00327   {
00328     ROS_WARN("The set Controller function is not implemented in the EtherCAT compatibility wrapper, please use the provided services directly.");
00329     return 0;
00330   }
00331 
00332   JointControllerData EtherCATCompatibilityHand::getContrl( std::string contrlr_name )
00333   {
00334     JointControllerData no_result;
00335     ROS_WARN("The get Controller function is not implemented in the EtherCAT compatibility wrapper, please use the provided services directly.");
00336     return no_result;
00337   }
00338 
00339   short EtherCATCompatibilityHand::setConfig( std::vector<std::string> myConfig )
00340   {
00341     ROS_WARN("The set config function is not implemented.");
00342     return 0;
00343   }
00344 
00345   void EtherCATCompatibilityHand::getConfig( std::string joint_name )
00346   {
00347     ROS_WARN("The get config function is not implemented.");
00348   }
00349 
00350   std::vector<DiagnosticData> EtherCATCompatibilityHand::getDiagnostics()
00351   {
00352     std::vector<DiagnosticData> returnVect;
00353     return returnVect;
00354   }
00355 
00356   void EtherCATCompatibilityHand::joint_states_callback(const sensor_msgs::JointStateConstPtr& msg)
00357   {
00358     if( !joints_map_mutex.try_lock() )
00359       return;
00360     std::string fj0char;
00361     bool fj0flag=false;
00362     double fj0pos,fj0vel,fj0eff;
00363     //loop on all the names in the joint_states message
00364     for(unsigned int index = 0; index < msg->name.size(); ++index)
00365       {
00366         std::string joint_name = msg->name[index];
00367         JointsMap::iterator iter = joints_map.find(joint_name);
00368 
00369         //not found => can be a joint from the arm 
00370         if(iter == joints_map.end())
00371           {
00372             continue;
00373           }
00374         else
00375           {
00376             //joint found but may be a FJ1 or FJ2 to combine into FJ0
00377             if( joint_name.find("FJ1")!=std::string::npos)
00378               {
00379                 //ROS_INFO("FJ1found");
00380                 if(fj0flag && joint_name[0]==fj0char[0]) //J2 already found for SAME joint
00381                   {
00382                     std::string j0name=fj0char+"FJ0"; 
00383                     JointsMap::iterator myiter = joints_map.find(j0name);
00384                     if(myiter == joints_map.end()) // joint is not existing
00385                       continue;
00386                     else
00387                       {
00388                         JointData tmpDatazero(myiter->second);
00389                         tmpDatazero.position = fj0pos+ sr_math_utils::to_degrees(msg->position[index]);
00390                         tmpDatazero.velocity = fj0vel + msg->effort[index];
00391                         tmpDatazero.force = fj0eff + msg->velocity[index];
00392                         joints_map[j0name] = tmpDatazero;
00393         
00394                         fj0pos=0;
00395                         fj0vel=0;
00396                         fj0eff=0;
00397                         fj0char.clear();
00398                         fj0flag=false;
00399                       }
00400                   }
00401                 else
00402                   {
00403                     fj0pos=sr_math_utils::to_degrees(msg->position[index]);
00404                     fj0eff=msg->effort[index];
00405                     fj0vel=msg->velocity[index];
00406                     fj0char.push_back(joint_name[0]);
00407                     fj0flag=true;
00408                     //ROS_INFO("FFJ1found%s, j0char:%s,flag:%d,equal:%d",joint_name.c_str(),fj0char.c_str(),fj0flag==true?1:0,joint_name[0]==fj0char[0]?1:0);
00409                   }
00410               }
00411             else if( joint_name.find("FJ2")!=std::string::npos)
00412               {
00413                 //ROS_INFO("FJ2found");
00414                 if(fj0flag && joint_name[0]==fj0char[0]) //J1 already found for SAME joint
00415                   {
00416                     std::string j0name=fj0char+"FJ0";
00417                     JointsMap::iterator myiter = joints_map.find(j0name);
00418                     if(myiter == joints_map.end()) // joint is not existing
00419                       continue;
00420                     else
00421                       {
00422                         JointData tmpDatazero(myiter->second);
00423                         tmpDatazero.position = fj0pos+ sr_math_utils::to_degrees(msg->position[index]);
00424                         tmpDatazero.force = fj0vel + msg->effort[index];
00425                         tmpDatazero.velocity = fj0eff + msg->velocity[index];
00426                         joints_map[j0name] = tmpDatazero;
00427           
00428                         fj0pos=0;
00429                         fj0vel=0;
00430                         fj0eff=0;
00431                         fj0char.clear();
00432                         fj0flag=false;
00433                       }
00434                   }
00435                 else
00436                   {
00437                     fj0pos=sr_math_utils::to_degrees(msg->position[index]);
00438                     fj0eff=msg->effort[index];
00439                     fj0vel=msg->velocity[index];
00440                     fj0char.push_back(joint_name[0]);
00441                     fj0flag=true;
00442                     //ROS_INFO("FFJ2found%s, j0char:%s,flag:%d,equal:%d",joint_name.c_str(),fj0char.c_str(),fj0flag==true?1:0,joint_name[0]==fj0char[0]?1:0);
00443                   }
00444               }
00445             // any way do fill the found joint data and update the joint map.
00446             JointData tmpData(iter->second);
00447             tmpData.position = sr_math_utils::to_degrees(msg->position[index]);
00448             tmpData.force = msg->effort[index];
00449             tmpData.velocity = msg->velocity[index];
00450             joints_map[joint_name] = tmpData;
00451           }  
00452     
00453       }
00454 
00455     joints_map_mutex.unlock();
00456   }
00457 
00458 } //end namespace
00459 
00460 
00461 /* For the emacs weenies in the crowd.
00462 Local Variables:
00463    c-basic-offset: 2
00464 End:
00465 */


sr_hand
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:08:51