2
import roslib; roslib.load_manifest('sr_control_gui')
9
import rosgraph.masterapi
10
from sr_hand.msg import sendupdate, joint, joints_data
11
from sensor_msgs.msg import *
14
def __init__(self, name="", motor="", min=0, max=90):
20
class ShadowHand_ROS():
22
This is a python library used to easily access the shadow hand ROS interface.
26
Builds the library, creates the communication node in ROS and
27
initializes the hand publisher and subscriber to the default
28
values of shadowhand_data and sendupdate
30
print "Creating good library"
31
self.handJoints = [Joint("THJ1", "smart_motor_th1"),
32
Joint("THJ2", "smart_motor_th2", -30, 30),
33
Joint("THJ3", "smart_motor_th3",-15, 15),
34
Joint("THJ4", "smart_motor_th4",0, 75),
35
Joint("THJ5", "smart_motor_th5",-60, 60),
36
Joint("FFJ0", "smart_motor_ff2", 0, 180),
37
Joint("FFJ3", "smart_motor_ff3"),
38
Joint("FFJ4", "smart_motor_ff4", -25, 25),
39
Joint("MFJ0", "smart_motor_mf2", 0, 180),
40
Joint("MFJ3", "smart_motor_mf3"),
41
Joint("MFJ4", "smart_motor_mf4", -25, 25),
42
Joint("RFJ0", "smart_motor_rf2", 0, 180),
43
Joint("RFJ3", "smart_motor_rf3"),
44
Joint("RFJ4", "smart_motor_rf4", -25,25),
45
Joint("LFJ0", "smart_motor_lf2", 0, 180),
46
Joint("LFJ3", "smart_motor_lf3"),
47
Joint("LFJ4", "smart_motor_lf4", -25, 25),
48
Joint("LFJ5", "smart_motor_lf5", 0, 45),
49
Joint("WRJ1", "smart_motor_wr1", -30, 40),
50
Joint("WRJ2", "smart_motor_wr2", -30, 10),
52
self.armJoints = [Joint("trunk_rotation", "", -45, 90),
53
Joint("shoulder_rotation", "", 0, 90),
54
Joint("elbow_abduction", "", 0,120),
55
Joint("forearm_rotation", "", -90,90)
59
self.cyberglove_pub = 0
60
self.cyberglove_sub = 0
61
self.cybergrasp_pub = 0
62
self.cyberglove_sub = 0
63
self.sub = rospy.Subscriber('srh/shadowhand_data', joints_data ,self.callback)
64
self.pub = rospy.Publisher('srh/sendupdate',sendupdate)
65
self.sub_arm = rospy.Subscriber('sr_arm/shadowhand_data', joints_data,self.callback_arm)
66
self.pub_arm = rospy.Publisher('sr_arm/sendupdate',sendupdate)
67
self.isFirstMessage = True
68
self.isFirstMessageArm = True
73
self.jointToRecord = 'None'
76
self.dict_arm_pos = {}
77
self.dict_arm_tar = {}
78
self.dict_record_pos = {}
79
self.dict_record_tar={}
80
self.dict_record_time={}
81
rospy.init_node('python_hand_library')
82
threading.Thread(None, rospy.spin)
84
def callback(self, data):
86
@param data: The ROS message received which called the callback
87
If the message is the first received, initializes the dictionnaries
88
Else, it updates the lastMsg
91
if self.isFirstMessage :
92
for joint in self.lastMsg.joints_list :
93
self.dict_pos[joint.joint_name]=joint.joint_position
94
self.dict_tar[joint.joint_name]=joint.joint_target
95
self.dict_record_pos[joint.joint_name]=[]
96
self.dict_record_tar[joint.joint_name]=[]
97
self.isFirstMessage = False
99
if self.toRecord > 0 :
100
self.dict_record_pos[self.jointToRecord].append(self.dict_pos[self.jointToRecord])
101
self.dict_record_tar[self.jointToRecord].append(self.dict_tar)
102
self.dict_record_time[self.jointToRecord].append(rospy.Time.now())
103
toRecord = toRecord - 1
105
def callback_arm(self, data):
107
@param data: The ROS message received which called the callback
108
If the message is the first received, initializes the dictionnaries
109
Else, it updates the lastMsg
111
self.lastArmMsg = data
112
if self.isFirstMessageArm :
113
for joint in self.lastArmMsg.joints_list :
114
self.dict_arm_pos[joint.joint_name]=joint.joint_position
115
self.dict_arm_tar[joint.joint_name]=joint.joint_target
119
print('Library deleted')
121
def close_connections(self):
122
print('Connection closed')
124
def setShadowhand_data_topic(self, topic):
126
@param topic: The new topic to be set as the hand publishing topic
127
Set the library to listen to a new topic
129
print 'Changing subscriber to ' + topic
130
self.sub = rospy.Subscriber(topic, joints_data ,self.callback)
132
def setSendUpdate_topic(self, topic):
134
@param topic: The new topic to be set as the hand subscribing topic
135
Set the library to publish to a new topic
137
print 'Changing publisher to ' + topic
138
self.pub = rospy.Publisher(topic,sendupdate)
140
def sendupdate_from_dict(self, dicti):
142
@param dicti: Dictionnary containing all the targets to send, mapping the name of the joint to the value of its target
143
Sends new targets to the hand from a dictionnary
147
for join in dicti.keys():
148
message.append(joint(joint_name=join, joint_target=dicti[join]))
149
self.pub.publish(sendupdate(len(message), message))
151
def sendupdate(self, jointName, angle=0):
153
@param jointName: Name of the joint to update
154
@param angle: Target of the joint, 0 if not set
155
Sends a new target for the specified joint
157
message = [joint(joint_name=jointName, joint_target=angle)]
158
self.pub.publish(sendupdate(len(message), message))
160
def sendupdate_arm_from_dict(self, dicti):
162
@param dicti: Dictionnary containing all the targets to send, mapping the name of the joint to the value of its target
163
Sends new targets to the hand from a dictionnary
168
for join in dicti.keys():
169
message.append(joint(joint_name=join, joint_target=dicti[join]))
170
self.pub_arm.publish(sendupdate(len(message), message))
172
def sendupdate_arm(self, jointName, angle=0):
174
@param jointName: Name of the joint to update
175
@param angle: Target of the joint, 0 if not set
176
Sends a new target for the specified joint
178
message = [joint(joint_name=jointName, joint_target=angle)]
179
self.pub_arm.publish(sendupdate_arm(len(message), message))
181
def valueof(self, jointName):
183
@param jointName: Name of the joint to read the value
184
@return: 'NaN' if the value is not correct, the actual position of the joint else
186
for joint in self.lastMsg.joints_list:
187
if joint.joint_name == jointName:
188
return float(joint.joint_position)
189
for joint in self.lastArmMsg.joints_list:
190
if joint.joint_name == jointName:
191
return float(joint.joint_position)
196
@return : True if an arm is detected on the roscore
198
if not self.hasarm == 0:
202
#process = subprocess.Popen("rostopic list -p".split(), stdout=subprocess.PIPE)
203
#self.liste = process.communicate()[0]
204
#self.liste = self.liste.split('\n')
205
master = rosgraph.masterapi.Master('/rostopic')
206
self.liste = master.getPublishedTopics('/')
207
for topic_typ in self.liste :
208
for topic in topic_typ:
209
if 'sr_arm/shadowhand_data' in topic :
213
def record_step_to_file(self, filename, grasp_as_xml):
215
@param filename: name (or path) of the file to save to
216
@param grasp_as_xml: xml-formatted grasp
217
Write the grasp at the end of the file, creates the file if does not exist
219
if os.path.exists(filename) :
220
obj = open(filename, 'r')
221
text = obj.readlines()
223
objWrite = open(filename,'w')
224
for index in range(0,len(text)-1):
225
objWrite.write(text[index])
226
objWrite.write(grasp_as_xml)
227
objWrite.write('</root>')
230
objWrite = open(filename,'w')
231
objWrite.write('<root>\n')
232
objWrite.write(grasp_as_xml)
233
objWrite.write('</root>')
236
def save_hand_position_to_file(self, filename):
237
objFile=open(filename,'w')
238
for key, value in self.dict_pos:
239
objFile.write(key + ' ' + value + '\n')
242
def read_all_current_positions(self):
244
@return: dictionnary mapping joint names to actual positions
245
Read all the positions in the lastMsg
247
for joint in self.lastMsg.joints_list:
248
self.dict_pos[joint.joint_name] = joint.joint_position
251
def read_all_current_targets(self):
253
@return: dictionnary mapping joint names to current targets
254
Read all the targets in the lastMsg
256
for joint in self.lastMsg.joints_list:
257
self.dict_tar[joint.joint_name] = joint.joint_position
260
def read_all_current_arm_positions(self):
262
@return: dictionnary mapping joint names to actual positions
263
Read all the positions in the lastMsg
265
for joint in self.lastArmMsg.joints_list:
266
self.dict_arm_pos[joint.joint_name] = joint.joint_position
267
return self.dict_arm_pos
269
def read_all_current_arm_targets(self):
271
@return: dictionnary mapping joint names to actual targets
272
Read all the targets in the lastMsg
274
for joint in self.lastArmMsg.joints_list:
275
self.dict_arm_tar[joint.joint_name] = joint.joint_target
276
return self.dict_arm_tar
278
def resend_targets(self):
280
Resend the targets read in the lastMsg to the hand
282
for key, value in self.dict_tar.items():
283
self.sendupdate(jointName=key, angle=value)
285
def replay_hand_position_from_file(self, filename):
287
Depreciated, should not be used
290
objFile=open(filename,'r')
291
result = objFile.readlines()
296
dict_temp[name]=angle
298
self.sendupdate_from_dict(dict_temp)
300
def readRecordedData(self, jointName, recordingTimeInSeconds):
302
Depreciated, should not be used
305
self.fileIsComplete(recordingTimeInSeconds*100)
306
""" Getting through only when ready"""
307
positions = dict_record_pos[jointName]
308
targets = dict_record_tar[jointName]
309
times = dict_record_time[jointName]
310
for index in range(0, len(targets)) :
311
ret.append([int(times[i]), float(positions[i]), float(targets[i])])
313
for index in range(0, len(ret)) :
314
ret[index][0] = float((ret[index][0]-minTime))/1000000000.0
322
def initializeJointsPosition(self):
325
def saveContrlrSettings(self):
328
def stepFunction(self):
329
self.sendupdate(joint.name, joint.min + 0.6*(joint.max - joint.min))
331
def smoothSinFunction(self, joint):
333
for index in range(numberOfSteps):
334
theta = (float(index)/float(numberOfSteps)) * math.pi
335
mysinvalue = math.sin(theta)
336
self.sendupdate(joint.name, joint.min + mysinvalue*(joint.max - joint.min))
341
def staircaseFunction(self):
342
for percentage in [0, 0.10, 0.20, 0.30, 0.40, 0.50, 0.60, 0.70, 0.80, 0.90, 1.00, 0.90 , 0.80, 0.70, 0.60, 0.50, 0.40, 0.30, 0.20, 0.10, 0]:
343
self.sendupdate(joint.name, joint.min + percentage*(joint.max - joint.min))
347
def callVisualisationService(self, callList=0, reset = 0):
349
@param callList: dictionnary mapping joint names to information that should be displayed
350
@param reset: flag used to tell if the parameters should be replaced by the new ones or just added to the previous ones
351
Calls a ROS service to display various information in Rviz
added
removed
sr_hand/python_lib/shadowhand_ros.py
