def __init__(self): # Fetch parameters self.map_topic = rospy.get_param("~map_topic") self.num_beams_per_particle = rospy.get_param("~num_beams_per_particle") self.scan_theta_discretization = rospy.get_param("~scan_theta_discretization") self.scan_field_of_view = rospy.get_param("~scan_field_of_view") #################################### # TODO # Precompute the sensor model here # (You should probably write a # function for this) #################################### # Create a simulated laser scan self.scan_sim = PyScanSimulator2D( self.num_beams_per_particle, self.scan_field_of_view, 0, # This is not the simulator, don't add noise 0.01, # This is used as an epsilon self.scan_theta_discretization) # Subscribe to the map self.map_set = False rospy.Subscriber( self.map_topic, OccupancyGrid, self.map_callback, queue_size=1)
def __init__(self): self.save_path = os.path.join(rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") #%Y-%m-%d-%H-%M-%S self.trajectory = LineTrajectory("/built_trajectory") ''' Insert appropriate subscribers/publishers here ''' self.data_points = [] self.count = 0 self.num_waypoints = 4 self.waypoints = [] self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PolygonStamped, queue_size=10) self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20) self.trajectory.publish_viz() #duration=40.0 self.rtt_pub = rospy.Publisher("/rtt/final", Marker, queue_size = 10) self.rtt_tree_pub = rospy.Publisher("/rtt/tree", Marker, queue_size = 10000) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) # Create a simulated laser scan self.scan_sim = PyScanSimulator2D( 2, # number of beams to send out 0, # field of view centered around theta = 0 0, # don't add noise 0.01, # used as an epsilon 500) # discretize the theta space for faster ray tracing #subscribe to map self.map_set = False self.permissible_region = None self.resolution = None self.origin = None self.width = None self.height = None self.permissible_indices = None rospy.Subscriber( "/map", OccupancyGrid, self.map_callback, queue_size = 1) self.rrt = RRT(self.rtt_tree_pub,self.scan_sim)
def __init__(self): # Fetch parameters self.map_topic = rospy.get_param("~map_topic") self.num_beams_per_particle = rospy.get_param( "~num_beams_per_particle") self.scan_theta_discretization = rospy.get_param( "~scan_theta_discretization") self.scan_field_of_view = rospy.get_param("~scan_field_of_view") #################################### # TODO # Adjust these parameters self.alpha_hit = 0 self.alpha_short = 0 self.alpha_max = 0 self.alpha_rand = 0 self.sigma_hit = 0 # Your sensor table will be a `table_width` x `table_width` np array: self.table_width = 201 #################################### # Precompute the sensor model table self.sensor_model_table = None self.precompute_sensor_model() # Create a simulated laser scan self.scan_sim = PyScanSimulator2D( self.num_beams_per_particle, self.scan_field_of_view, 0, # This is not the simulator, don't add noise 0.01, # This is used as an epsilon self.scan_theta_discretization) # Subscribe to the map self.map = None self.map_set = False rospy.Subscriber(self.map_topic, OccupancyGrid, self.map_callback, queue_size=1)
def __init__(self): # Fetch parameters self.map_topic = rospy.get_param("~map_topic") self.num_beams_per_particle = rospy.get_param("~num_beams_per_particle") self.scan_theta_discretization = rospy.get_param("~scan_theta_discretization") self.scan_field_of_view = rospy.get_param("~scan_field_of_view") self.INV_SQUASH_FACTOR = 1.0 / float(rospy.get_param("~squash_factor")) #################################### # Precompute the sensor model here # (You should probably write a # function for this) self.MAX_RANGE_PX = 400 self.LIDAR_SCALE_TO_MAP_SCALE = rospy.get_param("~lidar_scale_to_map_scale", 1.0) self.sensor_model_table = None self.particle_scans = None self.precompute_sensor_model() #################################### # Create a simulated laser scan self.scan_sim = PyScanSimulator2D( self.num_beams_per_particle, self.scan_field_of_view, 0, # This is not the simulator, don't add noise 0.01, # This is used as an epsilon self.scan_theta_discretization) # Subscribe to the map self.map_set = False self.map = None self.permissible_region = None self.map_info = None rospy.Subscriber( self.map_topic, OccupancyGrid, self.map_callback, queue_size=1)
def __init__(self): self.on_robot = rospy.get_param("~on_robot") # Fetch parameters self.map_topic = rospy.get_param("~map_topic") #self.num_beams_per_particle = rospy.get_param("~num_beams_per_particle") if self.on_robot == 1: self.num_beams_per_particle = 109 #1081 else: self.num_beams_per_particle = 100 self.scan_theta_discretization = rospy.get_param( "~scan_theta_discretization") self.scan_field_of_view = rospy.get_param("~scan_field_of_view") #################################### def p_hit(zt, z_star, z_max, sigma): output = 1 / np.sqrt( 2 * np.pi * sigma**2) * np.e**(-(zt - z_star)**2 / (2 * sigma**2)) mask = ((zt >= 0) & (zt <= z_max)) output[~mask] = 0 return output def p_short(zt, z_star): with np.errstate(divide='ignore', invalid='ignore'): output = 2 / z_star * (1 - zt / z_star) output[output == np.inf] = 0 output = np.nan_to_num(output) mask = ((zt >= 0) & (zt <= z_star)) output[~mask] = 0 return output def p_max(zt, z_max): return (zt == z_max) * 1 def p_rand(zt, z_max): output = np.ones((zt.shape[0], 1)) output = output / z_max mask = ((zt >= 0) & (zt < z_max)) output[~mask] = 0 return output def p(zt, z_star, z_max, sigma, a_hit, a_short, a_max, a_rand): answer = a_hit * p_hit(zt, z_star, z_max, sigma) \ + a_short * p_short(zt, z_star) \ + a_max * p_max(zt, z_max) \ + a_rand * p_rand(zt, z_max) # print("Hit: ", p_hit(zt, z_star, z_max, sigma)) # print("Short: ", p_short(zt, z_star)) # print("Max: ", p_max(zt, z_max)) # print("Rand: ", p_rand(zt, z_max)) return answer self.z_max = 10 sigma = 0.5 a_hit = rospy.get_param("~a_hit") a_short = rospy.get_param("~a_short") a_max = rospy.get_param("~a_max") a_rand = rospy.get_param("~a_rand") self.increment = 0.01 elements = np.arange(0, self.z_max + self.increment, self.increment) pairs = np.array(np.meshgrid(elements, elements)).T.reshape(-1, 2) zt = pairs[:, 0: 1] # Takes the first col of pairs (Ex. like the x value of a coordinate pair) z_star = pairs[:, 1: 2] # Takes the second col of pairs (Ex. like the y value of a coordinate pair) grid = p(zt, z_star, self.z_max, sigma, a_hit, a_short, a_max, a_rand) grid = self.normalize(grid) self.grid = grid.reshape((elements.shape[0], elements.shape[0])).T #################################### # Create a simulated laser scan self.scan_sim = PyScanSimulator2D( self.num_beams_per_particle, self.scan_field_of_view, 0, # This is not the simulator, don't add noise 0.01, # This is used as an epsilon self.scan_theta_discretization) # Subscribe to the map self.map_set = False rospy.Subscriber(self.map_topic, OccupancyGrid, self.map_callback, queue_size=1)
def __init__(self): # Fetch parameters self.map_topic = rospy.get_param("~map_topic") self.num_beams_per_particle = rospy.get_param("~num_beams_per_particle") self.scan_theta_discretization = rospy.get_param("~scan_theta_discretization") self.scan_field_of_view = rospy.get_param("~scan_field_of_view") #################################### # TODO # Precompute the sensor model here # (You should probably write a # function for this) #with open('index_data.pkl', 'rb') as f: # *** MAKE SURE PKL FILE IS IN src DIRECTORY *** with open('/home/racecar/racecar_ws/src/localization/sensor_model/index_data.pkl', 'rb') as f: self.table = pickle.load(f) # lookup table for probabilities # key = (zt, zt_star) = (measured distance, ground truth distance) # value = probability(measured | ground truth) def prob_lookup(zt, zt_star): ''' look up probability given zt (measured) and zt_star (ground truth) floor indices to match values stored in the table -- zt min: 0.5 max: 100 num_samples: 200 -- zt_star min: 0.5 max: 100 num_samples: 200 :param zt: measured distance (m) :param zt_star: ground truth distance :return: probability (zt | zt_star) ''' n = 200 zmax = 100 # TODO: use round() instead of math.floor()? Will it have an index out of range error if rounds up at max index? zt_index = int(math.floor((zt - 0.5)*(n-1)/(zmax-0))) zt_star_index = int(math.floor((zt_star-0.5)*(n-1)/(zmax-0.1))) return self.table[zt_index][zt_star_index] # self.prob_lookup is a function that can take a nested sequence of objects or numpy arrays as inputs and return a single numpy array as output self.prob_lookup = np.vectorize(prob_lookup) #################################### # Create a simulated laser scan self.scan_sim = PyScanSimulator2D( self.num_beams_per_particle, self.scan_field_of_view, 0, # This is not the simulator, don't add noise 0.01, # This is used as an epsilon self.scan_theta_discretization) # Subscribe to the map self.map_set = False rospy.Subscriber( self.map_topic, OccupancyGrid, self.map_callback, queue_size=1) rospy.loginfo('sensor model initialized')