ros_robotis.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 # Copyright (c) 2009, Georgia Tech Research Corporation
00004 # All rights reserved.
00005 # 
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Georgia Tech Research Corporation nor the
00014 #       names of its contributors may be used to endorse or promote products
00015 #       derived from this software without specific prior written permission.
00016 # 
00017 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00018 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00021 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00022 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00023 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00024 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00025 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00026 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 #
00028 
00029 ## Controlling Robotis Dynamixel RX-28 & RX-64 servos from python
00030 ## using the USB2Dynamixel adaptor.
00031 
00032 ## Authors: Travis Deyle & Advait Jain (Healthcare Robotics Lab, Georgia Tech.)
00033 
00034 
00035 # ROS imports
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     # This class provides ROS services for a select few lib_robotis functions
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         #self.servo.disable_torque()
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     # A utility class that will setup and poll a number of ROS_Robotis_Servos on one USB2Dynamixel
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     # Provides access to the ROS services in the server.
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     # Important note: You cannot (!) use the same device (dyn) in another
00174     # process. The device is only "thread-safe" within the same
00175     # process (i.e.  between servos (and callbacks) instantiated
00176     # within that process) 
00177     
00178     # NOTE: If you are going to be polling the servers as in the snippet
00179     #       below, I recommen using a poller!  See "SAMPLE POLLER" below.
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 ## SAMPLE POLLER
00201 
00202 # The above example just constantly polls all the servos, while also
00203 # making the services available.  This generally useful code is
00204 # encapsulated in a more general poller class (which also has nicer
00205 # shutdown / restart properties).  Thus, the above example is best used as:
00206 
00207 # ROS_Robotis_Poller( '/dev/robot/servo_left', [11,12], ['pan', 'tilt'] )
00208 
00209     
00210 ## SAMPLE CLIENTS:
00211         
00212 #     tilt = ROS_Robotis_Client( 'tilt' )
00213 #     tilt.move_angle( math.radians( 0 ), math.radians(10), blocking=False)
00214 #     while tilt.is_moving():
00215 #         print 'Tilt is moving'
00216 
00217 #     pan = ROS_Robotis_Client( 'pan' )
00218 #     pan.move_angle( math.radians( 0 ), math.radians(10), blocking=False)
00219 #     while pan.is_moving():
00220 #         print 'pan is moving'
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