def main():
rospy.init_node('SLAM', anonymous=True)
PF = ParticleFilter(Np=100)
PF.init()
OG = occupancy_grid(max_rays=100,
grid_size=[30, 30],
initial_pose=[15, 15])
OG.update_map(scan=PF.scan.z, initialize=1)
PF.get_map(OG)
r = rospy.Rate(5)
OG.publish_occupancy_grid()
while not rospy.is_shutdown():
OG.publish_occupancy_grid()
print 'runing!'
r.sleep()
PF.pub()
if PF.i_MU[0] > 0.01 or PF.i_MU[1] > 0.01:
#print np.mean(PF.particles,axis = 0).shape
OG.update_map(X=np.mean(PF.particles, axis=0),
scan=PF.scan.z,
initialize=0)
PF.get_map(OG)
PF.i_MU = [0.0, 0.0]
print 'updated map'
rospy.spin()
def main():
rospy.init_node('SLAM', anonymous = True)
PF = ParticleFilter(Np=100)
OG = occupancy_grid()
OG.update_map(scan = PF.scan.z,initialize = 1)
PF.get_map(OG)
r = rospy.Rate(5)
while not rospy.is_shutdown():
r.sleep()
if PF.i_MU[0] > 0.01 or PF.i_MU[1] > 0.01:
OG.update_map(X = np.mean(PF.particles),scan = PF.scan.z,initialize = 0)
PF.get_map(OG)
PF.i_MU = [0.0,0.0]
rospy.spin()
