def make_wall(self): n_ranges = 2 theta = self.start_orientation y = self.start_loc[0] - 10 * np.sin(theta) x = self.start_loc[1] - 10 * np.cos(theta) max_scan_angle = 90 * (np.pi / 180) testMap = range_libc.PyOMap(self.map_loc, 1) bl = range_libc.PyBresenhamsLine(testMap, 500) queries = np.zeros((n_ranges, 3), dtype=np.float32) ranges = np.zeros(n_ranges, dtype=np.float32) queries[:, 0] = x queries[:, 1] = y queries[:, 2] = theta + np.linspace(-max_scan_angle, max_scan_angle, n_ranges) points = bl.calc_range_many(queries, ranges) bl.saveTrace("/home/racecar/racecar-ws/src/lab6/maps/test.png") # print imread("./test.png")[500][600] map_img = imread("/home/racecar/racecar-ws/src/lab6/maps/test.png") cells_to_remove = set() for i in range(len(map_img[0])): for j in range(len(map_img)): if map_img[i][j][2] == 200: self.open_cells.discard((j, i)) # print (j,i) self.open_cells_tuple = tuple(self.open_cells) self.start_flag = True print "wall made; set goal!"
def put_cylinder_obstacle(self, x, y, r, check_collision=False, collision_inflation_radius=0.0): xys = [] xys_collision = [] for i in range(360): s = math.cos(np.deg2rad(i)) c = math.sin(np.deg2rad(i)) xys.append((x + r * c, y + r * s)) xys_collision.append((x + (r + collision_inflation_radius) * c, y + (r + collision_inflation_radius) * s)) xys = np.array(xys) xys = self.grid_coord_batch(xys, int(1.0 / self.resolution)) if check_collision: xys_collision = np.array(xys_collision) xys_collision = self.grid_coord_batch(xys_collision, int(1.0 / self.resolution)) if np.any(self.occupancy_grid[xys_collision[:, 1], xys_collision[:, 0]] == 0): return False # if np.sum(self.occupancy_grid[yy1:yy2, xx1:xx2]) > 0: # return False self.occupancy_grid[xys[:, 1], xys[:, 0]] = 0.0 self.square_omap[xys[:, 0], xys[:, 1]] = 1 self.omap = range_libc.PyOMap(self.square_omap) self.range_scanner = range_libc.PyBresenhamsLine(self.omap, 1000) return True
def _rasterize(self, resolution): import cv2 division = int(1.0 / resolution) x1, x2, y1, y2 = self.map_bbox origin = (x1, y1) width = int((x2 - x1) / resolution) height = int((y2 - y1) / resolution) def grid_coord(x, y, n_division): return int((x - origin[0]) * n_division + 0.5), int((y - origin[1]) * n_division + 0.5) canvas = np.zeros((height, width), np.uint8) for i in range(len(self.lines)): x1, y1, x2, y2 = self.lines[i] cv2.line(canvas, grid_coord(x1, y1, division), grid_coord(x2, y2, division), 255, 2) # print canvas.shape # cv2.imshow('canvas', canvas) # cv2.imwrite('canvas.jpg', canvas) # cv2.waitKey(0) # exit(0) self.rasterized = np.zeros((max(height, width), max(height, width)), np.uint8) self.rasterized[:width, :height] = np.transpose(canvas, (1, 0)) self.omap = range_libc.PyOMap(self.rasterized) self.range_scanner = range_libc.PyBresenhamsLine( self.omap, int(self.range_max * division))
def update_range_method_map(self): self.map_msg.data = self.dynamic_map.astype(int).flatten().tolist() print "UPDATING MAP" self.map_debug_pub.publish(self.map_msg) oMap = range_libc.PyOMap(self.map_msg) if self.WHICH_RM == "bl": self.range_method = range_libc.PyBresenhamsLine( oMap, self.MAX_RANGE_PX) elif "cddt" in self.WHICH_RM: self.range_method = range_libc.PyCDDTCast( oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) if self.WHICH_RM == "pcddt": print "Pruning..." self.range_method.prune() elif self.WHICH_RM == "rm": self.range_method = range_libc.PyRayMarching( oMap, self.MAX_RANGE_PX) elif self.WHICH_RM == "rmgpu": self.range_method = range_libc.PyRayMarchingGPU( oMap, self.MAX_RANGE_PX) elif self.WHICH_RM == "glt": self.range_method = range_libc.PyGiantLUTCast( oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) self.range_method.set_sensor_model(self.sensor_model_table)
def make_wall(self): scale = 0.0504 n_ranges = 2 theta = self.start_orientation y = self.start_loc[0] - 10*np.sin(theta) x = self.start_loc[1] - 10*np.cos(theta) max_scan_angle = 90*(np.pi/180) # map_loc = testMap = range_libc.PyOMap(self.map_loc,1) bl = range_libc.PyBresenhamsLine(testMap, 500) queries = np.zeros((n_ranges, 3), dtype=np.float32) ranges = np.zeros(n_ranges, dtype=np.float32) queries[:,0] = x queries[:,1] = y queries[:,2] = theta + np.linspace(-max_scan_angle, max_scan_angle, n_ranges) points = bl.calc_range_many(queries,ranges) bl.saveTrace("/home/racecar/racecar-ws/src/lab6/maps/test.png") # print imread("./test.png")[500][600] map_img = imread("/home/racecar/racecar-ws/src/lab6/maps/test.png") cells_to_remove = set() wall = [] for i in range(len(map_img[0])): for j in range(len(map_img)): if map_img[i][j][2] == 200: self.open_cells.discard((j,i)) y = -16.50 + i*scale x = 25.90 - j*scale p = Point() p.x = x p.y = y p.z = 0 wall.append(p) # print (j,i) self.open_cells_tuple = tuple(self.open_cells) self.start_flag = True print "wall made; set goal!" marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.pose.orientation.w = 1 marker.points = wall t = rospy.Duration() marker.lifetime = t marker.scale.x = 0.4 marker.scale.y = 0.4 marker.scale.z = 0.4 marker.color.a = 1.0 marker.color.r = 1.0 self.pub_wall.publish(marker)
def get_omap(self): ''' Fetch the occupancy grid map from the map_server instance, and initialize the correct RangeLibc method. Also stores a matrix which indicates the permissible region of the map ''' # this way you could give it a different map server as a parameter map_service_name = rospy.get_param("~static_map", "static_map") print("getting map from service: ", map_service_name) rospy.wait_for_service(map_service_name) self.map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map self.dilated_map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map self.map_info = self.map_msg.info self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS / self.map_info.resolution) self.init_dynamic_map(self.MAP_BASE_PATH) self.map_msg.data = self.dynamic_map.astype(float).flatten().tolist() oMap = range_libc.PyOMap(self.map_msg) # initialize range method print "Initializing range method:", self.WHICH_RM if self.WHICH_RM == "bl": self.range_method = range_libc.PyBresenhamsLine( oMap, self.MAX_RANGE_PX) elif "cddt" in self.WHICH_RM: self.range_method = range_libc.PyCDDTCast( oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) if self.WHICH_RM == "pcddt": print "Pruning..." self.range_method.prune() elif self.WHICH_RM == "rm": self.range_method = range_libc.PyRayMarching( oMap, self.MAX_RANGE_PX) elif self.WHICH_RM == "rmgpu": self.range_method = range_libc.PyRayMarchingGPU( oMap, self.MAX_RANGE_PX) elif self.WHICH_RM == "glt": self.range_method = range_libc.PyGiantLUTCast( oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) print "Done loading map" # 0: permissible, -1: unmapped, 100: blocked array_255 = np.array(self.map_msg.data).reshape( (self.map_msg.info.height, self.map_msg.info.width)) # 0: not permissible, 1: permissible self.permissible_region = np.zeros_like(array_255, dtype=bool) self.permissible_region[array_255 == 0] = 1 self.map_initialized = True
def get_omap(self): # this way you could give it a different map server as a parameter map_service_name = rospy.get_param("~static_map", "static_map") print("getting map from service: ", map_service_name) rospy.wait_for_service(map_service_name) map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map self.map_info = map_msg.info oMap = range_libc.PyOMap(map_msg) # this value is the max range used internally in RangeLibc # it also should be the size of your sensor model table self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS / self.map_info.resolution) # initialize range method print "Initializing range method:", self.WHICH_RANGE_METHOD if self.WHICH_RANGE_METHOD == "bl": self.range_method = range_libc.PyBresenhamsLine( oMap, self.MAX_RANGE_PX) elif "cddt" in self.WHICH_RANGE_METHOD: self.range_method = range_libc.PyCDDTCast( oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) if self.WHICH_RANGE_METHOD == "pcddt": print "Pruning..." self.range_method.prune() elif self.WHICH_RANGE_METHOD == "rm": self.range_method = range_libc.PyRayMarching( oMap, self.MAX_RANGE_PX) elif self.WHICH_RANGE_METHOD == "rmgpu": self.range_method = range_libc.PyRayMarchingGPU( oMap, self.MAX_RANGE_PX) elif self.WHICH_RANGE_METHOD == "glt": self.range_method = range_libc.PyGiantLUTCast( oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) print "Done loading map" # 0: permissible, -1: unmapped, large value: blocked array_255 = np.array(map_msg.data).reshape( (map_msg.info.height, map_msg.info.width)) # 0: not permissible, 1: permissible # this may be useful for global particle initialization - don't initialize particles in non-permissible states self.permissible_region = np.zeros_like(array_255, dtype=bool) self.permissible_region[array_255 == 0] = 1 self.map_initialized = True
def __init__(self, num_rays=None, min_angle=None, max_angle=None, max_range=None, range_method=None, frame=None, omap=None): self.max_range = max_range self.laser_ranges = np.zeros(num_rays, dtype=np.float32) self.laser_angles = np.linspace(min_angle, max_angle, num_rays, endpoint=True) self.queries = np.zeros((num_rays, 3), dtype=np.float32) map_msg = omap.map_msg map_info = map_msg.info oMap = range_libc.PyOMap(map_msg) self.MAX_RANGE_PX = int(self.max_range / map_info.resolution) self.THETA_DISCRETIZATION = 200 # initialize range method print "Initializing range method:", range_method if range_method == "bl": self.range_method = range_libc.PyBresenhamsLine( oMap, self.MAX_RANGE_PX) elif "cddt" in range_method: self.range_method = range_libc.PyCDDTCast( oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) if range_method == "pcddt": print "Pruning..." self.range_method.prune() elif range_method == "rm": self.range_method = range_libc.PyRayMarching( oMap, self.MAX_RANGE_PX) elif range_method == "rmgpu": self.range_method = range_libc.PyRayMarchingGPU( oMap, self.MAX_RANGE_PX) elif range_method == "glt": self.range_method = range_libc.PyGiantLUTCast( oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) print "Simulated Laser Scanner Initialized..."
def put_box_obstacle(self, x, y, w, h, check_collision=False): x1, y1 = x - w * 0.5, y - h * 0.5 x2, y2 = x + w * 0.5, y + h * 0.5 xx1, yy1 = self.grid_coord(x1, y1, int(1.0 / self.resolution)) xx2, yy2 = self.grid_coord(x2, y2, int(1.0 / self.resolution)) if check_collision: if np.any(self.occupancy_grid[yy1:yy2, xx1:xx2] == 0): return False # if np.sum(self.occupancy_grid[yy1:yy2, xx1:xx2]) > 0: # return False self.occupancy_grid[yy1:yy2, xx1:xx2] = 0.0 self.square_omap[xx1:xx2, yy1:yy2] = 1 self.omap = range_libc.PyOMap(self.square_omap) self.range_scanner = range_libc.PyBresenhamsLine(self.omap, 1000) return True
def get_omap(self): ''' Fetch the occupancy grid map from the map_server instance, and initialize the correct RangeLibc method. Also stores a matrix which indicates the permissible region of the map ''' # this way you could give it a different map server as a parameter # map_service_name = rospy.get_param('/static_map', '/static_map') map_service_name = '/static_map' print('getting map from service: ', map_service_name) print('map service name: {}'.format(map_service_name)) rospy.wait_for_service(map_service_name) map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map self.map_info = map_msg.info oMap = range_libc.PyOMap(map_msg) self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS / self.map_info.resolution) # initialize range method print 'Initializing range method:', self.WHICH_RM if self.WHICH_RM == 'bl': self.range_method = range_libc.PyBresenhamsLine(oMap, self.MAX_RANGE_PX) elif 'cddt' in self.WHICH_RM: self.range_method = range_libc.PyCDDTCast(oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) if self.WHICH_RM == 'pcddt': print 'Pruning...' self.range_method.prune() elif self.WHICH_RM == 'rm': self.range_method = range_libc.PyRayMarching(oMap, self.MAX_RANGE_PX) elif self.WHICH_RM == 'rmgpu': self.range_method = range_libc.PyRayMarchingGPU(oMap, self.MAX_RANGE_PX) elif self.WHICH_RM == 'glt': self.range_method = range_libc.PyGiantLUTCast(oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) print 'Done loading map' # 0: permissible, -1: unmapped, 100: blocked array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width)) # 0: not permissible, 1: permissible self.permissible_region = np.zeros_like(array_255, dtype=bool) self.permissible_region[array_255==0] = 1 self.map_initialized = True
def clear_obstacles(self): self.occupancy_grid = np.array(self.occupancy_grid_copy, copy=True) self.square_omap = np.array(self.square_omap_copy, copy=True) self.omap = range_libc.PyOMap(self.square_omap) self.range_scanner = range_libc.PyBresenhamsLine(self.omap, 1000)
def __init__(self, occupancy_grid, resolution, origin, initial_pos=None, path_map=None, path_map_division=16, path_map_dilation=2, path_map_weighted_dilation=False, reachable_area_dilation=3, destination_map=None, background_traversable=True, name=None): """ path_map encodes the allowed space for path planning reachable_area is path_map shrinked by reachable_area_dilation. This is useful for selecting starting point and goal points, which should be on the path map but also should not be too close to obstacles. :param occupancy_grid: a grayscale image where 255 means free space. :param resolution: real width of a pixel in the occupancy map in meters. :param destination_map: a colored path map where each color represents a destination. :param path_map_dilation: higher value will shrink the path map further :param path_map_weighted_dilation: create the path map from a weighted combination between the original path map and the dilated path map. This prevents tight openings from being closed due to the dilation process and also allows the agent to start at positions anywhere from the original path maps. :param reachable_area_dilation: higher value will shrink the reachable area further :param background_traversable: this affects map bounding box calculation. """ self.g = { 'path_map_dilation': path_map_dilation, 'reachable_area_dilation': reachable_area_dilation, 'path_map_weighted_dilation': path_map_weighted_dilation } self.occupancy_grid = occupancy_grid self.occupancy_grid_copy = np.array(self.occupancy_grid, copy=True) self.resolution = resolution self.origin = origin self.name = name free_space = (self.occupancy_grid >= 254).astype(np.uint8) # hard_obstacles = (self.occupancy_grid == 0).astype(np.uint8) self.binary_occupancy = (1 - free_space) * 255 # 0 means no obstacle # Compute distance transform of the map start_time = time.time() self.map_dist_transform, _ = cv2.distanceTransformWithLabels( 255 - self.binary_occupancy, cv2.DIST_L2, cv2.DIST_MASK_PRECISE) print('distance transform time: %.2f sec' % (time.time() - start_time)) self.visible_map_bbox = self._compute_visible_map_bbox( background_traversable) self.path_map_division = path_map_division if path_map is None: # Automatically generate path map. This is usually the case if we use a real # map generated by laser scan. path_map_scale = self.resolution / (1.0 / self.path_map_division) m = cv2.resize(self.binary_occupancy, dsize=(0, 0), fx=path_map_scale, fy=path_map_scale, interpolation=cv2.INTER_NEAREST) self.path_map = m else: self.path_map = (path_map > 0).astype(np.uint8) * 255 # cv2.imshow('path_map1', cv2.resize(self.path_map, (1024, 1024))) # The path map may contain multiple disconnected areas. Only the area where # the initial position falls will be considered. # However, if initial_pos is None, we use the occupancy grid itself as the path map. if initial_pos is not None: init_pos_grid = self.grid_coord(initial_pos[0], initial_pos[1], self.path_map_division) self.path_map = 255 - self.find_reachable_area( self.path_map, init_pos_grid) # Shrink the path map (i.e., dilate the occupied area) path_map_dilated = self.dilate(self.path_map, path_map_dilation) self.path_map_dilation = path_map_dilation self.reachable_area = 255 - self.dilate(path_map_dilated, reachable_area_dilation) self.reachable_area_dilation = reachable_area_dilation self.reachable_locs = list(zip(*np.nonzero(self.reachable_area)[::-1])) self.reachable_locs_per_destination = {} if path_map_weighted_dilation: self.path_map = (self.path_map * 0.2 + path_map_dilated * 0.8).astype(np.uint8) else: self.path_map = path_map_dilated self.path_map_weighted_dilation = path_map_weighted_dilation print('path map area', self._compute_path_map_area()) # self.path_map = path_map_dilated # print self.path_map.max() # exit(0) # cv2.imshow('reachable_area', cv2.resize(self.reachable_area, (1024, 1024))) # cv2.imshow('path_map2', cv2.resize(self.path_map, (1600, 1600))) # cv2.imwrite('/tmp/path_map.png', self.path_map) # cv2.waitKey(0) # exit(0) if destination_map is None: self.destination_map = None else: assert destination_map.shape[:2] == self.path_map.shape[:2] self.destination_map = destination_map self._gen_per_goal_reachable_locations() print('%d destinations' % len(self.reachable_locs_per_destination)) self.destination_centroids = self._precompute_destination_centroids( ) print('precompute destination paths...') start_time = time.time() self.destination_paths = self._precompute_destination_paths() print('precompute destination paths time %.2f sec' % (time.time() - start_time)) # pad occupancy map to make it a square because it seems range_libc will crash # on non-square map. h, w = self.binary_occupancy.shape self.square_omap = np.zeros((max(h, w), max(h, w)), np.uint8) self.square_omap[:w, :h] = np.transpose(self.binary_occupancy, (1, 0)) self.square_omap_copy = np.array(self.square_omap, copy=True) self.omap = range_libc.PyOMap(self.square_omap) self.range_scanner = range_libc.PyBresenhamsLine(self.omap, 1000)
# print(range_libc.USE_CACHED_CONSTANT)S # print(range_libc.USE_FAST_ROUND) # print(range_libc.NO_INLINE) # print(range_libc.USE_LRU_CACHE) # print(range_libc.LRU_CACHE_SIZE) testMapFileName = "../maps/test_medium.png".encode('utf8') testMap = range_libc.PyOMap(testMapFileName, 1) if testMap.error(): exit() max_range = 30 theta_discretization = 1000 print("Initializing: bl (max range = {0})".format(max_range)) bl = range_libc.PyBresenhamsLine(testMap, max_range) print("Initializing: rm (max range = {0})".format(max_range)) rm = range_libc.PyRayMarching(testMap, max_range) print("Initializing: cddt (max range = {0}, theta_discretization = {1})". format(max_range, theta_discretization) ) cddt = range_libc.PyCDDTCast(testMap, max_range, theta_discretization) cddt.prune() print("Initializing: glt (max range = {0}, theta_discretization = {1})". format(max_range, theta_discretization) ) glt = range_libc.PyGiantLUTCast(testMap, max_range, theta_discretization) print() # For testing / debugging def fixed_scan(num_ranges, print_sample=True):
# def make_scan(x,y,theta,n_ranges): # MAX_SCAN_ANGLE = (0.75 * np.pi) # bl = range_libc.PyBresenhamsLine(testMap, 300) # # bl = range_libc.PyRayMarching(testMap, 500) # queries = np.zeros((n_ranges,3),dtype=np.float32) # ranges = np.zeros(n_ranges,dtype=np.float32) # queries[:,0] = x # queries[:,1] = y # queries[:,2] = theta + np.linspace(-MAX_SCAN_ANGLE, MAX_SCAN_ANGLE, n_ranges) # bl.calc_range_many(queries,ranges) # bl.saveTrace("./test.png") # make_scan(510,520,np.pi/2.0,61) print("Init: bl") bl = range_libc.PyBresenhamsLine(testMap, 500) print("Init: rm") rm = range_libc.PyRayMarching(testMap, 500) print("Init: cddt") cddt = range_libc.PyCDDTCast(testMap, 500, 108) cddt.prune() print("Init: glt") glt = range_libc.PyGiantLUTCast(testMap, 500, 108) # this is for testing the amount of raw functional call overhead, does not compute ranges # null = range_libc.PyNull(testMap, 500, 108) for x in range(10): vals = np.random.random((3, num_vals)).astype(np.float32) vals[0, :] *= (testMap.width() - 2.0) vals[1, :] *= (testMap.height() - 2.0) vals[0, :] += 1.0