Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 import roslib
00037 roslib.load_manifest('robotis')
00038 import rospy
00039
00040 from std_msgs.msg import Float64
00041 from robotis.srv import None_Float
00042 from robotis.srv import None_FloatResponse
00043 from robotis.srv import MoveAng
00044 from robotis.srv import MoveAngResponse
00045 from robotis.srv import None_Int32
00046 from robotis.srv import None_Int32Response
00047
00048 import robotis.lib_robotis as rs
00049 import time
00050 import math
00051 from threading import Thread
00052
00053
00054 class ROS_Robotis_Server():
00055
00056 def __init__(self, servo = None, name = '' ):
00057 if servo == None:
00058 raise RuntimeError( 'ROS_Robotis_Servo: No servo specified for server.\n' )
00059
00060 self.servo = servo
00061 self.name = name
00062
00063 try:
00064 rospy.init_node( 'robotis_servo_' + self.name )
00065 except rospy.ROSException:
00066 pass
00067
00068
00069
00070 rospy.logout( 'ROS_Robotis_Servo: Starting Server /robotis/servo_' + self.name )
00071 self.channel = rospy.Publisher('/robotis/servo_' + self.name, Float64)
00072
00073 self.__service_ang = rospy.Service('/robotis/servo_' + name + '_readangle',
00074 None_Float, self.__cb_readangle)
00075
00076 self.__service_ismove = rospy.Service('/robotis/servo_' + name + '_ismoving',
00077 None_Int32, self.__cb_ismoving)
00078
00079 self.__service_moveang = rospy.Service('/robotis/servo_' + name + '_moveangle',
00080 MoveAng, self.__cb_moveangle)
00081
00082 def __cb_readangle( self, request ):
00083 ang = self.update_server()
00084 return None_FloatResponse( ang )
00085
00086 def __cb_ismoving( self, request ):
00087 status = self.servo.is_moving()
00088 return None_Int32Response( int(status) )
00089
00090 def __cb_moveangle( self, request ):
00091 ang = request.angle
00092 angvel = request.angvel
00093 blocking = bool( request.blocking )
00094 self.servo.move_angle( ang, angvel, blocking )
00095 return MoveAngResponse()
00096
00097 def update_server(self):
00098 ang = self.servo.read_angle()
00099 self.channel.publish( Float64(ang) )
00100 return ang
00101
00102
00103 class ROS_Robotis_Poller( Thread ):
00104
00105 def __init__( self, dev_name, ids, names ):
00106 Thread.__init__(self)
00107
00108 self.should_run = True
00109 self.dev_name = dev_name
00110 self.ids = ids
00111 self.names = names
00112
00113 for n in self.names:
00114 rospy.logout( 'ROS_Robotis_Servo: Starting Up /robotis/servo_' + n + ' on ' + self.dev_name )
00115
00116 self.dyn = rs.USB2Dynamixel_Device( self.dev_name )
00117 self.servos = [ rs.Robotis_Servo( self.dyn, i ) for i in self.ids ]
00118 self.ros_servers = [ ROS_Robotis_Server( s, n ) for s,n in zip( self.servos, self.names ) ]
00119
00120 rospy.logout( 'ROS_Robotis_Servo: Setup Complete on ' + self.dev_name )
00121
00122 self.start()
00123
00124 def run( self ):
00125 while self.should_run and not rospy.is_shutdown():
00126 [ s.update_server() for s in self.ros_servers ]
00127 time.sleep(0.001)
00128
00129 for n in self.names:
00130 rospy.logout( 'ROS_Robotis_Servo: Shutting Down /robotis/servo_' + n + ' on ' + self.dev_name )
00131
00132 def stop(self):
00133 self.should_run = False
00134 self.join(3)
00135 if (self.isAlive()):
00136 raise RuntimeError("ROS_Robotis_Servo: unable to stop thread")
00137
00138
00139
00140 class ROS_Robotis_Client():
00141
00142 def __init__(self, name = '' ):
00143 self.name = name
00144
00145 rospy.wait_for_service('/robotis/servo_' + name + '_readangle')
00146 rospy.wait_for_service('/robotis/servo_' + name + '_ismoving')
00147 rospy.wait_for_service('/robotis/servo_' + name + '_moveangle')
00148
00149 self.__service_ang = rospy.ServiceProxy('/robotis/servo_' + name + '_readangle',
00150 None_Float)
00151
00152 self.__service_ismove = rospy.ServiceProxy('/robotis/servo_' + name + '_ismoving',
00153 None_Int32)
00154
00155 self.__service_moveang = rospy.ServiceProxy('/robotis/servo_' + name + '_moveangle',
00156 MoveAng)
00157
00158 def read_angle( self ):
00159 resp = self.__service_ang()
00160 ang = resp.value
00161 return ang
00162
00163 def is_moving( self ):
00164 resp = self.__service_ismove()
00165 return bool( resp.value )
00166
00167 def move_angle( self, ang, angvel = math.radians(50), blocking = True ):
00168 self.__service_moveang( ang, angvel, int(blocking) )
00169
00170 if __name__ == '__main__':
00171 print 'Sample Server: '
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181 dev_name = '/dev/robot/servo_left'
00182 ids = [11, 12]
00183 names = ['pan', 'tilt']
00184
00185 dyn = rs.USB2Dynamixel_Device( dev_name )
00186
00187 servos = [ rs.Robotis_Servo( dyn, i ) for i in ids ]
00188 ros_servers = [ ROS_Robotis_Server( s, n ) for s,n in zip( servos, names ) ]
00189
00190 try:
00191 while not rospy.is_shutdown():
00192 [ s.update_server() for s in ros_servers ]
00193 time.sleep(0.001)
00194 except:
00195 pass
00196
00197 for n in names:
00198 print 'ROS_Robotis_Servo: Shutting Down /robotis/servo_'+n
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
robotis
Author(s): Travis Deyle, Advait Jain, Marc Killpack, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:37:53