def initializeParticle(self):
self.rate.sleep()
for i in range(self.numPart):
x = random.uniform(0, self.width)
y = random.uniform(0, self.height)
while (self.mapOri.get_cell(x, y) != 0):
x = random.uniform(0, self.width)
y = random.uniform(0, self.height)
self.particleX.append(x)
self.particleY.append(y)
self.particleTheta.append(random.uniform(0, 2 * np.pi))
self.particleWeight.append(1 / self.numPart)
self.pose_array.poses.append(
get_pose(self.particleX[i], self.particleY[i],
self.particleTheta[i]))
self.poseArray_publisher.publish(self.pose_array)
obstalces = filter(lambda x: self.mapOri.get_cell(x[0], x[1]) != 0.0,
[(x, y) for x in range(self.width)
for y in range(self.height)])
self.obstacle_tree = KDTree(obstalces, leaf_size=20)
self.mapLH.grid = np.array([[
GaussianPDF(
self.get_nearest_obstacle(x, y)[0][0][0], 0,
self.laser_sigma_hit) for x in range(self.width)
] for y in range(self.height)])
self.likelihood_publisher.publish(self.mapLH.to_message())
def first_move(self):
move_list = self.config["move_list"][0]
a = move_list[0] * m.pi / 180.0
d = move_list[1]
n = move_list[2]
hf.move_function(a, 0)
p = self.particles
for i in xrange(self.size):
p[i].theta += a + r.gauss(0, self.config["first_move_sigma_angle"])
if (p[i].theta >= 2 * m.pi):
p[i].theta -= 2 * m.pi
elif (p[i].theta < 0):
p[i].theta += 2 * m.pi
for _ in xrange(n):
hf.move_function(0, d)
for i in xrange(self.size):
dx = d * m.cos(p[i].theta) + r.gauss(
0, self.config["first_move_sigma_x"])
dy = d * m.sin(p[i].theta) + r.gauss(
0, self.config["first_move_sigma_y"])
p[i].x += dx
p[i].y += dy
self.pose_array.poses[i] = hf.get_pose(p[i].x, p[i].y,
p[i].theta)
self.particles = p
self.particle_pub.publish(self.pose_array)
self.moved = True
self.num_moves += 1
def first_move(self):
move_list = self.config["move_list"][0]
a = move_list[0] * m.pi / 180.0
d = move_list[1]
n = move_list[2]
hf.move_function(a, 0)
p = self.particles
for i in xrange(self.size):
p[i].theta += a + r.gauss(0, self.config["first_move_sigma_angle"])
if(p[i].theta >= 2*m.pi):
p[i].theta -= 2*m.pi
elif(p[i].theta < 0):
p[i].theta += 2*m.pi
for _ in xrange(n):
hf.move_function(0,d)
for i in xrange(self.size):
dx = d*m.cos(p[i].theta)+r.gauss(0,self.config["first_move_sigma_x"])
dy = d*m.sin(p[i].theta)+r.gauss(0,self.config["first_move_sigma_y"])
p[i].x += dx
p[i].y += dy
self.pose_array.poses[i] = hf.get_pose(p[i].x, p[i].y, p[i].theta)
self.particles = p
self.particle_pub.publish(self.pose_array)
self.moved = True
self.num_moves += 1
def resample(self, particles, wheel):
poses = []
sdx = self.config['resample_sigma_x']
sdy = self.config['resample_sigma_y']
sdt = self.config['resample_sigma_angle']
new_p = np.random.choice(particles, self.size, p = wheel)
for i in xrange(self.size):
self.particles[i].x = new_p[i].x + r.gauss(0,sdx)
self.particles[i].y = new_p[i].y + r.gauss(0,sdy)
self.particles[i].theta = new_p[i].theta + r.gauss(0,sdt)
self.particles[i].weight = new_p[i].weight
poses.append(hf.get_pose(self.particles[i].x,self.particles[i].y,self.particles[i].theta))
self.publish_pose(poses)
self.moved = False
def initialize_particles(self):
self.size = self.config["num_particles"]
self.particles = []
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
for i in xrange(self.size):
x = r.randint(0, self.width)
y = r.randint(0, self.height)
t = r.random() * m.pi * 2
w = 1. / self.size
pose = hf.get_pose(x, y, t)
self.pose_array.poses.append(pose)
self.particles.append(Particle(x, y, t, w))
self.particle_pub.publish(self.pose_array)
def initialize_particles(self):
self.size = self.config["num_particles"]
self.particles = []
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
for i in xrange(self.size):
x = r.randint(0,self.width)
y = r.randint(0,self.height)
t = r.random()*m.pi*2
w = 1./self.size
pose = hf.get_pose(x,y,t)
self.pose_array.poses.append(pose)
self.particles.append(Particle(x,y,t,w))
self.particle_pub.publish(self.pose_array)
def moveParticles(self):
self.moveList = self.config["move_list"]
for i in range (len (self.moveList)):
self.first = i
self.mAngle = self.moveList[i][0]
self.mDist = self.moveList[i][1]
self.mSteps = self.moveList[i][2]
with open ("moveparticleslog.txt", 'a') as infile:
infile.write("mDist, mAngle, mSteps")
infile.write(str(self.mDist))
infile.write(str(self.mAngle))
infile.write(str(self.mSteps))
infile.write("\n")
#move robot by the angle
hf.move_function( m.radians( self.mAngle ), float(0.0) )
#turn by angle, the particles and add noise only for the first move
for p in range(len (self.particleArray)):
self.particleArray[p][2] += m.radians(self.mAngle)
if i == 0:
self.particleArray[p][2] += random.gauss(0, self.config["first_move_sigma_angle"])
#move particles by m.Dist mStep times
for j in range (self.mSteps):
hf.move_function(0.0, self.mDist)
#if self.first == 0:
self.stepUpdate()
with open ("moveparticleslog.txt", 'a') as infile:
infile.write("before entering the weigh particles fucntion")
infile.write("\n")
#if self.first == 0:
#weigh and resample particles here
self.weighAndResampleParticles()
self.poseArray.poses = []
for p in range(len (self.particleArray)):
self.pose = hf.get_pose(self.particleArray[p][0], self.particleArray[p][1], self.particleArray[p][2])
self.poseArray.poses.append(self.pose)
self.particleArray[p][4] = self.pose
#if self.first == 0:
#publishing particles to particle cloud
self.particlePublisher.publish(self.poseArray)
def map_callback(self, data):
self.map = Map(data)
print "map called"
#Initialize particles
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
for n in range(PARTICLE_NUM):
randX = random.randint(0, self.map.width)
randY = random.randint(0, self.map.height)
randTheta = random.uniform(0, 2 * math.pi)
self.pose_array.poses.append(helper_functions.get_pose(randX, randY, randTheta))
#Publish initial particles
self.cloud_topic.publish(self.pose_array)
#Construct likelihood field
kdt = KDTree()
def initialize_particles(self):
self.size = self.config["num_particles"]
self.particles = []
poses = []
valid_spaces = []
prob_list = np.empty(self.size)
prob_list.fill(1.0/self.size)
for i in xrange(self.width):
for j in xrange(self.height):
if self.map.get_cell(i,j) < 1.0:
valid_spaces.append(i + j*self.width)
for i in xrange(self.size):
ind = np.random.choice(valid_spaces, 1)
y = ind[0] / self.width
x = ind[0] - y*self.width
t = r.uniform(0, 2*m.pi)
w = 1./self.size
poses.append(hf.get_pose(x,y,t))
self.particles.append(Particle(x,y,t,w))
self.publish_pose(poses)
def initializeParticle(self): self.rate.sleep() for i in range(self.numPart): x = random.uniform(0,self.width) y = random.uniform(0,self.height) while(self.mapOri.get_cell(x,y) != 0): x = random.uniform(0,self.width) y = random.uniform(0,self.height) self.particleX.append(x) self.particleY.append(y) self.particleTheta.append(random.uniform(0,2*np.pi)) self.particleWeight.append(1/self.numPart) self.pose_array.poses.append(get_pose(self.particleX[i],self.particleY[i],self.particleTheta[i])) self.poseArray_publisher.publish(self.pose_array) obstalces = filter(lambda x: self.mapOri.get_cell(x[0], x[1]) != 0.0, [(x, y) for x in range(self.width) for y in range(self.height)]) self.obstacle_tree = KDTree(obstalces, leaf_size=20) self.mapLH.grid = np.array([[GaussianPDF(self.get_nearest_obstacle(x,y)[0][0][0], 0, self.laser_sigma_hit) for x in range(self.width)] for y in range(self.height)]) self.likelihood_publisher.publish(self.mapLH.to_message())
def initializeParticles(self):
#initialize all my particles
#self.particleArray = []
self.particle = []
self.numP = self.config["num_particles"]
for i in range(self.numP ):
#self.particle is an array
self.particle = np.random.uniform(0, self.map.width, 1)
self.particle = np.append(self.particle, np.random.uniform(0, self.map.height, 1) )
self.particle = np.append(self.particle, np.random.uniform(0, 6.28, 1) )
self.particle = np.append(self.particle, np.array([1.0/800.0] ) )
self.pose = hf.get_pose(self.particle[0], self.particle[1], self.particle[2])
self.poseArray.poses.append(self.pose)
self.particle = np.append(self.particle, np.array(self.pose))
self.particleArray.append(self.particle)
print ("particle publishing")
self.particlePublisher.publish(self.poseArray)
def move(self): first_move = True for moveStep in self.move_list: angle = moveStep[0] dist = moveStep[1] num = moveStep[2] print(angle) print(dist) print(num) if(first_move == True): move_function(angle,0) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): self.particleTheta[i] = self.particleTheta[i]+(angle / 180.0 * self.angle_max)+random.gauss(0,self.first_move_sigma_angle) self.particleX[i] = (self.particleX[i]+random.gauss(0,self.first_move_sigma_x)) self.particleY[i] = (self.particleY[i]+random.gauss(0,self.first_move_sigma_y)) particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i]/wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if(re<=resampleProb[j]): index = j break resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x)) resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y)) resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) for manyTimes in range(num): move_function(0,dist) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): self.particleX[i] = self.particleX[i]+random.gauss(0,self.first_move_sigma_x) self.particleY[i] = self.particleY[i]+random.gauss(0,self.first_move_sigma_y) self.particleTheta[i] = self.particleTheta[i]+random.gauss(0,self.first_move_sigma_angle) particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 else: self.particleX[i] = self.particleX[i]+math.cos(self.particleTheta[i])*dist self.particleY[i] = self.particleY[i]+math.sin(self.particleTheta[i])*dist index = 0 pTot = 1 for z in self.ranges: laserAngle = self.angle_min + self.angle_incr*index index+=1 if(z>=self.range_min and z<=self.range_max): xz = self.particleX[i]+math.cos(self.particleTheta[i]+laserAngle)*z yz = self.particleY[i]+math.sin(self.particleTheta[i]+laserAngle)*z prob = self.mapLH.get_cell(xz,yz) if(math.isnan(prob)): prob = self.laser_z_rand else: prob = self.laser_z_hit * prob + self.laser_z_rand pTot *= prob self.particleWeight[i] = self.particleWeight[i]*(pTot+0.001) wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i]/wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if(re<=resampleProb[j]): index = j break resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x)) resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y)) resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) first_move = False else: move_function(angle,0) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): self.particleTheta[i] = self.particleTheta[i]+(angle / 180.0 * self.angle_max) particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i]/wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if(re<=resampleProb[j]): index = j break resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x)) resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y)) resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) for manyTimes in range(num): move_function(0,dist) wTot = 0 wSoFar = 0 resampleProb = [] resampleX = [] resampleY = [] resampleAngle = [] resampleWeight = [] for i in range(self.numPart): particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i]) if math.isnan(particle_poss) or particle_poss == 1.0: self.particleWeight[i] = 0.0 else: self.particleX[i] = (self.particleX[i]+math.cos(self.particleTheta[i])*dist) self.particleY[i] = (self.particleY[i]+math.sin(self.particleTheta[i])*dist) index = 0 pTot = 1 for z in self.ranges: laserAngle = self.angle_min + self.angle_incr*index index+=1 if(z>=self.range_min and z<=self.range_max): xz = self.particleX[i]+math.cos(self.particleTheta[i]+laserAngle)*z yz = self.particleY[i]+math.sin(self.particleTheta[i]+laserAngle)*z prob = self.mapLH.get_cell(xz,yz) if(math.isnan(prob)): prob = self.laser_z_rand else: prob = self.laser_z_hit * prob + self.laser_z_rand pTot *= prob self.particleWeight[i] = self.particleWeight[i]*(-pTot+0.001) wTot += self.particleWeight[i] for i in range(self.numPart): self.particleWeight[i] = self.particleWeight[i]/wTot wSoFar += self.particleWeight[i] resampleProb.append(wSoFar) self.pose_array.header.stamp = rospy.Time.now() self.pose_array.poses = [] for i in range(self.numPart): re = random.random() index = -1 for j in range(self.numPart): if(re<=resampleProb[j]): index = j break resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x)) resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y)) resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle)) resampleWeight.append(self.particleWeight[index]) self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i])) self.particleX = resampleX self.particleY = resampleY self.particleTheta = resampleAngle self.particleWeight = resampleWeight self.poseArray_publisher.publish(self.pose_array) self.result_publisher.publish(self.mess) self.complete_publisher.publish(self.mess)
def get_pose(self):
return helper_functions.get_pose(self.x, self.y, self.theta)
def update_pose(self):
self.pose = helper_functions.get_pose(self.x, self.y, self.theta)
def __init__(self, x, y, theta, weight):
self.x = x
self.y = y
self.theta = theta
self.weight = weight
self.pose = helper_functions.get_pose(x, y, theta)
def move(self):
first_move = True
for moveStep in self.move_list:
angle = moveStep[0]
dist = moveStep[1]
num = moveStep[2]
print(angle)
print(dist)
print(num)
if (first_move == True):
move_function(angle, 0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i] + (
angle / 180.0 * self.angle_max) + random.gauss(
0, self.first_move_sigma_angle)
self.particleX[i] = (
self.particleX[i] +
random.gauss(0, self.first_move_sigma_x))
self.particleY[i] = (
self.particleY[i] +
random.gauss(0, self.first_move_sigma_y))
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i], resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0, dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleX[i] = self.particleX[i] + random.gauss(
0, self.first_move_sigma_x)
self.particleY[i] = self.particleY[i] + random.gauss(
0, self.first_move_sigma_y)
self.particleTheta[
i] = self.particleTheta[i] + random.gauss(
0, self.first_move_sigma_angle)
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = self.particleX[i] + math.cos(
self.particleTheta[i]) * dist
self.particleY[i] = self.particleY[i] + math.sin(
self.particleTheta[i]) * dist
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr * index
index += 1
if (z >= self.range_min
and z <= self.range_max):
xz = self.particleX[i] + math.cos(
self.particleTheta[i] + laserAngle) * z
yz = self.particleY[i] + math.sin(
self.particleTheta[i] + laserAngle) * z
prob = self.mapLH.get_cell(xz, yz)
if (math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i] * (
pTot + 0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(
self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(
self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i],
resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
first_move = False
else:
move_function(angle, 0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i] + (
angle / 180.0 * self.angle_max)
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i], resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0, dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = (
self.particleX[i] +
math.cos(self.particleTheta[i]) * dist)
self.particleY[i] = (
self.particleY[i] +
math.sin(self.particleTheta[i]) * dist)
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr * index
index += 1
if (z >= self.range_min
and z <= self.range_max):
xz = self.particleX[i] + math.cos(
self.particleTheta[i] + laserAngle) * z
yz = self.particleY[i] + math.sin(
self.particleTheta[i] + laserAngle) * z
prob = self.mapLH.get_cell(xz, yz)
if (math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i] * (
-pTot + 0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(
self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(
self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i],
resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
self.result_publisher.publish(self.mess)
self.complete_publisher.publish(self.mess)