def __init__(self, scan_topic, laser_ray_step, exclude_max_range_rays, max_range_meters, map_msg, particles, weights, state_lock=None): if state_lock is None: self.state_lock = Lock() else: self.state_lock = state_lock self.particles = particles self.weights = weights self.LASER_RAY_STEP = laser_ray_step # Step for downsampling laser scans self.EXCLUDE_MAX_RANGE_RAYS = exclude_max_range_rays # Whether to exclude rays that are beyond the max range self.MAX_RANGE_METERS = max_range_meters # The max range of the laser oMap = range_libc.PyOMap(map_msg) # A version of the map that range_libc can understand max_range_px = int(self.MAX_RANGE_METERS / map_msg.info.resolution) # The max range in pixels of the laser self.range_method = range_libc.PyCDDTCast(oMap, max_range_px, THETA_DISCRETIZATION) # The range method that will be used for ray casting #self.range_method = range_libc.PyRayMarchingGPU(oMap, max_range_px) # The range method that will be used for ray casting self.range_method.set_sensor_model(self.precompute_sensor_model(max_range_px)) # Load the sensor model expressed as a table self.queries = None # Do not modify this variable self.ranges = None # Do not modify this variable self.downsampled_angles = None # The angles of the downsampled rays self.do_resample = True # Set so that outside code can know that it's time to resample # Subscribe to laser scans self.laser_sub = rospy.Subscriber(scan_topic, LaserScan, self.lidar_cb, queue_size=1)
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 __init__(self, map_msg, particles, weights, state_lock=None): if state_lock is None: self.state_lock = Lock() else: self.state_lock = state_lock self.particles = particles self.weights = weights self.LASER_RAY_STEP = int(rospy.get_param( "~laser_ray_step")) # Step for downsampling laser scans self.EXCLUDE_MAX_RANGE_RAYS = bool( rospy.get_param("~exclude_max_range_rays") ) # Whether to exclude rays that are beyond the max range self.MAX_RANGE_METERS = float( rospy.get_param("~max_range_meters")) # The max range of the laser oMap = range_libc.PyOMap( map_msg) # A version of the map that range_libc can understand max_range_px = int( self.MAX_RANGE_METERS / map_msg.info.resolution) # The max range in pixels of the laser self.range_method = range_libc.PyCDDTCast( oMap, max_range_px, THETA_DISCRETIZATION ) # The range method that will be used for ray casting #self.range_method = range_libc.PyRayMarchingGPU(oMap, max_range_px) # The range method that will be used for ray casting self.range_method.set_sensor_model( self.precompute_sensor_model( max_range_px)) # Load the sensor model expressed as a table self.queries = None self.ranges = None self.laser_angles = None # The angles of each ray self.downsampled_angles = None # The angles of the downsampled rays self.do_resample = False # Set so that outside code can know that it's time to resample
def __init__(self): self.UPDATE_RATE = float(rospy.get_param("~update_rate")) self.THETA_DISCRETIZATION = float(rospy.get_param("~theta_discretization")) self.RANGE_MIN_METERS = float(rospy.get_param("~range_min")) self.RANGE_MAX_METERS = float(rospy.get_param("~range_max")) self.RANGE_BLACKOUT_METERS = float(rospy.get_param("~range_no_obj")) self.ANGLE_STEP = float(rospy.get_param("~angle_step")) self.ANGLE_MIN = float(rospy.get_param("~angle_min")) self.ANGLE_MAX = float(rospy.get_param("~angle_max")) self.ANGLES = np.arange( self.ANGLE_MIN, self.ANGLE_MAX, self.ANGLE_STEP, dtype=np.float32 ) self.CAR_LENGTH = float(rospy.get_param("vesc/chassis_length")) self.Z_SHORT = float(rospy.get_param("~z_short")) self.Z_MAX = float(rospy.get_param("~z_max")) self.Z_BLACKOUT_MAX = float(rospy.get_param("~z_blackout_max")) self.Z_RAND = float(rospy.get_param("~z_rand")) self.Z_HIT = float(rospy.get_param("~z_hit")) self.Z_SIGMA = float(rospy.get_param("~z_sigma")) self.TF_PREFIX = str(rospy.get_param("~car_name").rstrip("/")) if len(self.TF_PREFIX) > 0: self.TF_PREFIX = self.TF_PREFIX + "/" self.BASE_FRAME_ID = self.TF_PREFIX + "base_link" self.ODOM_FRAME_ID = self.TF_PREFIX + "odom" self.LASER_FRAME_ID = self.TF_PREFIX + "laser_link" map_msg = self.get_map() occ_map = range_libc.PyOMap(map_msg) max_range_px = round(self.RANGE_MAX_METERS / map_msg.info.resolution) self.range_method = range_libc.PyCDDTCast( occ_map, max_range_px, self.THETA_DISCRETIZATION ) self.tl = tf.TransformListener() try: self.tl.waitForTransform( self.BASE_FRAME_ID, self.LASER_FRAME_ID, rospy.Time(0), rospy.Duration(15.0), rospy.Duration(1.0) ) position, orientation = self.tl.lookupTransform( self.BASE_FRAME_ID, self.LASER_FRAME_ID, rospy.Time(0) ) self.x_offset = position[0] except Exception: rospy.logfatal("Laser: transform from {0} to {1} not found, aborting!". format(self.LASER_FRAME_ID, self.BASE_FRAME_ID) ) exit() self.laser_pub = rospy.Publisher("laser/scan", LaserScan, queue_size=1) self.update_timer = rospy.Timer( rospy.Duration.from_sec(1.0 / self.UPDATE_RATE), self.timer_cb )
def __init__(self, scan_topic, laser_ray_step, exclude_max_range_rays, max_range_meters, map_msg, particles, weights, car_length, state_lock=None): if state_lock is None: self.state_lock = Lock() else: self.state_lock = state_lock self.particles = particles self.weights = weights self.LASER_RAY_STEP = laser_ray_step # Step for downsampling laser scans self.EXCLUDE_MAX_RANGE_RAYS = exclude_max_range_rays # Whether to exclude rays that are beyond the max range self.MAX_RANGE_METERS = max_range_meters # The max range of the laser self.CAR_LENGTH = car_length oMap = range_libc.PyOMap(map_msg) # A version of the map that range_libc can understand max_range_px = int(self.MAX_RANGE_METERS / map_msg.info.resolution) # The max range in pixels of the laser self.range_method = range_libc.PyCDDTCast(oMap, max_range_px, THETA_DISCRETIZATION) # The range method that will be used for ray casting #self.range_method = range_libc.PyRayMarchingGPU(oMap, max_range_px) # The range method that will be used for ray casting self.range_method.set_sensor_model(self.precompute_sensor_model(max_range_px)) # Load the sensor model expressed as a table self.queries = None self.ranges = None self.laser_angles = None # The angles of each ray self.downsampled_angles = None # The angles of the downsampled rays self.do_resample = False # Set so that outside code can know that it's time to resample # Subscribe to laser scans self.laser_sub = rospy.Subscriber(scan_topic, LaserScan, self.lidar_cb, queue_size=1) # FOR KIDNAPPED ROBOT PROBLEM self.do_confidence_update = False self.CONF_HISTORY_SIZE = 10 self.conf_history = Queue.Queue() self.conf_sum = 0.0 self.confidence = 1.0
def build(self, map_msg, mrx, theta_disc): """ ros_map : Numpy array with map grid values(as float) resolution : Resolution of the map origin : State of car first """ self.omap = range_libc.PyOMap(map_msg) self.scan_method = range_libc.PyCDDTCast(self.omap, mrx, theta_disc)
def getRangeLib(mapMsg): # Pre-compute range lib oMap = range_libc.PyOMap(mapMsg) rospy.loginfo("initializing range_libc...") MAX_RANGE_METERS = 60 MAX_RANGE_PX = int(MAX_RANGE_METERS / mapMsg.info.resolution) THETA_DISCRETIZATION = 200 rangeLib = range_libc.PyCDDTCast(oMap, MAX_RANGE_PX, THETA_DISCRETIZATION) rospy.loginfo("pruning...") # rangeLib.prune() rospy.loginfo("range_libc initialized") return rangeLib
def __init__(self): self.UPDATE_RATE = float(rospy.get_param("~update_rate", 10.0)) self.THETA_DISCRETIZATION = float( rospy.get_param("~theta_discretization", 656)) self.MIN_RANGE_METERS = float( rospy.get_param("~min_range_meters", 0.02)) self.MAX_RANGE_METERS = float(rospy.get_param("~max_range_meters", 5.6)) self.ANGLE_STEP = float( rospy.get_param("~angle_step", 0.00613592332229)) self.ANGLE_MIN = float(rospy.get_param("~angle_min", -2.08621382713)) self.ANGLE_MAX = float(rospy.get_param("~angle_max", 2.09234976768)) self.ANGLES = np.arange(self.ANGLE_MIN, self.ANGLE_MAX, self.ANGLE_STEP, dtype=np.float32) self.CAR_LENGTH = float(rospy.get_param("~car_length", 0.33)) self.Z_SHORT = float(rospy.get_param("~z_short", 0.03)) self.Z_MAX = float(rospy.get_param("~z_max", 0.16)) self.Z_BLACKOUT_MAX = float(rospy.get_param("~z_blackout_max", 50)) self.Z_RAND = float(rospy.get_param("~z_rand", 0.01)) self.Z_HIT = float(rospy.get_param("~z_hit", 0.8)) self.Z_SIGMA = float(rospy.get_param("~z_sigma", 0.03)) self.TF_PREFIX = str(rospy.get_param("~tf_prefix", "").rstrip("/")) if len(self.TF_PREFIX) > 0: self.TF_PREFIX = self.TF_PREFIX + "/" map_msg = self.get_map() occ_map = range_libc.PyOMap(map_msg) max_range_px = int(self.MAX_RANGE_METERS / map_msg.info.resolution) self.range_method = range_libc.PyCDDTCast(occ_map, max_range_px, self.THETA_DISCRETIZATION) self.tl = tf.TransformListener() while not self.tl.frameExists(self.TF_PREFIX + "base_link"): pass while not self.tl.frameExists(self.TF_PREFIX + "laser_link"): pass position, orientation = self.tl.lookupTransform( self.TF_PREFIX + "base_link", self.TF_PREFIX + "laser_link", rospy.Time(0)) self.x_offset = position[0] self.laser_pub = rospy.Publisher("scan", LaserScan, queue_size=1) self.update_timer = rospy.Timer( rospy.Duration.from_sec(1.0 / self.UPDATE_RATE), self.timer_cb)
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 __init__(self, map_msg, particles, weights, state_lock=None): if state_lock is None: self.state_lock = Lock() else: self.state_lock = state_lock self.particles = particles self.weights = weights self.weights_unsquashed = np.array(weights) self.LASER_RAY_STEP = int(rospy.get_param( "~laser_ray_step")) # Step for downsampling laser scans self.MAX_RANGE_METERS = float( rospy.get_param("~max_range_meters")) # The max range of the laser print("ZZ!") oMap = range_libc.PyOMap( map_msg) # A version of the map that range_libc can understand print("ZA!") print("ZA!") max_range_px = int( self.MAX_RANGE_METERS / map_msg.info.resolution) # The max range in pixels of the laser self.range_method = range_libc.PyCDDTCast( oMap, max_range_px, THETA_DISCRETIZATION ) # The range method that will be used for ray casting print("ZZZZZ!") self.range_method.set_sensor_model( self.precompute_sensor_model( max_range_px)) # Load the sensor model expressed as a table print("ZB!") self.queries = None self.ranges = None self.laser_angles = None # The angles of each ray self.downsampled_angles = None # The angles of the downsampled rays self.do_resample = False # Set so that outside code can know that it's time to resample print("ZC!") self.laser_sub = rospy.Subscriber(rospy.get_param( "~scan_topic", "/scan"), LaserScan, self.lidar_cb, queue_size=1) print("ZD!")
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 get_omap(self): """ Loads the map and initializes RangeLibc Map server must be running """ print("Loading Map") map_service_name = "static_map" rospy.wait_for_service(map_service_name) map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map self.map_info = map_msg.info print(self.map_info) oMap = range_libc.PyOMap(map_msg) # max range used internally in RangeLibc self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS / self.map_info.resolution) # initialize range method self.range_method = range_libc.PyCDDTCast(oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION) print "Done loading map" self.map_initialized = True
def __init__(self): self.GT_INDEX = 0 # Ground truth index (for sampling in K > 1 poses) # Scan constants self.THETA_DISCRETIZATION = 112 # Discretization of scanning angle self.Z_SHORT = 0.01 #0.14 # Weight for short reading self.LAMBDA_SHORT = 0.05 # Intrinsic parameter of the exponential dist. self.Z_MAX = 0.07 #0.015 # Weight for max reading self.Z_RAND = 0.12 #0.045 # Weight for random reading self.SIGMA_HIT = 8.0 # Noise value for hit reading self.Z_HIT = 0.80 #0.80 # Weight for hit reading self.MAX_RANGE_METERS = float(rospy.get_param( "~max_range_meters", 5.6)) # The max range of the laser # Dynamics constants self.MIN_VEL = -0.75 #TODO make sure these are right self.MAX_VEL = 0.75 self.MIN_DEL = -0.34 self.MAX_DEL = 0.34 self.laser_params = { "angle_min": -2.08621382713, "angle_max": 2.09234976768, "angle_increment": 0.00613592332229, "time_increment": 9.76562732831e-05, "scan_time": 0.10000000149, "range_min": 0.019999999553, "range_max": 5.59999990463 } self.num_ranges = 682 # Number of ranges to publish (M) # This is equivalent to int(max-min)/incr + 2 self.dtype = torch.cuda.FloatTensor self.K = 5 # Number of particles to publish # TODO: These values will need to be tuned self.sigma = torch.Tensor([0.25, 0.1]).type(self.dtype).expand(self.K, 2) # Initial position [x, y, theta] INITIAL_POSE = torch.Tensor([0.0, 0.0, 0.0]).type(self.dtype) dt = 0.1 # Constant dt, same as trained model # TODO: get topic names from server self.pose_pub_topic = "/sim/ground_truth/pose" self.poses_pub_topic = "/sim/ground_truth/poses" # TODO get rid of poses / or create a ROI self.scan_pub_topic = "/sim/scan" self.set_pose_sub_topic = "/sim/set_pose" self.control_sub_topic = "/vesc/high_level/ackermann_cmd_mux/input/nav_0" #self.model_name = rospy.get_param("~nn_model", "/media/JetsonSSD/model3.torch") self.model_name = rospkg.RosPack().get_path( "environment_sim") + "/models/dynamics_model_noisy.torch" # Initialize publishers and subscribers self.pose_pub = rospy.Publisher(self.pose_pub_topic, PoseStamped, queue_size=10) self.poses_pub = rospy.Publisher(self.poses_pub_topic, PoseArray, queue_size=10) self.scan_pub = rospy.Publisher(self.scan_pub_topic, LaserScan, queue_size=1) self.model = torch.load(self.model_name) self.model.cuda() # Tell torch to run the network on the GPU self.model.eval() # Model ideally runs faster in eval mode print("Loading:", self.model_name) print("Model:\n", self.model) print("Torch Datatype:", self.dtype) self.noise = torch.Tensor(self.K, 2).type(self.dtype) # (K,2) self.ctrl = torch.zeros(self.K, 2).type(self.dtype) # (K,2) self.pose = INITIAL_POSE.repeat(self.K, 1) # (K,3) self.pose_dot = torch.zeros(self.K, 3).type(self.dtype) # (K,3) self.nn_input = torch.zeros(self.K, 8).type(self.dtype) # (K,8) self.nn_input[:, 4] = 1.0 # cos(0) self.nn_input[:, 7] = dt # set dt = 0.1 # 0. Wait for map to publish / static map server rospy.wait_for_service('static_map') static_map = rospy.ServiceProxy('/static_map', GetMap) map_msg = static_map().map self.ranges = np.zeros(self.num_ranges, dtype=np.float32) self.query = np.zeros((1, 3), dtype=np.float32) # (num particles = 1, 3) self.obs_angles = np.arange(self.laser_params["angle_min"], self.laser_params["angle_max"] + self.laser_params["angle_increment"], self.laser_params["angle_increment"], dtype=np.float32) oMap = range_libc.PyOMap( map_msg) # A version of the map that range_libc can understand max_range_px = int( self.MAX_RANGE_METERS / map_msg.info.resolution) # The max range in pixels of the laser self.range_method = range_libc.PyCDDTCast( oMap, max_range_px, self.THETA_DISCRETIZATION ) # The range method that will be used for ray casting # TODO: do we need this to create a range self.range_method.set_sensor_model( self.precompute_sensor_model( max_range_px)) # Load the sensor model expressed as a table self.control_sub = rospy.Subscriber(self.control_sub_topic, AckermannDriveStamped, self.control_cb, queue_size=10) self.set_pose_sub = rospy.Subscriber(self.set_pose_sub_topic, PoseStamped, self.set_pose_cb, queue_size=1)
# 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 ranges = np.zeros(num_vals, dtype=np.float32)
def set_map(self, occupancy_grid): self.map = range_libc.PyOMap(occupancy_grid) self.max_range_px = int(self.max_range_meters / occupancy_grid.info.resolution) self.range_method = range_libc.PyCDDTCast(self.map, self.max_range_px, self.theta_discretization)
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) 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)