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