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