def update_particle_cloud(self, scan):
#remove NaN and inf values
scan.ranges = scan_filter(scan.ranges, self.sensor_model.scan_range_max)
if len(scan.ranges) > 0:
weights = [self.sensor_model.get_weight(scan, pose) for pose in self.particlecloud.poses]
else:
print "Error: Scan ranges null"
return
# used for finding how confident you are in the weights - decay the weight average
w_avg = mean(weights)
self.slow_decay += 0.001 * (w_avg - self.slow_decay)
self.fast_decay += 0.1 * (w_avg - self.fast_decay)
breakpoints=cumsum(weights)
maximum = max(breakpoints)
# the probability of the weights being okay
prob = max(0.0, 1.0 - (self.fast_decay/self.slow_decay))
if not prob == 0:
loops = int(len(self.particlecloud.poses) * prob)
else:
loops = 0
# Update particlecloud, given map and laser scan
pose_arr = PoseArray()
for i in range(0,len(self.particlecloud.poses)):
new_pose = Pose()
# add completely random noise to re-localise
if i < loops:
# 33.1 and 31.95 being the x and y sizes of the map respectively
new_pose.position.x = uniform(0, 33.1)
new_pose.position.y = uniform(0,31.95)
new_pose.orientation = rotateQuaternion(Quaternion(w=1), uniform(0, math.pi*2))
# otherwise use roulette wheel resampling to resample
else:
# make a random pick
pick = uniform(0, maximum)
# an nlogn implementation of the roulette wheel search - by splitting sorted list in half repeatedly
i = bisect.bisect(breakpoints, pick)
choice = self.particlecloud.poses[i]
position = choice.position
orientation = choice.orientation
# add resampling noise to cope with changes in odometry
new_pose.position.x = gauss(position.x, 0.05)
new_pose.position.y = gauss(position.y, 0.05)
rotation = gauss(0, 0.125)
new_pose.orientation = rotateQuaternion(orientation, rotation)
new_pose.orientation = orientation
pose_arr.poses.append(new_pose)
self.particlecloud = pose_arr
def initialise_particle_cloud(self, initialpose):
rospy.loginfo("initialise_particle_cloud")
# Set particle cloud to initialpose plus noise
new_poses = PoseArray()
new_poses.header.frame_id = 0 # ?
new_poses.header.stamp = rospy.Time.now()
x_var = initialpose.pose.covariance[6 * 0 + 0]
y_var = initialpose.pose.covariance[6 * 1 + 1]
rot_var = initialpose.pose.covariance[6 * 5 + 5]
for i in range(self.INITIAL_PARTICLE_COUNT):
new_pose = Pose()
new_pose.position.x = gauss(mu=initialpose.pose.pose.position.x,
sigma=x_var)
new_pose.position.y = gauss(mu=initialpose.pose.pose.position.y,
sigma=y_var)
new_pose.orientation = rotateQuaternion(
q_orig=initialpose.pose.pose.orientation,
yaw=gauss(mu=0, sigma=rot_var))
new_poses.poses.append(new_pose)
new_poses = self.topup_poses_with_random(new_poses)
return new_poses
def initialise_particle_cloud(self, initialpose):
#Set particle cloud to initialpose plus noise
noise = 1
#create an array of Poses
posArray = PoseArray()
#iterate over the number of particles and append to PosArray
for x in range(0, self.NUMBER_PREDICTED_READINGS):
pose = Pose()
pose.position.x = random.gauss(initialpose.pose.pose.position.x,
noise)
pose.position.y = random.gauss(initialpose.pose.pose.position.y,
noise)
pose.position.z = 0
#TODO:reconfig the parameters
pose.orientation = rotateQuaternion(
initialpose.pose.pose.orientation,
math.radians(random.gauss(0, 30)))
posArray.poses.extend([pose])
#print posArray
return posArray
def update_particle_cloud(self, scan):
scan.ranges = np.fromiter(
(0.0 if np.isnan(r) else r for r in scan.ranges), float)
# Update particlecloud, given map and laser scan
self.particlecloud.poses = [
particle for particle in self.particlecloud.poses
if not self.map_cell_occupied(particle)
]
weights = np.fromiter((self.sensor_model.get_weight(scan, pose)
for pose in self.particlecloud.poses), float)
new_poses = self.resample(self.particlecloud.poses, weights,
self.RESAMPLE_PARTICLE_COUNT)
self.particlecloud.poses = new_poses
self.particlecloud = self.topup_poses_with_random(self.particlecloud)
# TODO: what should these actually be?
x_var = 0.05
y_var = 0.05
rot_var = 0.05
for pose in self.particlecloud.poses:
pose.position.x = gauss(mu=pose.position.x, sigma=x_var)
pose.position.y = gauss(mu=pose.position.y, sigma=y_var)
pose.orientation = rotateQuaternion(q_orig=pose.orientation,
yaw=gauss(mu=0, sigma=rot_var))
def update_particle_cloud(self, scan):
"""
This should use the supplied laser scan to update the current
particle cloud. i.e. self.particlecloud should be updated.
:Args:
| scan (sensor_msgs.msg.LaserScan): laser scan to use for update
"""
self._latest_scan = scan
#print(scan)
#print("Entering update ",len(self.particlecloud.poses))
self.weighted_dist = OrderedDict(
) #set up an ordered dictionary of the cumulative weights as the key and the particles as the value
cumulative_weight = 0
for particle in self.particlecloud.poses:
#print(scan)
#print(self.sensor_model.get_weight(scan,particle))
self.weighted_dist[self.sensor_model.get_weight(scan, particle) +
cumulative_weight] = particle
cumulative_weight += self.sensor_model.get_weight(scan, particle)
new_PC = PoseArray()
choice = 0
increment = cumulative_weight / self.NUMBER_PREDICTED_READINGS #choose increment for resampling
#print("choice", choice, "cumulative weight", cumulative_weight)
while (choice < cumulative_weight):
random_choice = np.random.uniform(0, 1, 1)
#print("random choice",random_choice)
p = Pose()
sampled_particle = self.choose(choice) #get the resampled particle
if (random_choice < 0.9):
p.position.x = sampled_particle.position.x
p.position.y = sampled_particle.position.y
p.position.z = sampled_particle.position.z
p.orientation = sampled_particle.orientation
#print("0.95")
#print("0.95 ",p.position.x)
else:
xNoise = self.ODOM_TRANSLATION_NOISE * np.random.normal(
0, 0.5
) #with a small probability generate a sample further away to deal with the kidnapped robot problem
yNoise = self.ODOM_DRIFT_NOISE * np.random.normal(0, 0.5)
zNoise = 0
random_angular_noise = self.ODOM_ROTATION_NOISE * np.random.normal(
0, 0.3)
p.position.x = sampled_particle.position.x # + xNoise #pose positions+noise
p.position.y = sampled_particle.position.y #+ yNoise
p.position.z = sampled_particle.position.z #+ zNoise
p.orientation = rotateQuaternion(sampled_particle.orientation,
0) #random_angular_noise)
#print("0.05")
#print("0.05 ",p.position.x)
#print("Appending to new_PC ",p.position.x)
new_PC.poses.append(p) #add particle to new particle cloud
choice = choice + increment #increment for next resampled particle
#print("Length of new particle cloud ", len(new_PC.poses))
self.particlecloud = new_PC #set new particle cloud
def systematic_resampling(S, W):
# cumulative density function
cdf = []
# keep track of running total
cdf_counter = 0
# create cdf entry for each sample
for (p, w) in S:
cdf.append((p, cdf_counter + w / W))
cdf_counter += w / W
U = random.uniform(0, 1 / len(S))
counter = 0
S_n = PoseArray()
for j in range(0, len(S)):
while U > cdf[counter][1]:
counter += 1
new_particle = Pose()
new_particle.position.x = cdf[counter][0].position.x + random.gauss(
0, 0.1)
new_particle.position.y = cdf[counter][0].position.y + random.gauss(
0, 0.1)
new_particle.orientation = rotateQuaternion(
Quaternion(w=1),
getHeading(cdf[counter][0].orientation) + random.gauss(0, 0.05))
S_n.poses.append(new_particle)
U += (1 / len(S))
return S_n
def initialise_particle_cloud(self, initialpose):
"""
Set particle cloud to initialpose plus noise
Called whenever an initialpose message is received (to change the
starting location of the robot), or a new occupancy_map is received.
self.particlecloud can be initialised here. Initial pose of the robot
is also set here.
:Args:
| initialpose: the initial pose estimate
:Return:
| (geometry_msgs.msg.PoseArray) poses of the particles
"""
SPREAD = 10
result = PoseArray()
self.weights = []
for i in range(self.NUMBER_OF_PARTICLES):
new_point = Pose()
new_point.position.x = initialpose.pose.pose.position.x + random.uniform(
-SPREAD, SPREAD)
new_point.position.y = initialpose.pose.pose.position.y + random.uniform(
-SPREAD, SPREAD)
new_point.orientation = util.rotateQuaternion(
initialpose.pose.pose.orientation,
random.uniform(-math.pi, math.pi))
result.poses.append(new_point)
self.weights.append(1.0)
return result
def weighted_zscore(self):
result = Pose()
sin_rotation = 0.0
cos_rotation = 0.0
total = 0.0
for i in range(self.NUMBER_OF_PARTICLES):
total += self.weights[i]
j = 0
for i in self.particlecloud.poses:
result.position.x += i.position.x * self.weights[j] / total
result.position.y += i.position.y * self.weights[j] / total
theta = util.getHeading(i.orientation) * self.weights[j] / total
sin_rotation += math.sin(theta) * self.weights[j] / total
cos_rotation += math.cos(theta) * self.weights[j] / total
j += 1
rotation = math.atan2(sin_rotation, cos_rotation)
result.orientation = util.rotateQuaternion(Quaternion(w=1.0), rotation)
dist = []
for i in self.particlecloud.poses:
x = (i.position.x - result.position.x)**2 + (i.position.y -
result.position.y)**2
dist.append(math.sqrt(x))
dist = np.array(dist)
zscore = stats.zscore(dist)
result = Pose()
sin_rotation = 0.0
cos_rotation = 0.0
total = 0.0
#print(zscore)
for i in range(self.NUMBER_OF_PARTICLES):
if zscore[i] < 0.7 and zscore[i] > -0.7:
result.position.x += self.particlecloud.poses[i].position.x
result.position.y += self.particlecloud.poses[i].position.y
theta = util.getHeading(
self.particlecloud.poses[i].orientation)
sin_rotation += math.sin(theta)
cos_rotation += math.cos(theta)
total += 1.0
print(self.NUMBER_OF_PARTICLES)
print(total)
result.position.x /= total
result.position.y /= total
rotation = math.atan2(sin_rotation / total, cos_rotation / total)
result.orientation = util.rotateQuaternion(Quaternion(w=1.0), rotation)
return result
def update_non_amcl(self, scan, pf): self.weight_particles(scan, pf) resampledPoses = [] notAccepted = True numParticles = len(pf.particlecloud.poses) #Resamples the poses for i in range(0,numParticles): notAccepted = True while (notAccepted): index = random.randint(0,numParticles-1) posX = pf.particlecloud.poses[index].position.x posY = pf.particlecloud.poses[index].position.y if (random.uniform(0,1) < particleWeights[index]/totalWeight): notAccepted = False resampledPoses.append(pf.particlecloud.poses[index]) cont = True pArray = PoseArray() temp = [] val = Pose() count = 0 #Smudges the poses while cont: temp = [] val = resampledPoses[0] count = 0 #Removes the duplicate poses from the list for i in range(0, len(resampledPoses)): if (resampledPoses[i] == val): count = count + 1 else: temp.append(resampledPoses[i]) resampledPoses = temp #Checks that we have allocated all particles to be smudged if (len(resampledPoses) == 0): cont = False #Apply smuding to all but one of the same resampled particle for i in range(0, count): if i > 0: newPose = Pose() newPose.position.x = random.gauss(val.position.x, 0.3) #TEST THIS newPose.position.y = random.gauss(val.position.y, 0.3) newPose.orientation = rotateQuaternion(val.orientation, random.vonmisesvariate(0, 4)) #MAKE SURE TO TEST pArray.poses.append(newPose) else: pArray.poses.append(val) return pArray
def new_pose_with_error(self, pose, scan, position_noise, turn_noise=1.0):
on_map = True
newpose = Pose()
newpose = copy.deepcopy(pose)
newpose.position.x += gauss(0, 1) * position_noise
newpose.position.y += gauss(0, 1) * position_noise
newpose.orientation = rotateQuaternion(newpose.orientation,
gauss(0, 1) * turn_noise)
return newpose
def update_particle_cloud(self, scan):
"""
This should use the supplied laser scan to update the current
particle cloud. i.e. self.particlecloud should be updated.
:Args:
| scan (sensor_msgs.msg.LaserScan): laser scan to use for update
"""
self.scan = scan
movement_particles = []
dtime = rospy.Time.now().to_sec() - self.last_time.to_sec()
self.last_time = rospy.Time.now()
for i in self.particlecloud.poses:
x = i.position.x
y = i.position.y
theta = util.getHeading(i.orientation)
for j in range(4):
p = Pose()
p.position.x = x + random.uniform(-0.2, 0.2)
p.position.y = y + random.uniform(-0.2, 0.2)
p.position.z = 0
thetad = theta + random.uniform(-0.4, 0.4)
p.orientation = util.rotateQuaternion(Quaternion(w=1.0),
thetad)
probn = self.sensor_model.get_weight(scan, p)**2
movement_particles.append((probn, p))
w_avg = 0.0
for x in movement_particles:
w_avg += x[0] / len(movement_particles)
self.w_slow += self.A_SLOW * (w_avg - self.w_slow)
self.w_fast += self.A_FAST * (w_avg - self.w_fast)
particlecloudnew = PoseArray()
weights = np.asarray([i[0] for i in movement_particles]).cumsum()
new_weights = []
for i in range(self.NUMBER_OF_PARTICLES):
j = random.random() * w_avg * len(movement_particles)
index = np.searchsorted(weights, j)
pose = movement_particles[index][1]
weight = movement_particles[index][0]
if random.random() > (self.w_fast / self.w_slow):
pose = self.generate_random_map_pose()
weight = 0.0
particlecloudnew.poses.append(pose)
new_weights.append(weight)
self.particlecloud.poses = particlecloudnew.poses
self.weights = new_weights
def __init__(self):
#POSE RECORDE
self.pre_st_x=0
self.pre_st_y=0
self.pre_st_w=0
self._pre_pose = Pose()
self._pre_pose.position.x = 0
self._pre_pose.position.y = 0
self._pre_pose.position.z = 0
self._pre_pose.orientation.x = 0
self._pre_pose.orientation.y = 0
self._pre_pose.orientation.z = 0
self._pre_pose.orientation.w = 0
# Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self._update_lock = Lock()
# Parameters
self.ODOM_ROTATION_NOISE = 0 # Odometry model rotation noise
self.ODOM_TRANSLATION_NOISE = 0 # Odometry x axis (forward) noise
self.ODOM_DRIFT_NOISE = 0 # Odometry y axis (side-side) noise
self.NUMBER_PREDICTED_READINGS = 20
# Set 'previous' translation to origin
# All Transforms are given relative to 0,0,0, not in absolute coords.
self.prev_odom_x = 0.0 # Previous odometry translation from origin
self.prev_odom_y = 0.0 # Previous odometry translation from origin
self.prev_odom_heading = 0.0 # Previous heading from odometry data
self.last_odom_pose = None
# Request robot's initial odometry values to be recorded in prev_odom
self.odom_initialised = False
self.sensor_model_initialised = False
# Set default initial pose to initial position and orientation.
self.estimatedpose.pose.pose.position.x = self.INIT_X
self.estimatedpose.pose.pose.position.y = self.INIT_Y
self.estimatedpose.pose.pose.position.z = self.INIT_Z
self.estimatedpose.pose.pose.orientation = rotateQuaternion(Quaternion(w=1.0),
self.INIT_HEADING)
# NOTE: Currently not making use of covariance matrix
self.estimatedpose.header.frame_id = "/map"
self.particlecloud.header.frame_id = "/map"
# Sensor model
self.sensor_model = sensor_model.SensorModel()
def __init__(self):
#POSE RECORDE
self.pre_st_x = 0
self.pre_st_y = 0
self.pre_st_w = 0
self._pre_pose = Pose()
self._pre_pose.position.x = 0
self._pre_pose.position.y = 0
self._pre_pose.position.z = 0
self._pre_pose.orientation.x = 0
self._pre_pose.orientation.y = 0
self._pre_pose.orientation.z = 0
self._pre_pose.orientation.w = 0
# Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self._update_lock = Lock()
# Parameters
self.ODOM_ROTATION_NOISE = 0 # Odometry model rotation noise
self.ODOM_TRANSLATION_NOISE = 0 # Odometry x axis (forward) noise
self.ODOM_DRIFT_NOISE = 0 # Odometry y axis (side-side) noise
self.NUMBER_PREDICTED_READINGS = 20
# Set 'previous' translation to origin
# All Transforms are given relative to 0,0,0, not in absolute coords.
self.prev_odom_x = 0.0 # Previous odometry translation from origin
self.prev_odom_y = 0.0 # Previous odometry translation from origin
self.prev_odom_heading = 0.0 # Previous heading from odometry data
self.last_odom_pose = None
# Request robot's initial odometry values to be recorded in prev_odom
self.odom_initialised = False
self.sensor_model_initialised = False
# Set default initial pose to initial position and orientation.
self.estimatedpose.pose.pose.position.x = self.INIT_X
self.estimatedpose.pose.pose.position.y = self.INIT_Y
self.estimatedpose.pose.pose.position.z = self.INIT_Z
self.estimatedpose.pose.pose.orientation = rotateQuaternion(
Quaternion(w=1.0), self.INIT_HEADING)
# NOTE: Currently not making use of covariance matrix
self.estimatedpose.header.frame_id = "/map"
self.particlecloud.header.frame_id = "/map"
# Sensor model
self.sensor_model = sensor_model.SensorModel()
def update_particle_cloud(self, scan):
# Update particlecloud, given map and laser scan
particles = self.particlecloud.poses
# Work out weights of particles from sensor readings
weighted_particles = []
for p in particles:
pw = self.sensor_model.get_weight(scan, p)
weighted_particles.append((p, pw))
# Sort the particles by weight in descending order
sorted_weighted_particles = sorted(weighted_particles,
key=lambda p: p[1],
reverse=True)
# Keep the 3/4 of particles with the highest weights for resampling
pred_weighted_particles = sorted_weighted_particles[:int(
(1 - self.REDIST_PERCENTAGE) * self.NUMBER_PARTICLES)]
weight_sum = sum([p[1] for p in pred_weighted_particles])
# Discard and randomly distribute the remaining 1/4 of particles across the map
rand_weighted_particles = self.gen_random_particles(
int(self.REDIST_PERCENTAGE * self.NUMBER_PARTICLES))
# Resample particles according to weights
cdf_aux = 0.0
cdf = []
for (p, w) in pred_weighted_particles:
cdf.append((p, cdf_aux + w / weight_sum))
cdf_aux += w / weight_sum
u = random.uniform(0.0, 1.0 / len(pred_weighted_particles))
i = 0
new_particles = PoseArray()
for j in range(0, len(pred_weighted_particles)):
while (u > cdf[i][1]):
i += 1
new_particle = Pose()
new_particle.position.x = cdf[i][0].position.x + random.gauss(
0.0, 0.1)
new_particle.position.y = cdf[i][0].position.y + random.gauss(
0.0, 0.1)
new_particle.orientation = rotateQuaternion(
Quaternion(w=1.0),
getHeading(cdf[i][0].orientation) + random.gauss(0, 0.05))
new_particles.poses.append(new_particle)
u += (1.0 / len(pred_weighted_particles))
rand_weighted_particles = self.gen_random_particles(
int(self.REDIST_PERCENTAGE * self.NUMBER_PARTICLES))
new_particles.poses += rand_weighted_particles.poses
self.particlecloud = new_particles
def gauss_initialise(self, initialpose, numParticles, ratio, rand, pf, test): noisePose = 0.4 self.set_map_dim(pf) # Initialisation of Gaussian Parameters meanX = initialpose.position.x meanY = initialpose.position.y sigmaX = ratio # Test Value sigmaY = ratio # Test Value poseArray = PoseArray() poseArray.header.stamp = rospy.Time.now() #Generation of Poses for m in range(0, numParticles): xNewPose = -1 yNewPose = -1 xNewPose = meanX + (random.gauss(0,sigmaX)*noisePose) yNewPose = meanY + (random.gauss(0,sigmaY)*noisePose) #newPose Parameters newPose = Pose() newPose.position.x = xNewPose newPose.position.y = yNewPose newPose.orientation = None #Different orientations assigned for different tests/situations if test: newPose.orientation = initialpose.orientation elif rand: newPose.orientation = rotateQuaternion(initialpose.orientation, random.uniform(-math.pi, math.pi)) else: newPose.orientation = rotateQuaternion(initialpose.orientation, random.vonmisesvariate(0, 4)) poseArray.poses.append(newPose) return poseArray
def generate_random_map_pose(self):
pose = Pose()
pose.position.x = self.sensor_model.map_origin_x + random.uniform(
-(self.sensor_model.map_width / 2.0) *
self.sensor_model.map_resolution,
(self.sensor_model.map_width / 2.0) *
self.sensor_model.map_resolution)
pose.position.y = self.sensor_model.map_origin_y + random.uniform(
-(self.sensor_model.map_height / 2.0) *
self.sensor_model.map_resolution,
(self.sensor_model.map_height / 2.0) *
self.sensor_model.map_resolution)
thetad = random.uniform(-math.pi, math.pi)
pose.position.z = 0
pose.orientation = util.rotateQuaternion(Quaternion(w=1.0), thetad)
return pose
def smudge_amcl(self, resampledPoses): cont = True pArray = PoseArray() if len(resampledPoses): temp = [] val = Pose() count = 0 # TEST this value, rounding scalar scale = 0.66 while cont: temp = [] val = resampledPoses[0] count = 0 for i in range(0, len(resampledPoses)): if (resampledPoses[i] == val): count = count + 1 else: temp.append(resampledPoses[i]) resampledPoses = temp if (len(resampledPoses) == 0): cont = False # THIS NEEDS TESTS, look at scalar above if (count > 4) and len(resampledPoses) >= 50: #TEST #count = count - 2 count = int(count * scale) for i in range(0, count): if i > 0: newPose = Pose() newPose.position.x = random.gauss(val.position.x, 0.3) #TEST THIS newPose.position.y = random.gauss(val.position.y, 0.3) #TEST THIS newPose.orientation = rotateQuaternion(val.orientation, random.vonmisesvariate(0, 4)) #TEST THIS pArray.poses.append(newPose) else: pArray.poses.append(val) else: pArray.poses = [] return pArray
def initialise_particle_cloud(self, initialpose):
"""
Set particle cloud to initialpose plus noise
Called whenever an initialpose message is received (to change the
starting location of the robot), or a new occupancy_map is received.
self.particlecloud can be initialised here. Initial pose of the robot
is also set here.
:Args:
| initialpose: the initial pose estimate
:Return:
| (geometry_msgs.msg.PoseArray) poses of the particles
"""
poses = PoseArray() #initialising pose array
for num in range(0, self.NUMBER_PREDICTED_READINGS):
p = Pose() #initialising pose
xNoise = self.ODOM_TRANSLATION_NOISE * np.random.normal(
0, 0.1) #noise calculations
yNoise = self.ODOM_DRIFT_NOISE * np.random.normal(0, 0.1)
zNoise = 0
random_angular_noise = self.ODOM_ROTATION_NOISE * np.random.normal(
0, 0.1)
p.position.x = initialpose.pose.pose.position.x + xNoise #pose positions+noise
p.position.y = initialpose.pose.pose.position.y + yNoise
p.position.z = initialpose.pose.pose.position.z + zNoise
p.orientation = rotateQuaternion(initialpose.pose.pose.orientation,
random_angular_noise)
poses.poses.append(p)
#poses.header.seq left it blank
poses.header.stamp = rospy.Time.now(
) #defining the header of the pose array
poses.header.frame_id = "/map"
initialPose = Pose()
initialPose.position.x = initialpose.pose.pose.position.x
initialPose.position.y = initialpose.pose.pose.position.y
initialPose.position.z = initialpose.pose.pose.position.z
initialPose.orientation = initialpose.pose.pose.orientation
self.initial_pose = initialPose
self.particlecloud = poses
print("Initialising particle cloud to length ",
len(self.particlecloud.poses))
return poses
def initialise_particle_cloud(self, initialpose):
# Set particle cloud to initialpose plus noise
pose_arr = PoseArray()
init = initialpose.pose.pose
# using 200 particles
for _ in range(0,200):
pose = Pose()
# using gaussian distribution
pose.position.x = gauss(init.position.x,0.5)
pose.position.y = gauss(init.position.y,0.5)
rotation = gauss(0, 0.125)
pose.orientation = rotateQuaternion(init.orientation, rotation)
pose_arr.poses.append(pose)
return pose_arr
def initialise_particle_cloud(self, initialpose):
"""
Set particle cloud to initialpose plus noise
Called whenever an initialpose message is received (to change the
starting location of the robot), or a new occupancy_map is received.
self.particlecloud can be initialised here. Initial pose of the robot
is also set here.
:Args:
| initialpose: the initial pose estimate
:Return:
| (geometry_msgs.msg.PoseArray) poses of the particles
"""
'''
rospy.loginfo(type(initialpose))
'''
self.NUMBER_PREDICTED_READINGS = 200
rospy.loginfo(initialpose)
sd = np.std(initialpose.pose.covariance)
mean = 0
noise = 6
initialOrientation = initialpose.pose.pose.orientation
sdOri = np.pi / 4
# orientation, covariance, noise
poses = PoseArray()
for i in range(self.NUMBER_PREDICTED_READINGS):
nd = np.random.normal(mean, sd, 2)
ndOri = np.random.normal(mean, sdOri)
pose = Pose()
#yaw = getHeading(initialOrientation) * orientationNoise
pose.orientation = rotateQuaternion(initialOrientation,
ndOri) #getHeading(q)
pose.position.x = initialpose.pose.pose.position.x + nd[0] * noise
pose.position.y = initialpose.pose.pose.position.y + nd[1] * noise
pose.position.z = initialpose.pose.pose.position.z
poses.poses.append(pose)
rospy.loginfo("-----------------------poses----------------")
rospy.loginfo(poses)
rospy.loginfo(len(poses.poses))
rospy.loginfo("-----------------------endposes----------------")
return poses
def add_noise(std_pos, std_yaw):
poses = []
for p in self.particlecloud.poses: # Iterate over all poses
new_pose = Pose()
while not self.is_valid_position(
new_pose.position
): # Repeat until point is valid (unoccupied)
new_pose.position.x = gauss(
p.position.x, std_pos) # Add noise to x-coordinate
new_pose.position.y = gauss(
p.position.y, std_pos) # Add noise to y-coordinate
rotate_amounts = vonmises(0.0, 1 / std_yaw**2)
new_pose.orientation = rotateQuaternion(
p.orientation, rotate_amounts)
poses.append(new_pose)
self.particlecloud.poses = poses
def random_pose(self):
new_pose = Pose()
rand_angle = rand.uniform(0, 2 * math.pi)
new_pose.orientation = rotateQuaternion(q_orig=Quaternion(0, 0, 0, 1),
yaw=rand_angle)
while True:
x, y = self.map_coords_to_world(
map_x=rand.uniform(0, self.occupancy_map.info.width - 1),
map_y=rand.uniform(0, self.occupancy_map.info.height - 1))
new_pose.position.x = x
new_pose.position.y = y
if not self.map_cell_occupied(new_pose):
#rospy.loginfo("random x={}, y={}".format(new_pose.position.x, new_pose.position.y))
break
return new_pose
def next_waypoint(self):
"""
Determine the next waypoint to navigate to
:param: avail_space_model: The space model from which to select a waypoint
:return: Random point on the map
"""
new_pose = Pose()
self.clean_map()
(x, y) = self.least_space()
wx, wy = util.map_coords_to_world(x, y, self.map_model)
new_pose.position.x = wx
new_pose.position.y = wy
rand_a = rand.uniform(0, 2 * math.pi)
new_pose.orientation = util.rotateQuaternion(q_orig=Quaternion(
0, 0, 0, 1),
yaw=rand_a)
rospy.logwarn("Target pose created => x: {}, y: {}".format(x, y))
return new_pose
def rand_particles(self, N):
ret = PoseArray()
width = self.occupancy_map.info.width
height = self.occupancy_map.info.height
particles = 0
while particles < N:
generated_angle = random.vonmisesvariate(mu=0, kappa=0)
x = random.randint(0, width - 1) # maybe use gauss
y = random.randint(0, height - 1)
new_pose = Pose()
new_pose.position.x = x * 0.05
new_pose.position.y = y * 0.05
new_pose.orientation = rotateQuaternion(Quaternion(w=1.0),
generated_angle)
if self.occupancy_map.data[x + y * width] == 0:
ret.poses.append(new_pose)
particles += 1
return ret
def update_particle_cloud(self, scan):
# Update particlecloud, given map and laser scan
weighted_poses = []
weight_sum = 0
weight_max = 0
for particle in self.particlecloud.poses:
weight = self.sensor_model.get_weight(scan, particle)
weighted_poses.append([particle, weight])
weight_sum += weight
if(weight > weight_max):
weight_max = weight
#rospy.loginfo(weight_max)
#normalisation
# for counter, pose in enumerate(weighted_poses):
# weighted_poses[counter][1] = pose[1]/weight_sum
n_random = 0
if(weight_max < self.ADAPTATIVE_THRESHOLD):
n_random = int(self.ADAPTATIVE_RATIO * self.NUMBER_PARTICLES)
# elif(weight_max < 11):
# n_random = 50
# elif(weight_max < 15):
# n_random = 25
n_resampled = self.NUMBER_PARTICLES - n_random
# random.shuffle(weighted_poses)
resampled_poses = self.resample(weighted_poses, n_resampled)
resampled_cloud = PoseArray()
for particle in resampled_poses:
new_particle = deepcopy(particle[0])
# sample gaussian noise with sigma = noise parameters, and mean = location
new_particle.position.x = random.gauss(new_particle.position.x,self.ODOM_TRANSLATION_NOISE)
new_particle.position.y = random.gauss(new_particle.position.y,self.ODOM_DRIFT_NOISE)
# sample gaussian noise from the rotation parameter
# but does that make mathematical sense? What are the rotation noise units?
new_particle.orientation = rotateQuaternion(new_particle.orientation,(math.radians(random.gauss(0,self.ODOM_ROTATION_NOISE))))
resampled_cloud.poses.append(new_particle)
self.add_random_particle_distribution(1, n_random, resampled_cloud)
# self.add_random_particle_distribution(1, 20, resampled_cloud)
self.particlecloud = resampled_cloud
def uniform_initialise(self, initialpose, numParticles, pf): self.set_map_dim(pf) poseArray = PoseArray() poseArray.header.stamp = rospy.Time.now() listFreePoints = pf.listFreePoints for m in range(0, numParticles): randUninform = int(random.uniform(0,len(listFreePoints)-1)) coordinates = listFreePoints[randUninform] xNewPose = coordinates.x * pf.occupancy_map.info.resolution yNewPose = coordinates.y * pf.occupancy_map.info.resolution #newPose Parameters newPose = Pose() newPose.position.x = xNewPose newPose.position.y = yNewPose newPose.orientation = rotateQuaternion(initialpose.orientation, random.uniform(-math.pi, math.pi)) poseArray.poses.append(newPose) return poseArray
def avg_estimate(self, pf):
sumX = 0
sumY = 0
sumOrientation = 0
particles = pf.particlecloud
for i in range (0, len(particles.poses)):
sumX += particles.poses[i].position.x
sumY += particles.poses[i].position.y
heading = getHeading(particles.poses[i].orientation)
sumOrientation += getHeading(particles.poses[i].orientation)
pose = Pose()
pose.position.x = sumX/len(particles.poses)
pose.position.y = sumY/len(particles.poses)
orientation = pf.estimatedpose.pose.pose.orientation
pose.orientation = rotateQuaternion(orientation, sumOrientation/len(particles.poses) - getHeading(orientation))
rospy.loginfo("X: " + str(pose.position.x) +", Y: " + str(pose.position.y))
return pose
def generate_pose_inside_map(self):
"""
Generates a pose randmly inside the map
:return: (geometry_msgs.msg.Pose) A pose randomly placed inside the map
"""
pose = Pose()
# Gets a random pair of coordinates (x, y) inside the map and assigns them to the new pose
idx = random.randint(0, len(self.points_inside_map) - 1)
pose.position.x = self.points_inside_map[idx][0]
pose.position.y = self.points_inside_map[idx][1]
pose.position.z = 0
pose.orientation.w = 1
# Assigns a random orientation to the new pose by rotating a Quaternion
# by a random angle between 0 and 360 degrees
pose.orientation = rotateQuaternion(
pose.orientation,
math.radians(random.uniform(0, 360))
)
return pose
def initialise_particle_cloud(self, initialpose):
#Set particle cloud to initialpose plus noise
noise = 1
#create an array of Poses
posArray = PoseArray()
#iterate over the number of particles and append to PosArray
for x in range(0, self.NUMBER_PREDICTED_READINGS):
pose = Pose()
pose.position.x = random.gauss(initialpose.pose.pose.position.x, noise)
pose.position.y = random.gauss(initialpose.pose.pose.position.y, noise)
pose.position.z = 0
#TODO:reconfig the parameters
pose.orientation = rotateQuaternion(initialpose.pose.pose.orientation, math.radians(random.gauss(0,30)))
posArray.poses.extend([pose])
#print posArray
return posArray
def generate_random_pose(self):
""" Generates a random pose on the map.
Return:
(geometry_msgs.msg.Pose) the random pose on the map
"""
p = Pose() # Instantiate pose
while not self.is_valid_position(
p.position): # Repeat until unoccupied point is found
p.position.x = random(
) * self.occupancy_map.info.width # Sample x location on map
p.position.y = random(
) * self.occupancy_map.info.height # Sample y location on map
p.position.z = 0.0 # No elevation, z-coordinate is 0
p.orientation.w = 1.0
p.orientation.x = 0.0
p.orientation.y = 0.0
p.orientation.z = 0.0
p.orientation = rotateQuaternion(p.orientation, 2 * math.pi * random())
return p # Return pose # NotImplementedError("generate_random_pose not implemented!")
def equal_initialise(self, initialpose, numParticles, pf):
poseArray = PoseArray()
poseArray.header.stamp = rospy.Time.now()
listFreePoints = pf.listFreePoints
pixelGap = 18
for m in range(0, len(listFreePoints), pixelGap):
coordinates = listFreePoints[m]
if coordinates.y % pixelGap == 0.0:
xNewPose = coordinates.x * pf.occupancy_map.info.resolution
yNewPose = coordinates.y * pf.occupancy_map.info.resolution
#newPose Parameters
newPose = Pose()
newPose.position.x = xNewPose
newPose.position.y = yNewPose
newPose.orientation = rotateQuaternion(initialpose.orientation, random.uniform(-math.pi, math.pi))
poseArray.poses.append(newPose)
print("HI")
return poseArray
def initialise_particle_cloud(self, initialpose):
"""
Set particle cloud to initialpose plus noise
Called whenever an initialpose message is received (to change the
starting location of the robot), or a new occupancy_map is received.
self.particlecloud can be initialised here. Initial pose of the robot
is also set here.
:Args:
| initialpose: the initial pose estimate
:Return:
| (geometry_msgs.msg.self.particlecloud) poses of the particles
"""
#rospy.loginfo("initial pose %s"%initialpose)
X = initialpose.pose.pose.position.x
Y = initialpose.pose.pose.position.y
th = initialpose.pose.pose.orientation
rad = 1
#self.particlecloud=[]
particleArray = PoseArray()
particleArray.poses = [Odometry().pose.pose] * 250
#rospy.loginfo("particlearray pose %s"%len(particleArray.poses))
for i in range(0, 250):
particle = Pose()
th1 = random() * 360
th2 = random() * 360
radius = random() * rad
x = radius * math.sin(th1) + X
y = radius * math.cos(th1) + Y
particle.position.x = x
particle.position.y = y
particle.orientation = rotateQuaternion(th, th2)
particleArray.poses[i] = particle
#rospy.loginfo("random pose %s"%particle)
#rospy.loginfo("particlearray pose %s"%particleArray)
return particleArray
def generate_gaussian_pose(initial_pose):
p = Pose()
p.position.x = gauss(initial_pose.pose.pose.position.x,
self.POSITION_STANDARD_DEVIATION)
p.position.y = gauss(initial_pose.pose.pose.position.y,
self.POSITION_STANDARD_DEVIATION)
p.position.z = 0.0
# Convert initial orientation from quaternion into euler representation and get yaw angle
# initial_yaw = tf.transformations.euler_from_quaternion(orientation_to_vec(initial_pose.pose.pose.orientation))[2]
# rand_yaw = vonmises(initial_yaw, 1.0 / self.ORIENTATION_STANDARD_DEVIATION ** 2) # Generate random yaw angle
# vector = tf.transformations.quaternion_from_euler(0.0, 0.0, rand_yaw) # Convert yaw angle into quaternion representation
# p.orientation = vec_to_orientation(vector)
# heading = getHeading(initial_pose.pose.pose.orientation)
rotate_amounts = vonmises(
0.0, 1 / self.ORIENTATION_STANDARD_DEVIATION**2)
p.orientation = rotateQuaternion(
initial_pose.pose.pose.orientation, rotate_amounts)
return p
def gen_random_particles(self, number_of_particles):
particles = PoseArray()
map_width = self.occupancy_map.info.width
map_height = self.occupancy_map.info.height
accepted_particles = 0
while accepted_particles < number_of_particles:
x = random.randint(0, map_width - 1)
y = random.randint(0, map_height - 1)
theta = random.uniform(0, 2 * math.pi)
pose = Pose()
pose.position.x = x * 0.05
pose.position.y = y * 0.05
pose.orientation = rotateQuaternion(Quaternion(w=1.0), theta)
map_index = x + y * map_width
if self.occupancy_map.data[map_index] == 0:
particles.poses.append(pose)
accepted_particles += 1
return particles
def predict_from_odometry(self, odom):
"""
Adds the estimated motion from odometry readings to each of the
particles in particlecloud.
:Args:
| odom (nav_msgs.msg.Odometry): Recent Odometry data
"""
with self._update_lock:
t = time.time()
x = odom.pose.pose.position.x
y = odom.pose.pose.position.y
new_heading = getHeading(odom.pose.pose.orientation)
# ----- On our first run, the incoming translations may not be equal to
# ----- zero, so set them appropriately
if not self.odom_initialised:
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.odom_initialised = True
# ----- Find difference between current and previous translations
dif_x = x - self.prev_odom_x
dif_y = y - self.prev_odom_y
dif_heading = new_heading - self.prev_odom_heading
if dif_heading > math.pi:
dif_heading = (math.pi * 2) - dif_heading
if dif_heading < -math.pi:
dif_heading = (math.pi * 2) + dif_heading
# ----- Update previous pure odometry location (i.e. excluding noise)
# ----- with the new translation
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.last_odom_pose = odom
# ----- Find robot's linear forward/backward motion, given the dif_x and
# ----- dif_y changes and its orientation
distance_travelled = math.sqrt(dif_x * dif_x + dif_y * dif_y)
direction_travelled = math.atan2(dif_y, dif_x)
temp = abs(new_heading - direction_travelled)
if temp < -PI_OVER_TWO or temp > PI_OVER_TWO:
# ----- We are going backwards
distance_travelled = distance_travelled * -1
# ----- Update each particle with change in position (plus noise)
for p in self.particlecloud.poses:
rnd = random.normalvariate(0, 1)
# ----- Rotate particle according to odometry rotation, plus noise
p.orientation = (rotateQuaternion(
p.orientation, dif_heading +
rnd * dif_heading * self.ODOM_ROTATION_NOISE))
# ----- Get particle's new orientation
theta = getHeading(p.orientation)
# ----- Find translation in the direction of particle's orientation
travel_x = distance_travelled * math.cos(theta)
travel_y = distance_travelled * math.sin(theta)
p.position.x = (p.position.x + travel_x +
(rnd * travel_x * self.ODOM_TRANSLATION_NOISE))
p.position.y = (p.position.y + travel_y +
(rnd * travel_y * self.ODOM_DRIFT_NOISE))
return time.time() - t
def __init__(self, _num, _mapTopic, _algorithmName):
# Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self.weights = []
self.maxWeight = 0
self.totalWeight = 0
self.num = _num
self.clusterTask = ClusterTask()
self.floorName = _mapTopic
self.exploded = False
# Initialise objects
self.cloud = UpdateParticleCloud()
self.estimate = EstimatePose()
self.init = InitialiseCloud()
self._weighted_particle_publisher = rospy.Publisher("/weightedParticles", WeightedParticles)
# Parameters
self.ODOM_ROTATION_NOISE = 0 # Odometry model rotation noise
self.ODOM_TRANSLATION_NOISE = 0 # Odometry x axis (forward) noise
self.ODOM_DRIFT_NOISE = 0 # Odometry y axis (side-side) noise
self.NUMBER_PREDICTED_READINGS = 20
# Set 'previous' translation to origin
# All Transforms are given relative to 0,0,0, not in absolute coords.
self.prev_odom_x = 0.0 # Previous odometry translation from origin
self.prev_odom_y = 0.0 # Previous odometry translation from origin
self.prev_odom_heading = 0.0 # Previous heading from odometry data
self.last_odom_pose = None
# Request robot's initial odometry values to be recorded in prev_odom
self.odom_initialised = False
self.sensor_model_initialised = False
# Set default initial pose to initial position and orientation.
self.estimatedpose.pose.pose.position.x = self.INIT_X
self.estimatedpose.pose.pose.position.y = self.INIT_Y
self.estimatedpose.pose.pose.position.z = self.INIT_Z
self.estimatedpose.pose.pose.orientation = rotateQuaternion(Quaternion(w=1.0),self.INIT_HEADING)
# NOTE: Currently not making use of covariance matrix
self.estimatedpose.header.frame_id = "/map"
self.particlecloud.header.frame_id = "/map"
# Sensor model
self.sensor_model = sensor_model.SensorModel()
print(_algorithmName)
# What algorithm do we use?
if _algorithmName == "async":
self.asynchronous = True
elif _algorithmName == "sync":
self.asynchronous = False
else:
print("Invalid argument 3: expected \"sync\" or \"async\"")
sys.exit(1)
# Free Point where Robot can be
self.listFreePoints = []
def predict_from_odometry(self, odom):
"""
Adds the estimated motion from odometry readings to each of the
particles in particlecloud.
:Args:
| odom (nav_msgs.msg.Odometry): Recent Odometry data
"""
with self._update_lock:
t = time.time()
x = odom.pose.pose.position.x
y = odom.pose.pose.position.y
new_heading = getHeading(odom.pose.pose.orientation)
# On our first run, the incoming translations may not be equal to
# zero, so set them appropriately
if not self.odom_initialised:
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.odom_initialised = True
# Find difference between current and previous translations
dif_x = x - self.prev_odom_x
dif_y = y - self.prev_odom_y
dif_heading = new_heading - self.prev_odom_heading
if dif_heading > math.pi:
dif_heading = (math.pi * 2) - dif_heading
if dif_heading < -math.pi:
dif_heading = (math.pi * 2) + dif_heading
# Update previous pure odometry location (i.e. excluding noise)
# with the new translation
self.prev_odom_x = x
self.prev_odom_y = y
self.prev_odom_heading = new_heading
self.last_odom_pose = odom
# Find robot's linear forward/backward motion, given the dif_x and
# dif_y changes and its orientation
distance_travelled = math.sqrt(dif_x*dif_x + dif_y*dif_y)
direction_travelled = math.atan2(dif_y, dif_x)
temp = abs(new_heading - direction_travelled)
if temp < -PI_OVER_TWO or temp > PI_OVER_TWO:
# We are going backwards
distance_travelled = distance_travelled * -1
# Update each particle with change in position (plus noise)
for p in self.particlecloud.poses:
rnd = random.normalvariate(0, 1)
# Rotate particle according to odometry rotation, plus noise
p.orientation = (rotateQuaternion(p.orientation,
dif_heading + rnd * dif_heading * self.ODOM_ROTATION_NOISE))
# Get particle's new orientation
theta = getHeading(p.orientation)
# Find translation in the direction of particle's orientation
travel_x = distance_travelled * math.cos(theta)
travel_y = distance_travelled * math.sin(theta)
p.position.x = (p.position.x + travel_x +
(rnd * travel_x * self.ODOM_TRANSLATION_NOISE))
p.position.y = (p.position.y + travel_y +
(rnd * travel_y * self.ODOM_DRIFT_NOISE))
return time.time() - t
def update_kld_amcl(self, scan, pf):
self.weight_particles(scan, pf)
numParticles = len(pf.particlecloud.poses)
rospy.loginfo(maxWeight)
#if the maximum weighted particle has a weight below 10 reinitialise the particles
if maxWeight < 7:
pf.particlecloud = pf.reinitialise_cloud(pf.estimatedpose.pose.pose, 3.0, True)
self.weight_particles(scan, pf)
numParticles = len(pf.particlecloud.poses)
resampledPoses = []
index = 0
notAccepted = True
#Initialize KLD Sampling
ztable = [i/maxWeight for i in particleWeights]
support_samples=0
num_samples=0
quantile=0.5
kld_error = 0.1
bin_size = 0.1
min_samples=10
seed=-1
kld_samples = 0
if (min_samples < self.ABSOLUTE_MIN):
kld_samples=self.ABSOLUTE_MIN
else:
kld_samples=min_samples
bins = []
confidence=quantile-0.5; # ztable is from right side of mean
confidence=min(0.49998,max(0,confidence))
max_error = kld_error;
bin_size = bin_size; # list of lists
zvalue=4.1;
for i in range(len(ztable)):
if(ztable[i] >= confidence):
zvalue=i/100.0
break
#Resample the poses
samples = []
while (num_samples < min_samples and num_samples < 250) :
#Get Sample
notAccepted = True
while (notAccepted):
index = random.randint(0,numParticles-1)
posX = pf.particlecloud.poses[index].position.x
posY = pf.particlecloud.poses[index].position.y
if (random.uniform(0,1) < particleWeights[index]/totalWeight):
notAccepted = False
curr_sample = pf.particlecloud.poses[index]
samples.append(curr_sample)
num_samples = num_samples+1
curr_bin = curr_sample
if len(bins)==0 or curr_bin not in bins:
bins.append(curr_bin);
support_samples = support_samples+1
if support_samples>=2:
k = support_samples-1
k=math.ceil(k/(2*max_error)*pow(1-2/(9.0*k)+math.sqrt(2/(9.0*k))*zvalue,3))
if k>kld_samples:
kld_samples = k
min_samples = kld_samples
resampledPoses = samples
rospy.loginfo("Size Samples = %s"%len(samples))
cont = True
pArray = PoseArray()
temp = []
val = Pose()
count = 0
# TEST this value, rounding scalar
scale = 0.66
while cont:
temp = []
val = resampledPoses[0]
count = 0
for i in range(0, len(resampledPoses)):
if (resampledPoses[i] == val):
count = count + 1
else:
temp.append(resampledPoses[i])
resampledPoses = temp
if (len(resampledPoses) == 0):
cont = False
# THIS NEEDS TESTS, look at scalar above
if (count > 4) and len(resampledPoses) >= 50: #TEST
#count = count - 2
count = int(count * scale)
for i in range(0, count):
if i > 0:
newPose = Pose()
newPose.position.x = random.gauss(val.position.x, 0.3) #TEST THIS
newPose.position.y = random.gauss(val.position.y, 0.3) #TEST THIS
newPose.orientation = rotateQuaternion(val.orientation, random.vonmisesvariate(0, 4)) #TEST THIS
pArray.poses.append(newPose)
else:
pArray.poses.append(val)
return pArray
def update_particle_cloud(self, scan):
##---------------------------##
##---------PARAMETERs--------##
##---------------------------##
#list of poses
sum_weights = 0
sum_count = 0
cumulate_weight = []
separate_weight = []
cloud_pose = self.particlecloud.poses
##---------------------------##
##-----------WEIGHT----------##
##---------------------------##
##remove the invalid value.
scan.ranges = ma.masked_invalid(scan.ranges).filled(scan.range_max)
##record the weight for each pose, in case to use it again
pairs_partweight = []
#time1 = datetime.datetime.now()
##weight need scan_data and current_pose
for i in cloud_pose:
particle_weight = self.sensor_model.get_weight(scan, i)
sum_weights += particle_weight
pairs_partweight.append([i, particle_weight])
#time2 = datetime.datetime.now()
##calculate the time consume
#print (time2 - time1 ).microseconds
for pair in pairs_partweight:
particle_weight = pair[1]
weight_over_sum = particle_weight / sum_weights
sum_count += weight_over_sum
cumulate_weight.extend([sum_count])
#print cumulate_weight
##-----------------------##
##---------UPDATE--------##
##-----------------------##
updated_particlecloud = PoseArray()
for particle in cloud_pose:
count = 0
rand = random.uniform(0, 1)
for i in cumulate_weight:
##TODO:the repeat pose doesn't matters
if rand <= i:
updated_particlecloud.poses.extend([cloud_pose[count]])
#print count
break
count = count + 1
##-----------------------##
##---------NOISE---------##
##-----------------------##
updated_with_noise_cloud = PoseArray()
for i in updated_particlecloud.poses:
noise_pose = Pose()
noise_pose.position.x = random.gauss(
i.position.x, (i.position.x * self.ODOM_DRIFT_NOISE))
noise_pose.position.y = random.gauss(
i.position.y, (i.position.y * self.ODOM_TRANSLATION_NOISE))
noise_pose.orientation = rotateQuaternion(
i.orientation,
math.radians(
random.uniform(-self.ODOM_ROTATION_NOISE,
self.ODOM_ROTATION_NOISE)))
updated_with_noise_cloud.poses.extend([noise_pose])
##-----------------------##
##---------OUTPUT--------##
##-----------------------##
self.particlecloud = updated_with_noise_cloud
def update_particle_cloud(self, scan):
##---------------------------##
##---------PARAMETERs--------##
##---------------------------##
#list of poses
sum_weights = 0
sum_count = 0
cumulate_weight= []
separate_weight= []
cloud_pose = self.particlecloud.poses
##---------------------------##
##-----------WEIGHT----------##
##---------------------------##
##remove the invalid value.
scan.ranges=ma.masked_invalid(scan.ranges).filled(scan.range_max)
##record the weight for each pose, in case to use it again
pairs_partweight=[]
#time1 = datetime.datetime.now()
##weight need scan_data and current_pose
for i in cloud_pose:
particle_weight = self.sensor_model.get_weight(scan, i)
sum_weights+= particle_weight
pairs_partweight.append([i,particle_weight])
#time2 = datetime.datetime.now()
##calculate the time consume
#print (time2 - time1 ).microseconds
for pair in pairs_partweight:
particle_weight = pair[1]
weight_over_sum = particle_weight/sum_weights
sum_count+= weight_over_sum
cumulate_weight.extend([sum_count])
#print cumulate_weight
##-----------------------##
##---------UPDATE--------##
##-----------------------##
updated_particlecloud = PoseArray()
for particle in cloud_pose:
count = 0
rand = random.uniform(0,1)
for i in cumulate_weight:
##TODO:the repeat pose doesn't matters
if rand <= i:
updated_particlecloud.poses.extend([cloud_pose[count]])
#print count
break
count=count+1
##-----------------------##
##---------NOISE---------##
##-----------------------##
updated_with_noise_cloud = PoseArray()
for i in updated_particlecloud.poses:
noise_pose = Pose()
noise_pose.position.x = random.gauss(i.position.x,(i.position.x * self.ODOM_DRIFT_NOISE))
noise_pose.position.y = random.gauss(i.position.y,(i.position.y * self.ODOM_TRANSLATION_NOISE))
noise_pose.orientation = rotateQuaternion(i.orientation, math.radians(random.uniform(-self.ODOM_ROTATION_NOISE,self.ODOM_ROTATION_NOISE)))
updated_with_noise_cloud.poses.extend([noise_pose])
##-----------------------##
##---------OUTPUT--------##
##-----------------------##
self.particlecloud = updated_with_noise_cloud
def resample_kld(self, particleWT, tWeight):
#particleWT[i][0] is the map_topic associated with the particle
#particleWT[i][1] is the particle
#particleWT[i][2] is the weight associated with the particle
#self.mapInfo[i][0] is the map_topic associated with the listFreePoints
#self.mapInfo[i][1] is the listFreePoints
#self.mapInfo[i][2] is the resolution of the map associated with listFreePoints
numParticles = len(particleWT)
index = 0
notAccepted = True
listFreePoints = []
resampledPoses = []
numberSamplesMap = {}
#Initialize KLD Sampling
zvalue = 2.1
binsDict = {}
binsSize = 0
k = 0 #Number of Bins not empty
epsilon = 0.05
Mmin = 50
#Mmin = int(0.9 * len(particleWT))
M = 0
Mx = 0
#Initialising the bins
for i in range(0, len(self.mapInfo)):
listFreePoints = self.mapInfo[i][1]
for j in range(0,len(listFreePoints), 5):
currentCell = listFreePoints[j]
topic = self.mapInfo[i][0]
cellX = currentCell.x
cellY = currentCell.y
valueBin = False
binsDict[(topic, cellX, cellY)] = False
binsSize = binsSize + 1
#Initialising the numberSamplesMap
for n in range(0, len(self.mapInfo)):
numberSamplesMap[self.mapInfo[n][0]] = 0
"""#Resample the poses
for m in range(0, len(self.mapInfo)):
M = 0
Mx = 0
mapName = self.mapInfo[m][0]
while (M < Mx or M < Mmin) :
#Get Sample
notAccepted = True
while (notAccepted):
index = random.randint(0,numParticles-1)
particle = particleWT[index]
if (random.uniform(0,1) < particle[2]/tWeight) and (particle[0] == mapName):
notAccepted = False
curr_sample = (particle[0], particle[1])
resampledPoses.append(curr_sample)
M = M + 1
#Convert Coodinates of the Pose to know if the bin is Empty or not
for i in range(0,len(self.mapInfo)) :
if (particle[0] == self.mapInfo[i][0]):
mapResolution = self.mapInfo[i][2]
xBin = int(curr_sample[1].position.x / mapResolution)
yBin = int(curr_sample[1].position.y / mapResolution)
for l in range(0, binsSize):
if ((curr_sample[0], xBin, yBin) in binsDict):
#rospy.loginfo("Hello")
if (binsDict[(curr_sample[0], xBin, yBin)] == False):
binsDict[(curr_sample[0], xBin, yBin)] = True
k = k + 1
rospy.loginfo("k is %s"%k)
if (k > 1):
Mx = ((k-1)/(2*epsilon)) * math.pow(1 - (2/(9*(k-1))) + (math.sqrt(2/(9*(k-1)))*zvalue),3)
break
rospy.loginfo("Mx is %s"%Mx)
"""
while (M < Mx or M < Mmin) :
#Get Sample
notAccepted = True
while (notAccepted):
index = random.randint(0,numParticles-1)
particle = particleWT[index]
if (random.uniform(0,1) < particle[2]/tWeight):
notAccepted = False
curr_sample = (particle[0], particle[1])
resampledPoses.append(curr_sample)
M = M + 1
#Count Number Samples
numberSamplesMap[curr_sample[0]] = numberSamplesMap[curr_sample[0]] + 1
#Convert Coodinates of the Pose to know if the bin is Empty or not
xBin = -1
yBin = -1
for i in range(0,len(self.mapInfo)) :
if (particle[0] == self.mapInfo[i][0]):
mapResolution = self.mapInfo[i][2]
xBin = int(curr_sample[1].position.x / mapResolution)
yBin = int(curr_sample[1].position.y / mapResolution)
break
for l in range(0, binsSize):
if ((curr_sample[0], xBin, yBin) in binsDict):
#rospy.loginfo("Hello")
if (binsDict[(curr_sample[0], xBin, yBin)] == False):
binsDict[(curr_sample[0], xBin, yBin)] = True
k = k + 1
if (k > 1):
Mx = ((k-1)/(2*epsilon)) * math.pow(1 - (2/(9*(k-1))) + (math.sqrt(2/(9*(k-1)))*zvalue),3)
break
#Add particles if one map have no particles
nameMapAdd = None
mapResolution = 0
minNumberParticles = -1 #Must be different of 0
for key in numberSamplesMap:
rospy.loginfo("Map = %s"%key)
rospy.loginfo("Value = %s"%numberSamplesMap[key])
value = numberSamplesMap[key]
if value == 0 :
nameMapAdd = key
if (value != 0) and ((value < minNumberParticles) or minNumberParticles == -1):
minNumberParticles = value
if nameMapAdd != None:
rospy.loginfo("Map to re-Initialize = %s"%nameMapAdd)
#Finding Correct listFreePoints
listFreePoints = []
rospy.loginfo("minNumberParticles = %s"%minNumberParticles)
for p in range(0, len(self.mapInfo)):
if self.mapInfo[p][0] == nameMapAdd:
listFreePoints = self.mapInfo[p][1]
mapResolution = self.mapInfo[p][2]
#Generate Particles
for m in range(0, minNumberParticles):
rospy.loginfo("ListFree = %s"%len(listFreePoints))
randUninform = int(random.uniform(0,len(listFreePoints)-1))
coordinates = listFreePoints[randUninform]
xNewPose = coordinates.x * mapResolution
yNewPose = coordinates.y * mapResolution
#newPose Parameters
newPose = Pose()
newPose.position.x = xNewPose
newPose.position.y = yNewPose
newPose.orientation = rotateQuaternion(self.pose.orientation, random.uniform(-math.pi, math.pi))
curr_sample = (nameMapAdd, newPose)
resampledPoses.append(curr_sample)
rospy.loginfo("number of particles resampled %s"%len(resampledPoses))
return resampledPoses
