CAN_compatibility_arm.cpp
Go to the documentation of this file.
00001 
00028 #include "sr_hand/hand/CAN_compatibility_arm.hpp"
00029 #include <sr_utilities/sr_math_utils.hpp>
00030 
00031 #include <time.h>
00032 #include <ros/ros.h>
00033 #include <std_msgs/Float64.h>
00034 
00035 namespace shadowrobot
00036 {
00037   ROS_DEPRECATED CANCompatibilityArm::CANCompatibilityArm() :
00038     SRArticulatedRobot(), n_tilde("~")
00039   {
00040     ROS_WARN("This interface is deprecated, you should probably use the interface provided by the CAN driver directly.");
00041 
00042     initializeMap();
00043 
00044     joint_state_subscriber = node.subscribe("joint_states", 2, &CANCompatibilityArm::joint_states_callback, this);
00045 
00046   }
00047 
00048   CANCompatibilityArm::~CANCompatibilityArm()
00049   {
00050   }
00051 
00052   void CANCompatibilityArm::initializeMap()
00053   {
00054     joints_map_mutex.lock();
00055     JointData tmpData;
00056 
00057     std::string controller_suffix;
00058     std::string searched_param;
00059     n_tilde.searchParam("controller_suffix", searched_param);
00060     n_tilde.param(searched_param, controller_suffix, std::string("position_controller"));
00061     std::string topic_prefix = "/sa_";
00062     std::string topic_suffix = "_"+controller_suffix+"/command";
00063     std::string full_topic = "";
00064 
00065     tmpData.min = -45.0;
00066     tmpData.max = 45.0;
00067 
00068     full_topic = topic_prefix + "sr" + topic_suffix;
00069     CAN_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00070     int tmp_index = 0;
00071     tmpData.publisher_index = tmp_index;
00072     joints_map["ShoulderJRotate"] = tmpData;
00073 
00074     tmpData.min = 0.0;
00075     tmpData.max = 80.0;
00076 
00077     full_topic = topic_prefix + "ss" + topic_suffix;
00078     CAN_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00079     tmp_index ++;
00080     tmpData.publisher_index = tmp_index;
00081     joints_map["ShoulderJSwing"] = tmpData;
00082 
00083     tmpData.min = 20.0;
00084     tmpData.max = 120.0;
00085     full_topic = topic_prefix + "es" + topic_suffix;
00086     CAN_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00087     tmp_index ++;
00088     tmpData.publisher_index = tmp_index;
00089     joints_map["ElbowJSwing"] = tmpData;
00090 
00091     tmpData.min = -80.0;
00092     tmpData.max = 80.0;
00093 
00094     full_topic = topic_prefix + "er" + topic_suffix;
00095     CAN_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00096     tmp_index ++;
00097     tmpData.publisher_index = tmp_index;
00098     joints_map["ElbowJRotate"] = tmpData;
00099 
00100     joints_map_mutex.unlock();
00101   }
00102 
00103   short CANCompatibilityArm::sendupdate( std::string joint_name, double target )
00104   {
00105     if( !joints_map_mutex.try_lock() )
00106       return -1;
00107     JointsMap::iterator iter = joints_map.find(joint_name);
00108     std_msgs::Float64 target_msg;
00109 
00110     //not found
00111     if( iter == joints_map.end() )
00112     {
00113       ROS_DEBUG("Joint %s not found", joint_name.c_str());
00114 
00115       joints_map_mutex.unlock();
00116       return -1;
00117     }
00118 
00119     //joint found
00120     JointData tmpData(iter->second);
00121 
00122     if( target < tmpData.min )
00123       target = tmpData.min;
00124     if( target > tmpData.max )
00125       target = tmpData.max;
00126 
00127     tmpData.target = target;
00128 
00129     joints_map[joint_name] = tmpData;
00130     //the targets are in radians
00131     target_msg.data = sr_math_utils::to_rad( target );
00132 
00133         //      ROS_ERROR("Joint %s ,pub index %d, pub name %s", joint_name.c_str(),tmpData.publisher_index,CAN_publishers[tmpData.publisher_index].getTopic().c_str());
00134     CAN_publishers[tmpData.publisher_index].publish(target_msg);
00135 
00136     joints_map_mutex.unlock();
00137     return 0;
00138   }
00139 
00140   JointData CANCompatibilityArm::getJointData( std::string joint_name )
00141   {
00142     JointData noData;
00143     if( !joints_map_mutex.try_lock() )
00144       return noData;
00145 
00146     JointsMap::iterator iter = joints_map.find(joint_name);
00147 
00148     //joint found
00149     if( iter != joints_map.end() )
00150     {
00151       JointData tmp = JointData(iter->second);
00152 
00153       joints_map_mutex.unlock();
00154       return tmp;
00155     }
00156 
00157     ROS_ERROR("Joint %s not found.", joint_name.c_str());
00158     joints_map_mutex.unlock();
00159     return noData;
00160   }
00161 
00162   SRArticulatedRobot::JointsMap CANCompatibilityArm::getAllJointsData()
00163   {
00164     return joints_map;
00165   }
00166 
00167   short CANCompatibilityArm::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
00168   {
00169     ROS_WARN("The set Controller function is not implemented in the CAN compatibility wrapper, please use the provided services directly.");
00170     return 0;
00171   }
00172 
00173   JointControllerData CANCompatibilityArm::getContrl( std::string contrlr_name )
00174   {
00175     JointControllerData no_result;
00176     ROS_WARN("The get Controller function is not implemented in the CAN compatibility wrapper, please use the provided services directly.");
00177     return no_result;
00178   }
00179 
00180   short CANCompatibilityArm::setConfig( std::vector<std::string> myConfig )
00181   {
00182     ROS_WARN("The set config function is not implemented.");
00183     return 0;
00184   }
00185 
00186   void CANCompatibilityArm::getConfig( std::string joint_name )
00187   {
00188     ROS_WARN("The get config function is not implemented.");
00189   }
00190 
00191   std::vector<DiagnosticData> CANCompatibilityArm::getDiagnostics()
00192   {
00193     std::vector<DiagnosticData> returnVect;
00194     return returnVect;
00195   }
00196 
00197   void CANCompatibilityArm::joint_states_callback(const sensor_msgs::JointStateConstPtr& msg)
00198   {
00199     if( !joints_map_mutex.try_lock() )
00200       return;
00201     //loop on all the names in the joint_states message
00202     for(unsigned int index = 0; index < msg->name.size(); ++index)
00203     {
00204       std::string joint_name = msg->name[index];
00205       JointsMap::iterator iter = joints_map.find(joint_name);
00206 
00207       //not found => can be a joint from the arm / hand
00208       if(iter == joints_map.end())
00209         continue;
00210 
00211       //joint found
00212       JointData tmpData(iter->second);
00213 
00214                         tmpData.position = sr_math_utils::to_degrees(msg->position[index]);
00215                         tmpData.force = msg->effort[index];
00216                         tmpData.velocity = msg->velocity[index];
00217                         joints_map[joint_name] = tmpData;
00218 
00219     }
00220 
00221     joints_map_mutex.unlock();
00222   }
00223 
00224 } //end namespace
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