00001
00002
00003 import rfid_model
00004 import lib_pfilter
00005
00006 import roslib
00007 roslib.load_manifest('visualization_msgs')
00008 roslib.load_manifest('hrl_lib')
00009 roslib.load_manifest('pfilter')
00010 import rospy
00011
00012 import visualization_msgs.msg as vm
00013 import hrl_lib.transforms as tr
00014 import hrl_lib.viz as viz
00015 vsm = viz.single_marker
00016 import pfilter.pfilter as pfilter
00017
00018 import random as rd
00019 import numpy as np, math
00020 import pylab as pl
00021 import time
00022
00023 class DisplayWeights:
00024 def __init__( self, pub_topic = '/particles' ):
00025 self.m = None
00026 self.pub_mark = rospy.Publisher( pub_topic, vm.Marker )
00027 self.mid = 0
00028
00029 def create_mark( self, p, c = [1.0, 0.0, 0.0, 0.8], mid = None ):
00030 if mid == None:
00031 self.mid += 1
00032 mid = self.mid
00033
00034 m = vsm( np.matrix([ p[0], p[1], 0.0 ]).T,
00035 np.matrix([ 0.0, 0.0, 0.0, 1.0 ]).T,
00036 'sphere', '/map',
00037 scale = [0.05, 0.05, 0.05],
00038 color = [1.0, 0.0, 0.0, 0.8],
00039 duration = 10.0,
00040 m_id = mid )
00041 m.header.stamp = rospy.Time.now()
00042 return m
00043
00044
00045 def update( self, particles ):
00046 xyz = np.column_stack([ particles[:,0:2], np.zeros( particles.shape[0] )]).T
00047 w = particles[:,2]
00048
00049 wmin = np.min( w )
00050 wmax = np.max( w )
00051
00052
00053
00054 if wmin == wmax:
00055 colors = np.row_stack([ np.ones( particles.shape[0] ),
00056 np.zeros( particles.shape[0] ),
00057 np.zeros( particles.shape[0] ),
00058 np.ones( particles.shape[0] ) ])
00059 else:
00060 colors = np.array([ pl.cm.jet( int( 1.0 * ( wi - wmin ) / (wmax - wmin) * 255.0 ))
00061 for wi in w ]).T
00062 m = viz.list_marker( xyz, colors, [0.05, 0.05, 0.05], 'points', '/map', 300 )
00063 m.header.stamp = rospy.Time.now()
00064 for i in xrange( 10 ):
00065 self.pub_mark.publish( m )
00066 rospy.sleep( 0.2 )
00067
00068 return
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084 rospy.init_node( 'test_rfid_pf2' )
00085
00086
00087 X,Y = np.meshgrid( np.arange(-10,10,0.1),
00088 np.arange(-10,10,0.1) )
00089 xyw = np.row_stack([ X.flatten(),
00090 Y.flatten(),
00091 np.ones( X.shape ).flatten() ]).T
00092
00093 p_set = np.copy( xyw )
00094
00095 pf = pfilter.PFilter( rfid_model.NoMotion(), rfid_model.RfidModel( rfid_model.yaml_fname ))
00096
00097 t0 = time.time()
00098
00099 rv = pf.step( 0.0, 83, p_set, should_resample_func = pfilter.retFalse )
00100
00101
00102 t_new = tr.composeHomogeneousTransform( tr.rotZ( math.radians(90) ), np.matrix([5,-5,0]).T )
00103
00104 trans = tr.invertHomogeneousTransform( t_new )
00105 xy_new = np.array((trans * tr.xyToHomogenous( xyw[:,0:2].T) )[0:2].T)
00106 xyw_new = np.column_stack([ xy_new, rv[:,2] ])
00107
00108 rv = pf.step( 0.0, 83, xyw_new, should_resample_func = pfilter.retFalse )
00109
00110 t1 = time.time()
00111 print 'Time: %3.2f ' % (t1 - t0)
00112
00113 to_print = np.copy( xyw )
00114 to_print[:,2] = rv[:,2]
00115
00116
00117
00118 dw = DisplayWeights()
00119
00120 while not rospy.is_shutdown():
00121 print 'UPDATING'
00122 dw.update( to_print )
00123
00124 m = int( raw_input( 'New RSSI: ' ))
00125
00126
00127
00128