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
