rfid_hardware: pr2_ears_server.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 # Merges functionality from ros_M5e in hrl_rfid package.
00004 #     if opt.device == 'ears':
00005 #         print 'Starting Ears RFID Services'
00006 #         ros_rfid = ROS_M5e( name = 'ears', readPwr = opt.power,
00007 #                             portStr = '/dev/robot/RFIDreader',
00008 #                             antFuncs = [EleLeftEar, EleRightEar],
00009 #                             callbacks = [] )
00010 #         rospy.spin()
00011 #         ros_rfid.stop()
00012 
00013 
00014 import roslib
00015 roslib.load_manifest( 'rfid_hardware' )
00016 import rospy
00017 
00018 import time
00019 from threading import Thread
00020 
00021 # The Servers need to distinguish b/w real vs. simulated!
00022 #   They implement same services / functions. (Clients unaffected)
00023 import os
00024 if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim':
00025     roslib.load_manifest( 'rfid_people_following' )  # this code should really be moved to "RFID Sim" or some such thing, but I'm in a hurry.
00026     import rfid_people_following.robotis_servo_sim as rr
00027     import rfid_people_following.M5e_reader_sim as rM5e
00028     
00029 else:
00030     import robotis.ros_robotis as rr
00031     import hrl_rfid.ros_M5e as rM5e
00032 
00033 
00034 if __name__ == '__main__':
00035 #     p_right = rr.ROS_Robotis_Poller( '/dev/robot/servo1', [29,30], ['right_pan', 'right_tilt'] )
00036 #     p_left = rr.ROS_Robotis_Poller( '/dev/robot/servo0', [27,28], ['left_pan', 'left_tilt'] )
00037     p_left = rr.ROS_Robotis_Poller( '/dev/robot/servo1', [29,30], ['left_pan', 'left_tilt'] )
00038     p_right = rr.ROS_Robotis_Poller( '/dev/robot/servo0', [27,28], ['right_pan', 'right_tilt'] )
00039     # Hack to prevent the right servo from shaking.
00040     p_right.servos[0].write_address( 27, [3] ) # change the right pan compliance region
00041     p_right.servos[0].write_address( 26, [3] ) # change the right pan compliance region
00042 #     p_right = rr.ROS_Robotis_Poller( '/dev/robot/servo0', [29,30], ['right_pan', 'right_tilt'] )
00043 #     p_left = rr.ROS_Robotis_Poller( '/dev/robot/servo1', [27,28], ['left_pan', 'left_tilt'] )
00044     ros_rfid = rM5e.ROS_M5e( name = 'ears', readPwr = 3000,
00045                              portStr = '/dev/robot/RFIDreader',
00046                              antFuncs = [ rM5e.EleLeftEar, rM5e.EleRightEar ],
00047                              callbacks = [] )
00048     rospy.spin()
00049     
00050     ros_rfid.stop()
00051     p_right.stop()
00052     p_left.stop()
00053