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 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 setRaytracingMethod(self, method="RM"):
"""
Set which Ray tracing method to use
"""
if not self.hasMap:
print "for set RaytracingMethod use setMap first"
return
if method == "RM":
self.scan_method = range_libc.PyRayMarching(self.omap,
self.mrx)
elif method == "RMGPU":
self.scan_method = range_libc.PyRayMarchingGPU(self.omap,
self.mrx)
else:
print "Only ray marching is supported"
sys.exit()
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 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 as_sdf(self, raytracer=None):
NUMBA = False
RANGE_LIBC = True
occupied_points_ij = np.array(self.as_occupied_points_ij())
min_distances = np.ones(self.occupancy_.shape) * self.HUGE
if NUMBA:
compiled_sdf_math(occupied_points_ij, min_distances)
if RANGE_LIBC:
if raytracer is None:
import range_libc
pyomap = range_libc.PyOMap(np.ascontiguousarray(self.occupancy_.T >= self.thresh_occupied_))
rm = range_libc.PyRayMarching(pyomap, self.occupancy_.shape[0])
min_distances = np.ascontiguousarray(np.zeros_like(self.occupancy_, dtype=np.float32))
rm.get_dist(min_distances)
else:
min_distances = raytracer.get_dist()
# Change from i, j units to x, y units [meters]
min_distances = min_distances * self.resolution_
# Switch sign for occupied and unkown points (*signed* distance field)
min_distances[self.occupancy_ > self.thresh_free] *= -1.
return min_distances
# 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):
ranges_np = np.zeros(num_ranges, dtype=np.float32)
queries = np.zeros((num_ranges, 3), dtype=np.float32)
# 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
vals[1, :] += 1.0
vals[2, :] *= np.pi * 2.0
