def main():
global particles
global recive_particles
rospy.init_node('particle_filter', anonymous=True)
PF_l = ParticleFilter(Np=300)
PI_t = np.random.randn(300, 3)
fusion = RobotFusion(PF_l.particles, PI_t)
particles2fuse_cb = lambda x: particles2fuse(x, PF_l, fusion)
rospy.Subscriber('/particlecloud2fuse_in', PoseArray, particles2fuse_cb)
r = rospy.Rate(5)
#plt.ion()
#fig = plt.figure()
time_last = rospy.Time.now()
while not rospy.is_shutdown():
time_now = rospy.Time.now()
r.sleep()
fusion.PI_s = PF_l.particles
fusion.robot_tracking()
PF_l.prediction()
PF_l.likelihood_fild()
if np.abs(PF_l.update_TH()) > 0.1:
#PF_l.simpel_likelihood()
PF_l.resampling()
PF_l.i_TH = 0.0
print 'Updating particles...'
PF_l.pub()
if time_now.to_sec() - time_last.to_sec() > 2:
PF_l.pub2fuse()
time_last = time_now
if recive_particles:
fusion.PI_t = particles
PF_l.weights = fusion.update_weight()
PF_l.resampling()
recive_particles = 0
#mean = np.zeros(3)
#mean = np.mean(PF_l.particles,axis=0)
fusion.send_transfered_particles()
#M = PF_l.scan.loction_based(mean)
r.sleep()
#plt.imshow(-M+PF_l.scan.occupancy_grid)
# fig.canvas.draw()
rospy.spin()
def main():
global particles
global recive_particles
rospy.init_node('particle_filter', anonymous = True)
PF_l = ParticleFilter(Np=200)
PI_t = np.random.randn(200,3)
fusion = RobotFusion(PF_l.particles, PI_t)
particles2fuse_cb = lambda x: particles2fuse(x,PF_l,fusion)
rospy.Subscriber('/particlecloud2fuse_in', PoseArray, particles2fuse_cb)
r = rospy.Rate(5)
time_last= rospy.Time.now()
while not rospy.is_shutdown():
time_now = rospy.Time.now()
r.sleep()
fusion.PI_s = PF_l.particles
PF_l.pub()
E_x = np.mean(fusion.PI_s,axis=0)
if time_now.to_sec() - time_last.to_sec() > 1:
PF_l.pub2fuse()
time_last = time_now
if recive_particles:
print "recived particles!"
fusion.PI_t = particles
d = np.linalg.norm(np.mean(particles[:,0:2],axis=0) - fusion.mu_t_hat)
print "d= ", d
if d < 0.7:
PF_l.weights = fusion.update_weight()
PF_l.resampling()
recive_particles = 0
fusion.robot_tracking()
fusion.send_transfered_particles()
rospy.spin()
