← Back to branch summary

~shadowrobot/sr-ros-interface-ethercat/friction_comp_tests

« back to all changes in this revision

Viewing changes to sr_robot_lib/python_lib/etherCAT_hand_lib.py

  • Committer: Mark Addison
  • Date: 2012-02-17 17:09:18 UTC
  • mfrom: (532.2.2 shadow_robot_ethercat)
  • Revision ID: markpitchless.shadow@gmail.com-20120217170918-wkr7t2sohh09s8s3
Modifications on etherCAT_hand_lib to work with slider plugin.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sr_robot_lib/python_lib/etherCAT_hand_lib.py
20
20
import rospy
21
21
 
22
22
import time, math
 
23
import re
23
24
 
24
25
from std_msgs.msg import Float64
25
26
from sensor_msgs.msg import JointState
51
52
        self.pid_services = {}
52
53
        self.publishers = {}
53
54
        self.positions = {}
 
55
        self.velocities = {}
54
56
        self.efforts = {}
55
57
 
56
58
        self.msg_to_send = Float64()
83
85
        self.publishers[joint_name].publish(self.msg_to_send)
84
86
 
85
87
    def get_position(self, joint_name):
86
 
        """
87
 
        """
88
 
        return self.positions[joint_name]
 
88
        value = None
 
89
        try:
 
90
            value = self.positions[joint_name]
 
91
        except:
 
92
            #We check if the reason to except is that we are trying to access the joint 0
 
93
            #Position of the J0 is the addition of the positions of J1 and J2
 
94
            m = re.match("(?P<finger>\w{2})J0", joint_name)
 
95
            if m is not None:
 
96
                value = self.positions[m.group("finger") + "J1"] + self.positions[m.group("finger") + "J2"]
 
97
            else:
 
98
                raise
 
99
        return value
 
100
 
 
101
    def get_velocity(self, joint_name):
 
102
        value = None
 
103
        try:
 
104
            value = self.velocities[joint_name]
 
105
        except:
 
106
            #We check if the reason to except is that we are trying to access the joint 0
 
107
            m = re.match("(?P<finger>\w{2})J0", joint_name)
 
108
            if m is not None:
 
109
                value = self.velocities[m.group("finger") + "J1"] + self.velocities[m.group("finger") + "J2"]
 
110
            else:
 
111
                raise
 
112
        return value
89
113
 
90
114
    def get_effort(self, joint_name):
91
 
        return self.efforts[joint_name]
 
115
        value = None
 
116
        try:
 
117
            value = self.efforts[joint_name]
 
118
        except:
 
119
            #We check if the reason to except is that we are trying to access the joint 0
 
120
            #Effort of the J0 is the same as the effort of J1 and J2, so we pick J1
 
121
            m = re.match("(?P<finger>\w{2})J0", joint_name)
 
122
            if m is not None:
 
123
                value = self.efforts[m.group("finger") + "J1"]
 
124
            else:
 
125
                raise
 
126
        return value
92
127
 
93
128
    def start_record(self, joint_name, callback):
94
129
        self.record_js_callback = callback
119
154
        self.raw_values = msg.sensors
120
155
 
121
156
    def joint_state_callback(self, msg):
122
 
        for name,pos,effort in zip( msg.name, msg.position, msg.effort ):
 
157
        for name,pos,vel,effort in zip( msg.name, msg.position, msg.velocity, msg.effort ):
123
158
            self.positions[name] = pos
 
159
            self.velocities[name] = vel
124
160
            self.efforts[name] = effort
125
161
 
126
162
        if self.record_js_callback is not None:
163
199
        # return false (the library is not activated)
164
200
        try:
165
201
            rospy.wait_for_message("/debug_etherCAT_data", EthercatDebug, timeout = 0.2)
166
 
            rospy.wait_for_message("/joint_states", EthercatDebug, timeout = 0.2)
 
202
            rospy.wait_for_message("/joint_states", JointState, timeout = 0.2)
167
203
        except:
168
204
            return False
169
205
 
171
207
        self.joint_state_subscriber = rospy.Subscriber("/joint_states", JointState, self.joint_state_callback)
172
208
        return True
173
209
 
 
210
    def activate_joint_states(self):
 
211
        #check if something is being published to this topic, otherwise
 
212
        # return false (the library is not activated)
 
213
        try:
 
214
            rospy.wait_for_message("/gazebo/joint_states", JointState, timeout = 0.2)
 
215
        except:
 
216
            return False
 
217
 
 
218
        self.joint_state_subscriber = rospy.Subscriber("/gazebo/joint_states", JointState, self.joint_state_callback)
 
219
        return True
 
220
 
174
221
    def on_close(self):
175
222
        if self.debug_subscriber is not None:
176
223
            self.debug_subscriber.unregister()