def _build_map_with_rangeCoordMat(self, y_rangeCoordMat, x_rangeCoordMat):
# Round y and x coord into int
y_rangeCoordMat = (np.round(y_rangeCoordMat)).astype(np.int)
x_rangeCoordMat = (np.round(x_rangeCoordMat)).astype(np.int)
""" Check for index of y_mat and x_mat that are within the world """
inBound_ind = util.within_bound(
np.array([y_rangeCoordMat, x_rangeCoordMat]), self.world.shape)
""" delete coordinate that are not within the bound """
outside_ind = np.argmax(~inBound_ind, axis=1)
ok_ind = np.where(outside_ind == 0)[0]
need_amend_ind = np.where(outside_ind != 0)[0]
outside_ind = np.delete(outside_ind, ok_ind)
inside_ind = np.copy(outside_ind)
inside_ind[inside_ind != 0] -= 1
bound_ele_x = x_rangeCoordMat[need_amend_ind, inside_ind]
bound_ele_y = y_rangeCoordMat[need_amend_ind, inside_ind]
count = 0
for i in need_amend_ind:
x_rangeCoordMat[i, ~inBound_ind[i, :]] = bound_ele_x[count]
y_rangeCoordMat[i, ~inBound_ind[i, :]] = bound_ele_y[count]
count += 1
""" find obstacle along the laser range """
obstacle_ind = np.argmax(
self.world[y_rangeCoordMat,
x_rangeCoordMat] == self.map_color['obstacle'],
axis=1)
obstacle_ind[obstacle_ind == 0] = x_rangeCoordMat.shape[1]
""" generate a matrix of [[1,2,3,...],[1,2,3...],[1,2,3,...],...] for comparing with the obstacle coord """
bx = np.arange(x_rangeCoordMat.shape[1]).reshape(
1, x_rangeCoordMat.shape[1])
by = np.ones((x_rangeCoordMat.shape[0], 1))
b = np.matmul(by, bx)
""" get the coord that the robot can percieve (ignore pixel beyond obstacle) """
b = b <= obstacle_ind.reshape(obstacle_ind.shape[0], 1)
y_coord = y_rangeCoordMat[b]
x_coord = x_rangeCoordMat[b]
""" no slam error """
# self.slamMap[y_coord, x_coord] = self.world[y_coord, x_coord]
""" laser noise """
# [y_noise_ind,x_noise_ind, y_coord,x_coord]= self._laser_noise(y_rangeCoordMat,x_rangeCoordMat,y_coord,x_coord,b)
# self.slamMap[y_noise_ind,x_noise_ind]= self.world[y_coord,x_coord]
#
""" slam matching error """
# [y_err_ind,x_err_ind, y_coord,x_coord] = self._slam_error(y_coord,x_coord)
# self.slamMap[y_err_ind,x_err_ind]= self.world[y_coord,x_coord]
""" laser noise + slam matching error """
[y_all_noise, x_all_noise, y_coord,
x_coord] = self._laser_slam_error(y_rangeCoordMat, x_rangeCoordMat,
y_coord, x_coord, b)
self.slamMap[y_all_noise, x_all_noise] = self.world[y_coord, x_coord]
""" dilate/close to fill the holes """
# self.dslamMap= cv2.morphologyEx(self.slamMap,cv2.MORPH_CLOSE,np.ones((3,3)))
self.dslamMap = cv2.dilate(self.slamMap, np.ones((3, 3)), iterations=1)
return self.slamMap
def robotCrashed(self, pose):
if ~util.within_bound(pose, self.world.shape, self.robotRadius):
return True
py = np.round(pose[0]).astype(int)
px = np.round(pose[1]).astype(int)
r = self.robotRadius
# make a circle patch around robot location and check if there is obstacle pixel inside the circle
robotPatch, _ = util.make_circle(r, 1)
worldPatch = self.world[py - r:py + r + 1, px - r:px + r + 1]
worldPatch = worldPatch * robotPatch
return np.sum(worldPatch == self.map_color["obstacle"]) != 0
def _laser_slam_error(self, y_rangeCoordMat, x_rangeCoordMat, y_coord,
x_coord, b):
[y_noise_rangeCoordMat, x_noise_rangeCoordMat, y_coord,
x_coord] = self._laser_noise(y_rangeCoordMat, x_rangeCoordMat,
y_coord, x_coord, b)
[y_err_coord, x_err_coord, y_noise_ind,
x_noise_ind] = self._slam_error(y_noise_rangeCoordMat,
x_noise_rangeCoordMat)
inBound_ind = util.within_bound(np.array([y_err_coord, x_err_coord]),
self.world.shape)
y_coord = y_coord[inBound_ind]
x_coord = x_coord[inBound_ind]
return y_err_coord, x_err_coord, y_coord, x_coord
def _slam_error(self, y_coord, x_coord):
err_y = util.gauss_noise() * self.slamErr_linear
err_x = util.gauss_noise() * self.slamErr_linear
err_theta = util.gauss_noise() * self.slamErr_angular
""" rotate y_coord & x_coord by err_theta """
(y_coord_err, x_coord_err) = util.transform_coord(
y_coord, x_coord, self.robotPose,
np.array([err_y, err_x, err_theta]))
""" check for index where the coord are within bound """
inBound_ind = util.within_bound(np.array([y_coord_err, x_coord_err]),
self.world.shape)
x_err_coord = x_coord_err[inBound_ind]
y_err_coord = y_coord_err[inBound_ind]
x_coord = x_coord.reshape(x_coord.shape[0], 1)[inBound_ind]
y_coord = y_coord.reshape(y_coord.shape[0], 1)[inBound_ind]
return y_err_coord, x_err_coord, y_coord, x_coord
def _laser_noise(self, y_rangeCoordMat, x_rangeCoordMat, y_coord, x_coord,
b):
""" add laser noise, y_&x_coord are coord before obstacle | y_&x_rangeCoordMat are coord that the laser range covers
b is the vector that represent the index of rangeCoordMat where the coord are before obstacle """
noise_vector = np.random.normal(loc=0,
scale=self.laser_noiseSigma,
size=(x_rangeCoordMat.shape[0], 1))
noise_vector = np.round(noise_vector).astype(np.int64)
""" add noise to rangeCoordMat """
y_rangeCoordMat += noise_vector
x_rangeCoordMat += noise_vector
y_noise_coord = y_rangeCoordMat[b]
x_noise_coord = x_rangeCoordMat[b]
""" check for index of the coord that are within bound of world """
inBound_ind = util.within_bound(
np.array([y_noise_coord, x_noise_coord]), self.world.shape)
""" get the coord that are within bound """
x_noise_coord = x_noise_coord[inBound_ind]
y_noise_coord = y_noise_coord[inBound_ind]
x_coord = x_coord[inBound_ind]
y_coord = y_coord[inBound_ind]
return y_noise_coord, x_noise_coord, y_coord, x_coord