Beispiel #1
0
 def bag_cap( self, request ):
     # request.data => String array
     # sample args: ['rosbag', 'record', '/tf', '/rfid/ears_reader_arr', '-o', 'data/data']
     if (request.data == [] or request.data[0] == 'kill'):
         if self.bag_pid == None:
             rospy.logout( 'orient_node: No open bag to kill.' )
         else:
             rospy.logout( 'orient_node: Killing open bag.' )
             self.bag_pid.kill()
             self.bag_pid = None
         return int( True )
     
     s = reduce( lambda x,y: x+' '+y, request.data )
     rospy.logout( 'orient_node: Calling CmdProcess with args: %s' % s )
     self.bag_pid = CmdProcess( request.data )
     self.bag_pid.run()
     return int( True )
Beispiel #2
0
class OrientNode(  ):
    def __init__( self ):
        rospy.logout('orient_node: Initializing')
        rospy.init_node('orient_node')

        # After calling "flap ears", data will look something like this:
        # { 'TagID1': [[ang,rssi], [ang,rssi], ...]
        #   'TagID2': ... }
        #  * All angles are in /base_link and rssi's from both antennas
        self.data = {}

        # Will be transformed into base frame to determine best turn angle -- results in approximately 5-degrees (max) error for small angle assumption
        self.tag_gt = { 'EleLeftEar': PointStamped(), 'EleRightEar': PointStamped() }
        self.tag_gt[ 'EleLeftEar' ].header.frame_id = '/ear_antenna_left'
        self.tag_gt[ 'EleLeftEar' ].header.stamp = rospy.Time.now()
        self.tag_gt[ 'EleLeftEar' ].point.x = 10.0
        self.tag_gt[ 'EleRightEar' ].header.frame_id = '/ear_antenna_right'
        self.tag_gt[ 'EleRightEar' ].header.stamp = rospy.Time.now()
        self.tag_gt[ 'EleRightEar' ].point.x = 10.0

        self.listener = tf.TransformListener()
        self.listener.waitForTransform('/base_link', '/ear_antenna_left',
                                       rospy.Time(0), timeout = rospy.Duration(100) )
        self.listener.waitForTransform('/base_link', '/ear_antenna_right',
                                       rospy.Time(0), timeout = rospy.Duration(100) )
        rospy.logout('orient_node: Transforms ready')

        # For movement...
        self.rotate_backup_client = rb.RotateBackupClient()
        
        # "Ears" Setup
        self.p_left = rr.ROS_Robotis_Client( 'left_pan' )
        self.t_left = rr.ROS_Robotis_Client( 'left_tilt' )
        self.p_right = rr.ROS_Robotis_Client( 'right_pan' )
        self.t_right = rr.ROS_Robotis_Client( 'right_tilt' )
    
        self.EX_1 = 1.350
        self.EX_2 = 0.920 

        self.p_left.move_angle( self.EX_1, math.radians(10), blocking = False )
        self.p_right.move_angle( -1.0 * self.EX_1, math.radians(10), blocking = True )
        self.t_left.move_angle( 0.0, math.radians(10), blocking = False )
        self.t_right.move_angle( 0.0, math.radians(10), blocking = True )
        while self.p_left.is_moving() or self.p_right.is_moving():
            time.sleep( 0.01 )

        self.bag_pid = None

        self.r = rmc.ROS_M5e_Client('ears')
        self.__service_flap = rospy.Service( '/rfid_orient/flap',
                                             String_StringArr,
                                             self.flap_ears )
        self.__service_bag = rospy.Service( '/rfid_orient/bag',
                                             StringArr_Int32,
                                             self.bag_cap )
        self.__service_orient = rospy.Service( '/rfid_orient/orient',
                                             String_Int32,
                                             self.orient )
        self.tag_arr_sub = rospy.Subscriber( '/rfid/ears_reader_arr', 
                                             RFIDreadArr, 
                                             self.add_tags )
        rospy.logout( 'orient_node: Waiting for service calls.' )

    def bag_cap( self, request ):
        # request.data => String array
        # sample args: ['rosbag', 'record', '/tf', '/rfid/ears_reader_arr', '-o', 'data/data']
        if (request.data == [] or request.data[0] == 'kill'):
            if self.bag_pid == None:
                rospy.logout( 'orient_node: No open bag to kill.' )
            else:
                rospy.logout( 'orient_node: Killing open bag.' )
                self.bag_pid.kill()
                self.bag_pid = None
            return int( True )
        
        s = reduce( lambda x,y: x+' '+y, request.data )
        rospy.logout( 'orient_node: Calling CmdProcess with args: %s' % s )
        self.bag_pid = CmdProcess( request.data )
        self.bag_pid.run()
        return int( True )

    def orient( self, request ):
        tagid = request.data
        if not self.data.has_key( tagid ):
            rospy.logout( 'Tag id \'%s\' not found during last scan.' % tagid )
            return String_Int32Response( int( False ))
        arr = np.array( self.data[ tagid ]).T
        arr = arr[:,np.argsort( arr[0] )]
        h, bins = np.histogram( arr[0], 36, ( -np.pi, np.pi ))
        ind = np.sum(arr[0][:, np.newaxis] > bins, axis = 1) - 1  # Gives indices for data into bins
        bin_centers = (bins[:-1] + bins[1:]) / 2.0

        best_dir = 0.0
        best_rssi = 0.0
        for i in np.unique( ind ):
            avg_rssi = np.mean(arr[1,np.argwhere( ind == i )])
            if  avg_rssi > best_rssi:
                best_rssi = avg_rssi
                best_dir = bin_centers[i]

        rospy.logout( 'orient_node: Best dir (deg): %2.2f with avg rssi: %2.1f' %
                      ( math.degrees(best_dir), best_rssi ))

        self.rotate_backup_client.rotate_backup( best_dir, 0.0 )
        return String_Int32Response( int( True ))
            

    def add_tags( self, msg ):
        for read in msg.arr:
            if read.rssi == -1:
                return False

            self.tag_gt[ read.antenna_name ].header.stamp = rospy.Time(0)
            try:
                pt = self.listener.transformPoint( '/base_link', 
                                                   self.tag_gt[ read.antenna_name ])
            except:
                rospy.logout( 'orient_node: Transform failed' )
                return False

            if not self.data.has_key( read.tagID ):
                self.data[ read.tagID ] = []
            self.data[ read.tagID ].append([ calculate_angle( pt ), 1.0 * read.rssi ])
        return True

    def flap_ears( self, request ):
        self.data = {}
        tagid = request.data
        if tagid == '':
            rospy.logout( 'orient_node: capture for tagid: \'\' requested. Using QueryEnv.' )
            self.r.query_mode( )
        else:
            rospy.logout( 'orient_node: capture for tagid: \'%s\' requested' % tagid )
            self.r.track_mode( tagid )
            
        forward = False
        tilt_angs = [ math.radians( 0.0 ),
                      math.radians( 0.0 ) ]

        for ta in tilt_angs:
            # Tilt
            self.t_left.move_angle( ta, math.radians( 30.0 ), blocking = False )
            self.t_right.move_angle( -1.0 * ta, math.radians( 30.0 ), blocking = False )
            while self.t_left.is_moving() or self.t_right.is_moving():
                time.sleep(0.01)

            # Pan
            if forward:
                self.p_left.move_angle( self.EX_1, math.radians( PAN_RATE ), blocking = False )
                self.p_right.move_angle( -1.0 * self.EX_1, math.radians( PAN_RATE ), blocking = True )
                forward = False
            else:
                self.p_left.move_angle( -1.0 * self.EX_2, math.radians( PAN_RATE ), blocking = False )
                self.p_right.move_angle( self.EX_2, math.radians( PAN_RATE ), blocking = True )
                forward = True

            while self.p_left.is_moving() or self.p_right.is_moving():
                time.sleep(0.01)

        time.sleep(0.1)

        self.r.stop()
        print self.data.keys()

        # Reset / Stow
        self.p_left.move_angle( self.EX_1, math.radians(10), blocking = False )
        self.t_left.move_angle( 0.0, math.radians(10), blocking = False )
    
        self.p_right.move_angle( -1.0 * self.EX_1, math.radians(10), blocking = False )
        self.t_right.move_angle( 0.0, math.radians(10), blocking = False )

        rospy.logout( 'orient_node: capture completed' )
        # print self.data

        return [self.data.keys()]