00001
00002 import time
00003 import roslib
00004 roslib.load_manifest('rospy')
00005 roslib.load_manifest('geometry_msgs')
00006 roslib.load_manifest('std_msgs')
00007 roslib.load_manifest('hrl_rfid')
00008 roslib.load_manifest('robotis')
00009 roslib.load_manifest('rfid_people_following')
00010
00011 import time
00012 import rospy
00013 from geometry_msgs.msg import Twist
00014 from std_msgs.msg import Float64
00015 from rfid_people_following.srv import StringInt32_Int32
00016 from rfid_people_following.srv import Int32_Int32
00017 from rfid_people_following.srv import StringInt32_Int32Response
00018 from std_srvs.srv import Empty
00019
00020 import numpy as np, math
00021 import os
00022
00023 import hrl_rfid.ros_M5e_client as rmc
00024 import robotis.ros_robotis as rr
00025
00026 from threading import Thread
00027 from collections import deque
00028
00029 SCORE_BUFFER_LEN = 5
00030 SCORE_BUFFER_MOVE = 3
00031 ANGLE_FILTER_LEN = 5
00032 ANGLE_SLEW = math.radians( 10 )
00033
00034 def ctime():
00035 return rospy.Time.now().to_time()
00036
00037 class ServoNode( ):
00038 def __init__( self ):
00039 rospy.logout('servo_node: Initializing')
00040 try:
00041 rospy.init_node('servo_node')
00042 except:
00043 pass
00044
00045
00046 self.p_left = rr.ROS_Robotis_Client( 'left_pan' )
00047 self.t_left = rr.ROS_Robotis_Client( 'left_tilt' )
00048 self.p_right = rr.ROS_Robotis_Client( 'right_pan' )
00049 self.t_right = rr.ROS_Robotis_Client( 'right_tilt' )
00050
00051
00052
00053 self.scores = deque()
00054 self.og_pub = rospy.Publisher( 'assisted_teleop_check', Twist )
00055 self.og_sub = rospy.Subscriber( 'assisted_teleop_score', Float64, self.og_score_cb )
00056 self.command_pub = rospy.Publisher( 'rfid_cmd_vel', Twist )
00057 self._service_servo = rospy.Service('/rfid_servo/servo',
00058 StringInt32_Int32,
00059 self.run )
00060 self._service_stop = rospy.Service('/rfid_servo/stop', Empty, self.servo_stop )
00061 self._service_stop = rospy.Service('/rfid_servo/abort', Empty, self.servo_abort )
00062 self._service_stop_next_obs = rospy.Service('/rfid_servo/stop_next_obs', Int32_Int32, self.stop_request )
00063 self.stop_next_obs = False
00064 self.should_run = True
00065 self.abort = False
00066 rospy.logout( 'servo_node: Service ready and waiting' )
00067
00068 def stop_request( self, msg = None ):
00069
00070 self.stop_next_obs = not self.stop_next_obs
00071 rospy.logout( 'servo_node: Stop after next obstacle flag set to %s' % str( self.stop_next_obs ))
00072 return True
00073
00074 def servo_stop( self, msg = None ):
00075
00076 self.should_run = not self.should_run
00077 rospy.logout( 'servo_node: Toggled should_run to %s' % str( self.should_run ))
00078
00079 def servo_abort( self, msg = None ):
00080
00081 self.abort = True
00082 rospy.logout( 'servo_node: Aborting!' )
00083
00084 def og_score_cb( self, msg ):
00085 self.scores.append( msg.data )
00086 if len( self.scores ) > SCORE_BUFFER_LEN:
00087 self.scores.popleft()
00088
00089 def run( self, request ):
00090 rospy.logout( 'servo_node: Called for tagid: \'%s\', continous: %d' % (request.data, request.cont) )
00091
00092
00093 self.t_left.move_angle( 0.0, math.radians(30), blocking = False )
00094 self.t_right.move_angle( 0.0, math.radians(30), blocking = False )
00095 self.p_left.move_angle( math.radians(-40), math.radians(30), blocking = False )
00096 self.p_right.move_angle( math.radians(40), math.radians(30), blocking = True )
00097
00098 while self.p_left.is_moving() or self.p_right.is_moving():
00099
00100 rospy.sleep( 0.05 )
00101
00102 self.stop_next_obs = bool( request.cont )
00103 self.should_run = True
00104 self.abort = False
00105
00106
00107 self.rp = rfid_poller( request.data )
00108
00109 rospy.logout('servo_node: Running')
00110 rate = rospy.Rate( 15.0 )
00111 zed = deque([ 0.0 ])
00112 last_zed = 0.0
00113
00114 last_print = ctime()
00115
00116 while self.should_run and not rospy.is_shutdown():
00117 rate.sleep()
00118 left, right = self.rp.values
00119
00120
00121 zed.append( 0.10 * (left - right)/5.0 )
00122 if len( zed ) > ANGLE_FILTER_LEN:
00123 zed.popleft()
00124
00125 target_zed = np.mean( zed )
00126 new_zed = last_zed + np.clip( target_zed - last_zed, -1.0 * ANGLE_SLEW, ANGLE_SLEW )
00127 last_zed = new_zed
00128
00129 check = Twist()
00130 check.linear.x = 0.1
00131 check.angular.z = new_zed
00132 self.og_pub.publish( check )
00133
00134 move_command = Twist()
00135
00136 if self.abort:
00137 self.should_run = False
00138 elif len( self.scores ) < SCORE_BUFFER_LEN:
00139 rospy.logout( 'servo_node: Score buffer filling: %d of %d' % (len(self.scores), SCORE_BUFFER_LEN) )
00140 elif sum([ i != -1 for i in self.scores]) < SCORE_BUFFER_MOVE:
00141
00142 if ctime() - last_print > 1.0:
00143 rospy.logout( 'servo_node: Obstacle detected' )
00144 last_print = ctime()
00145 if self.stop_next_obs:
00146 self.should_run = False
00147 elif len( zed ) < ANGLE_FILTER_LEN:
00148 rospy.logout( 'servo_node: Angle buffer filling' )
00149 else:
00150 move_command.linear.x = check.linear.x
00151 move_command.angular.z = check.angular.z
00152
00153 self.command_pub.publish( move_command )
00154
00155
00156 self.command_pub.publish( Twist() )
00157 self.rp.stop()
00158
00159
00160 self.t_left.move_angle( 0.0, math.radians(10), blocking = False )
00161 self.t_right.move_angle( 0.0, math.radians(10), blocking = False )
00162 self.p_left.move_angle( -1.350, math.radians(10), blocking = False )
00163 if not (os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim'):
00164 self.p_right.move_angle( 1.350, math.radians(10), blocking = False )
00165 else:
00166 self.p_right.move_angle( 1.350, math.radians(10), blocking = True )
00167
00168 rospy.logout('servo_node: Returning')
00169 return StringInt32_Int32Response( int( True ))
00170
00171
00172 class ServoNodeClient():
00173 def __init__( self, service_name = '/rfid_servo/servo' ):
00174 rospy.logout( 'servo_node_client: Waiting for service: \'%s\'' % service_name )
00175 rospy.wait_for_service( service_name )
00176 rospy.logout( 'servo_node_client: Service ready.' )
00177
00178 self._servo_service = rospy.ServiceProxy( service_name, StringInt32_Int32 )
00179
00180 def servo( self, tagid, continuous_operation = True ):
00181 return self._servo_service( tagid, int( continuous_operation ))
00182
00183
00184 class rfid_poller( Thread ):
00185 def __init__( self, tagid ):
00186 Thread.__init__( self )
00187 self.reader = rmc.ROS_M5e_Client('ears')
00188 self.reader.track_mode( tagid )
00189 self.should_run = True
00190 self.values = [75.0, 75.0]
00191 self.start()
00192
00193 def stop( self ):
00194 self.should_run = False
00195 self.join(3)
00196 if (self.isAlive()):
00197 raise RuntimeError("rfid_poller: Unable to stop thread")
00198
00199 def run( self ):
00200 rospy.logout( 'rfid_poller: Starting' )
00201 while self.should_run and not rospy.is_shutdown():
00202 left = np.clip( self.reader.read('EleLeftEar')[-1], 75.0, 110.0 )
00203 right = np.clip( self.reader.read('EleRightEar')[-1], 75.0, 110.0 )
00204 self.values = [left, right]
00205 try:
00206 self.reader.stop()
00207 except:
00208 pass
00209 rospy.logout( 'rfid_poller: Exiting' )
00210
00211
00212
00213
00214
00215
00216 if __name__ == '__main__':
00217 sn = ServoNode()
00218 rospy.spin()
00219