def _increase_grid(self, out_of_bounds_pos):
"""
Extends the grid so the row,col coordinates in out_of_bounds_pos
fall within the grid.
"""
# get index of block that needs to be added or blocks to keep rectangular shape
sign_row = math.copysign(1, out_of_bounds_pos[0])
sign_col = math.copysign(1, out_of_bounds_pos[1])
new_pos = (
int(sign_row * CELLS_PER_BLOCK * math.ceil(abs(sign_row + out_of_bounds_pos[0]) / CELLS_PER_BLOCK)),
int(sign_col * CELLS_PER_BLOCK * math.ceil(abs(sign_col + out_of_bounds_pos[1]) / CELLS_PER_BLOCK)),
)
new_pos = tadd(self.minrange, new_pos)
current_size = self.grid.shape
new_minrange = tmin(self.minrange, new_pos)
new_maxrange = tmax(self.maxrange, new_pos)
new_size = tsub(new_maxrange, new_minrange)
offset = tsub(self.minrange, new_minrange)
offset_end = tadd(offset, current_size)
grid = np.zeros(shape=new_size)
grid[offset[0] : offset_end[0], offset[1] : offset_end[1]] = self.grid
self.grid = grid
self.minrange = new_minrange
self.maxrange = new_maxrange
def calc_weight(measurements, pose, map_):
probability = 0
# return probability
for sensor_angle, measured_dist in _measurement_per_angle(measurements):
# TODO: Pose of sensor is simplified to pose of robot
sensor_pose = Pose(pose.x, pose.y, pose.theta + sensor_angle)
# TODO: what if no expected distance is known? Fixed -> weight
"""likelihoods fields range finder, p172"""
if measured_dist is not None and measured_dist < MAX_RANGE: # discard max range readings
# coordinates of endpoint measured distance
x_co = sensor_pose.x + measured_dist * math.cos(sensor_pose.theta)
y_co = sensor_pose.y + measured_dist * math.sin(sensor_pose.theta)
nearest_object = find_nearest_neighbor(map_, (x_co, y_co))
# when sensor measurement falls into unknown category, prob is assumed constant p173
if nearest_object == "unknown":
# cell outside map
# cell prob = 0.5
# ==> measurement in unknown region
probability += 0.2
None
# find nearest neighbour that is occupied
elif nearest_object is not None:
# calculate Euclidean distance between measured coord and closest occupied coord
sub = tsub((x_co, y_co), nearest_object)
distance = math.hypot(sub[0], sub[1])
# prob = _prob_of_distances(distance, 0.0)
if distance < 5:
# distance to closest object under threshold
probability += 1
else:
# distance to closest object to far away
probability += 0
elif nearest_object is None:
# no object with prob > 0.9 around the cone top
probability += 0
else:
# sensor got out of range value (short reading or free region)
probability += 1
# calculate the region around the pose, if region is free this pose is more possible
grid_index = map_.cartesian2grid(pose.x, pose.y)
weight = 0
# look in a range around the sampled pose
RANGE = 4
for i in range(-RANGE, RANGE + 1, 1):
for j in range(-RANGE, RANGE + 1, 1):
pos = (grid_index[0] + i, grid_index[1] + j)
weight += calc_weight_pose(map_, pos)
# weight of the sensor measurement
# weight of the map correspondence
MAP_POSE_FACTOR = 1
MEASUREMENT_FACTOR = 30
# do something with weight
part1 = probability*MEASUREMENT_FACTOR
part2 = weight*MAP_POSE_FACTOR
print('part1 : {0} part2: {1}'.format(part1, part2))
return part1 + part2
def calc_weight(measurements, pose, map_):
probability = 0
# return probability
for sensor_angle, measured_dist in _measurement_per_angle(measurements):
# TODO: Pose of sensor is simplified to pose of robot
sensor_pose = Pose(pose.x, pose.y, pose.theta + sensor_angle)
# TODO: what if no expected distance is known? Fixed -> weight
"""likelihoods fields range finder, p172"""
if measured_dist is not None and measured_dist < MAX_RANGE: # discard max range readings
# coordinates of endpoint measured distance
x_co = sensor_pose.x + measured_dist * math.cos(sensor_pose.theta)
y_co = sensor_pose.y + measured_dist * math.sin(sensor_pose.theta)
nearest_object = find_nearest_neighbor(map_, (x_co, y_co))
# when sensor measurement falls into unknown category, prob is assumed constant p173
if nearest_object == "unknown":
# cell outside map
# cell prob = 0.5
# ==> measurement in unknown region
probability += 0.2
None
# find nearest neighbour that is occupied
elif nearest_object is not None:
# calculate Euclidean distance between measured coord and closest occupied coord
sub = tsub((x_co, y_co), nearest_object)
distance = math.hypot(sub[0], sub[1])
# prob = _prob_of_distances(distance, 0.0)
if distance < 5:
# distance to closest object under threshold
probability += 1
else:
# distance to closest object to far away
probability += 0
elif nearest_object is None:
# no object with prob > 0.9 around the cone top
probability += 0
else:
# sensor got out of range value (short reading or free region)
probability += 1
# calculate the region around the pose, if region is free this pose is more possible
grid_index = map_.cartesian2grid(pose.x, pose.y)
weight = 0
# look in a range around the sampled pose
RANGE = 4
for i in range(-RANGE, RANGE + 1, 1):
for j in range(-RANGE, RANGE + 1, 1):
pos = (grid_index[0] + i, grid_index[1] + j)
weight += calc_weight_pose(map_, pos)
# weight of the sensor measurement
# weight of the map correspondence
MAP_POSE_FACTOR = 1
MEASUREMENT_FACTOR = 30
# do something with weight
part1 = probability * MEASUREMENT_FACTOR
part2 = weight * MAP_POSE_FACTOR
print('part1 : {0} part2: {1}'.format(part1, part2))
return part1 + part2