00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import roslib; roslib.load_manifest('sr_hand')
00019 import time
00020 import os
00021 import math
00022 import rospy
00023 import subprocess
00024 import threading
00025 import rosgraph.masterapi
00026 from sr_robot_msgs.msg import sendupdate, joint, joints_data
00027 from sensor_msgs.msg import *
00028 from grasps_parser import GraspParser
00029 from grasps_interpoler import GraspInterpoler
00030
00031 class Joint():
00032 def __init__(self, name="", motor="", min=0, max=90):
00033 self.name = name
00034 self.motor = motor
00035 self.min = min
00036 self.max = max
00037
00038 class ShadowHand_ROS():
00039 """
00040 This is a python library used to easily access the shadow hand ROS interface.
00041 """
00042 def __init__(self):
00043 """
00044 Builds the library, creates the communication node in ROS and
00045 initializes the hand publisher and subscriber to the default
00046 values of shadowhand_data and sendupdate
00047 """
00048 self.allJoints = [Joint("THJ1", "smart_motor_th1"),
00049 Joint("THJ2", "smart_motor_th2", -30, 30),
00050 Joint("THJ3", "smart_motor_th3",-15, 15),
00051 Joint("THJ4", "smart_motor_th4",0, 75),
00052 Joint("THJ5", "smart_motor_th5",-60, 60),
00053 Joint("FFJ0", "smart_motor_ff2", 0, 180),
00054 Joint("FFJ3", "smart_motor_ff3"),
00055 Joint("FFJ4", "smart_motor_ff4", -25, 25),
00056 Joint("MFJ0", "smart_motor_mf2", 0, 180),
00057 Joint("MFJ3", "smart_motor_mf3"),
00058 Joint("MFJ4", "smart_motor_mf4", -25, 25),
00059 Joint("RFJ0", "smart_motor_rf2", 0, 180),
00060 Joint("RFJ3", "smart_motor_rf3"),
00061 Joint("RFJ4", "smart_motor_rf4", -25,25),
00062 Joint("LFJ0", "smart_motor_lf2", 0, 180),
00063 Joint("LFJ3", "smart_motor_lf3"),
00064 Joint("LFJ4", "smart_motor_lf4", -25, 25),
00065 Joint("LFJ5", "smart_motor_lf5", 0, 45),
00066 Joint("WRJ1", "smart_motor_wr1", -30, 40),
00067 Joint("WRJ2", "smart_motor_wr2", -30, 10),
00068 ]
00069 self.handJoints = []
00070 self.armJoints = [Joint("ShoulderJRotate", "", -45, 60),
00071 Joint("ShoulderJSwing", "", 0, 80),
00072 Joint("ElbowJSwing", "", 0,120),
00073 Joint("ElbowJRotate", "", -80,80)
00074 ]
00075 self.lastMsg = 0
00076 self.lastArmMsg = 0
00077 self.cyberglove_pub = 0
00078 self.cyberglove_sub = 0
00079 self.cybergrasp_pub = 0
00080 self.cyberglove_sub = 0
00081 self.sub = rospy.Subscriber('srh/shadowhand_data', joints_data ,self.callback)
00082 self.pub = rospy.Publisher('srh/sendupdate',sendupdate)
00083 self.sub_arm = rospy.Subscriber('sr_arm/shadowhand_data', joints_data,self.callback_arm)
00084 self.pub_arm = rospy.Publisher('sr_arm/sendupdate',sendupdate)
00085 self.isFirstMessage = True
00086 self.isFirstMessageArm = True
00087 self.isReady = False
00088 self.liste = 0
00089 self.hasarm = 0
00090 self.dict_pos = {}
00091 self.dict_tar = {}
00092 self.dict_arm_pos = {}
00093 self.dict_arm_tar = {}
00094
00095 self.sendupdate_lock = threading.Lock()
00096
00097
00098
00099 self.grasp_parser = GraspParser()
00100 process = subprocess.Popen("rospack find sr_hand".split(), stdout=subprocess.PIPE)
00101 self.rootPath = process.communicate()[0]
00102 self.rootPath = self.rootPath.split('\n')
00103 self.rootPath = self.rootPath[0]
00104
00105 self.grasp_parser.parse_tree(self.rootPath+"/python_lib/grasp/grasps.xml")
00106
00107 self.grasp_interpoler = 0
00108
00109 threading.Thread(None, rospy.spin)
00110
00111 def create_grasp_interpoler(self, current_step, next_step):
00112 self.grasp_interpoler = GraspInterpoler(current_step, next_step)
00113
00114 def callback(self, data):
00115 """
00116 @param data: The ROS message received which called the callback
00117 If the message is the first received, initializes the dictionnaries
00118 Else, it updates the lastMsg
00119 """
00120 self.lastMsg = data;
00121 if self.isFirstMessage :
00122 self.init_actual_joints()
00123 for joint in self.lastMsg.joints_list :
00124 self.dict_pos[joint.joint_name]=joint.joint_position
00125 self.dict_tar[joint.joint_name]=joint.joint_target
00126 self.isFirstMessage = False
00127 self.isReady = True
00128
00129 def callback_arm(self, data):
00130 """
00131 @param data: The ROS message received which called the callback
00132 If the message is the first received, initializes the dictionnaries
00133 Else, it updates the lastMsg
00134 """
00135 self.lastArmMsg = data
00136 if self.isFirstMessageArm :
00137 for joint in self.lastArmMsg.joints_list :
00138 self.dict_arm_pos[joint.joint_name]=joint.joint_position
00139 self.dict_arm_tar[joint.joint_name]=joint.joint_target
00140
00141 def init_actual_joints(self):
00142 """
00143 Initializes the library with just the fingers actually connected
00144 """
00145 for joint_all in self.allJoints :
00146 for joint_msg in self.lastMsg.joints_list :
00147 if joint_msg.joint_name == joint_all.name:
00148 self.handJoints.append(joint_all)
00149
00150 def check_hand_presence(self):
00151 """
00152 @return : true if the hand if detected
00153 """
00154 t = 0.0
00155 while not self.isReady:
00156 time.sleep(1.0)
00157 t = t+1.0
00158 rospy.loginfo("Waiting for service since "+str(t)+" seconds...")
00159 if t >= 5.0:
00160 rospy.logerr("No hand found. Are you sure the ROS hand is running ?")
00161 return False
00162 return True
00163
00164 def set_shadowhand_data_topic(self, topic):
00165 """
00166 @param topic: The new topic to be set as the hand publishing topic
00167 Set the library to listen to a new topic
00168 """
00169 print 'Changing subscriber to ' + topic
00170 self.sub = rospy.Subscriber(topic, joints_data ,self.callback)
00171
00172 def set_sendupdate_topic(self, topic):
00173 """
00174 @param topic: The new topic to be set as the hand subscribing topic
00175 Set the library to publish to a new topic
00176 """
00177 print 'Changing publisher to ' + topic
00178 self.pub = rospy.Publisher(topic,sendupdate)
00179
00180 def sendupdate_from_dict(self, dicti):
00181 """
00182 @param dicti: Dictionnary containing all the targets to send, mapping the name of the joint to the value of its target
00183 Sends new targets to the hand from a dictionnary
00184 """
00185
00186 message = []
00187 for join in dicti.keys():
00188 message.append(joint(joint_name=join, joint_target=dicti[join]))
00189 self.pub.publish(sendupdate(len(message), message))
00190
00191 def sendupdate(self, jointName, angle=0):
00192 """
00193 @param jointName: Name of the joint to update
00194 @param angle: Target of the joint, 0 if not set
00195 Sends a new target for the specified joint
00196 """
00197 self.sendupdate_lock.acquire()
00198 message = [joint(joint_name=jointName, joint_target=angle)]
00199 self.pub.publish(sendupdate(len(message), message))
00200 self.sendupdate_lock.release()
00201
00202 def sendupdate_arm_from_dict(self, dicti):
00203 """
00204 @param dicti: Dictionnary containing all the targets to send, mapping the name of the joint to the value of its target
00205 Sends new targets to the hand from a dictionnary
00206 """
00207 message = []
00208 for join in dicti.keys():
00209 message.append(joint(joint_name=join, joint_target=dicti[join]))
00210 self.pub_arm.publish(sendupdate(len(message), message))
00211
00212 def sendupdate_arm(self, jointName, angle=0):
00213 """
00214 @param jointName: Name of the joint to update
00215 @param angle: Target of the joint, 0 if not set
00216 Sends a new target for the specified joint
00217 """
00218 message = [joint(joint_name=jointName, joint_target=angle)]
00219 self.pub_arm.publish(sendupdate_arm(len(message), message))
00220
00221 def valueof(self, jointName):
00222 """
00223 @param jointName: Name of the joint to read the value
00224 @return: 'NaN' if the value is not correct, the actual position of the joint else
00225 """
00226 for joint in self.lastMsg.joints_list:
00227 if joint.joint_name == jointName:
00228 return float(joint.joint_position)
00229 for joint in self.lastArmMsg.joints_list:
00230 if joint.joint_name == jointName:
00231 return float(joint.joint_position)
00232 return 'NaN'
00233
00234 def has_arm(self):
00235 """
00236 @return : True if an arm is detected on the roscore
00237 """
00238 if not self.hasarm == 0:
00239 return self.hasarm
00240 self.hasarm = False
00241 if self.liste == 0:
00242 master = rosgraph.masterapi.Master('/rostopic')
00243 self.liste = master.getPublishedTopics('/')
00244 for topic_typ in self.liste :
00245 for topic in topic_typ:
00246 if 'sr_arm/shadowhand_data' in topic :
00247 self.hasarm = True
00248 return self.hasarm
00249
00250 def record_step_to_file(self, filename, grasp_as_xml):
00251 """
00252 @param filename: name (or path) of the file to save to
00253 @param grasp_as_xml: xml-formatted grasp
00254 Write the grasp at the end of the file, creates the file if does not exist
00255 """
00256 if os.path.exists(filename) :
00257 obj = open(filename, 'r')
00258 text = obj.readlines()
00259 obj.close()
00260 objWrite = open(filename,'w')
00261 for index in range(0,len(text)-1):
00262 objWrite.write(text[index])
00263 objWrite.write(grasp_as_xml)
00264 objWrite.write('</root>')
00265 objWrite.close()
00266 else :
00267 objWrite = open(filename,'w')
00268 objWrite.write('<root>\n')
00269 objWrite.write(grasp_as_xml)
00270 objWrite.write('</root>')
00271 objWrite.close()
00272
00273 def save_hand_position_to_file(self, filename):
00274 objFile=open(filename,'w')
00275 for key, value in self.dict_pos:
00276 objFile.write(key + ' ' + value + '\n')
00277 objFile.close()
00278
00279 def read_all_current_positions(self):
00280 """
00281 @return: dictionnary mapping joint names to actual positions
00282 Read all the positions in the lastMsg
00283 """
00284 if not self.isReady:
00285 return
00286 for joint in self.lastMsg.joints_list:
00287 self.dict_pos[joint.joint_name] = joint.joint_position
00288 return self.dict_pos
00289
00290 def read_all_current_targets(self):
00291 """
00292 @return: dictionnary mapping joint names to current targets
00293 Read all the targets in the lastMsg
00294 """
00295 for joint in self.lastMsg.joints_list:
00296 self.dict_tar[joint.joint_name] = joint.joint_target
00297 return self.dict_tar
00298
00299 def read_all_current_arm_positions(self):
00300 """
00301 @return: dictionnary mapping joint names to actual positions
00302 Read all the positions in the lastMsg
00303 """
00304 for joint in self.lastArmMsg.joints_list:
00305 self.dict_arm_pos[joint.joint_name] = joint.joint_position
00306 return self.dict_arm_pos
00307
00308 def read_all_current_arm_targets(self):
00309 """
00310 @return: dictionnary mapping joint names to actual targets
00311 Read all the targets in the lastMsg
00312 """
00313 for joint in self.lastArmMsg.joints_list:
00314 self.dict_arm_tar[joint.joint_name] = joint.joint_target
00315 return self.dict_arm_tar
00316
00317 def resend_targets(self):
00318 """
00319 Resend the targets read in the lastMsg to the hand
00320 """
00321 for key, value in self.dict_tar.items():
00322 self.sendupdate(jointName=key, angle=value)
00323
00324 def callVisualisationService(self, callList=0, reset = 0):
00325 """
00326 @param callList: dictionnary mapping joint names to information that should be displayed
00327 @param reset: flag used to tell if the parameters should be replaced by the new ones or just added to the previous ones
00328 Calls a ROS service to display various information in Rviz
00329 """
00330 if reset == 0:
00331 print 'no reset'
00332
00333 if reset == 1:
00334 print 'reset'
00335