def __init__( self, dev_name, ids, names ):
Thread.__init__(self)
self.should_run = True
self.dev_name = dev_name
self.ids = ids
self.names = names
for n in self.names:
rospy.logout( 'ROS_Robotis_Servo: Starting Up /robotis/servo_' + n + ' on ' + self.dev_name )
self.dyn = rs.USB2Dynamixel_Device( self.dev_name )
self.servos = [ rs.Robotis_Servo( self.dyn, i ) for i in self.ids ]
self.ros_servers = [ ROS_Robotis_Server( s, n ) for s,n in zip( self.servos, self.names ) ]
rospy.logout( 'ROS_Robotis_Servo: Setup Complete on ' + self.dev_name )
self.start()
# Important note: You cannot (!) use the same device (dyn) in another
# process. The device is only "thread-safe" within the same
# process (i.e. between servos (and callbacks) instantiated
# within that process)
# NOTE: If you are going to be polling the servers as in the snippet
# below, I recommen using a poller! See "SAMPLE POLLER" below.
dev_name = '/dev/robot/servo_left'
ids = [11, 12]
names = ['pan', 'tilt']
dyn = rs.USB2Dynamixel_Device( dev_name )
servos = [ rs.Robotis_Servo( dyn, i ) for i in ids ]
ros_servers = [ ROS_Robotis_Server( s, n ) for s,n in zip( servos, names ) ]
try:
while not rospy.is_shutdown():
[ s.update_server() for s in ros_servers ]
time.sleep(0.001)
except:
pass
for n in names:
print 'ROS_Robotis_Servo: Shutting Down /robotis/servo_'+n
## SAMPLE POLLER
# The above example just constantly polls all the servos, while also