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 __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
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) queries[:,0] = int(testMap.width() / 2.0) queries[:,1] = int(testMap.height() / 2.0) queries[:,2] = np.linspace(0, 2.0 * np.pi, num_ranges) queries_deg = np.copy(queries) queries_deg[:,2] *= 180.0 / np.pi print("Test points (x, y, th (degrees)):") print("--------------------") print(queries_deg) print()
# 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 ranges = np.zeros(num_vals, dtype=np.float32) test_states = [None] * num_vals for i in range(num_vals): test_states[i] = (vals[0, i], vals[1, i], vals[2, i])