$search
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 */