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()
print("gg")
break
if collision:
collision_count = 1
print("\rState: "+car.state_str(), "| Goal:", nav_pos, end="\t")
pos = (car.x, car.y, car.yaw)
sdata = lmodel.measure(pos)
plist = EndPoint(pos, [sensor_size,-120,120], sdata)
# Particle Filter
pf.feed((car.v, car.w, car.dt), sdata)
print(pf.neff)
if pf.neff < particle_size/2:
print("re")
pf.resampling()
pmimg = pf.particle_list[5].gmap.adaptive_get_map_prob()
pmimg = (255*pmimg).astype(np.uint8)
pmimg = cv2.cvtColor(pmimg, cv2.COLOR_GRAY2RGB)
pmimg_ = cv2.flip(pmimg,0)
cv2.imshow("pmap", pmimg_)
# Map
gm.update_map(pos, [sensor_size,-120,120,max_dist], sdata)
mimg = gm.adaptive_get_map_prob()
mimg = (255*mimg).astype(np.uint8)
mimg = cv2.cvtColor(mimg, cv2.COLOR_GRAY2RGB)
mimg_ = cv2.flip(mimg,0)
cv2.imshow("map", mimg_)
action = 4
if k == ord('i'):
action = 5
if k == ord('j'):
action = 6
if k == ord('l'):
action = 7
if k == ord('k'):
action = 8
if action > 0:
robot.move(action)
sensor_data = robot.measure(env)
SensorMapping(m, robot.pose, robot.sensor.config, sensor_data)
img = Draw(env, 1, robot.pose, sensor_data, robot.sensor.config)
mimg = AdaptiveGetMap(m)
pf.update(action, sensor_data)
mid = np.argmax(pf.weights)
imgp0 = AdaptiveGetMap(pf.particles[mid].grid_map)
img = DrawParticle(img, pf.particles)
cv2.imshow('view', img)
cv2.imshow('map', mimg)
cv2.imshow('particle_map', imgp0)
pf.resampling(sensor_data)
cv2.destroyAllWindows()
