def do_long_range_message(config, robot1, robot2, motion1, motion2): """Potentially transfer long range messages between robots. Depending on whether the robots are within thresh distance, they will exchange long range measurements. The long range measurements are sent in the form of LongRangeMeasurement objects. Args: config: Configuration file. robot1: First Robot object. robot2: Second Robot object. motion1: First RobotMotion object. motion2: Second RobotMotion object. """ # If within long range, exchange long range sensor measurements. if dist(motion1.pos, motion2.pos) < config['long_thresh']: # Get message data from each robot. message1to2_data = robot1.get_long_range_message() message2to1_data = robot2.get_long_range_message() # Create the sensor model, which will be used to make all measurements. sm = SensorModel(config) # Compute the pairwise sensor measurements between each robot. num_sensors = len(config['sensor_parameters']) robot1_measurements = [ sm.get_measurement(motion1, i, motion2) for i in range(num_sensors) ] robot2_measurements = [ sm.get_measurement(motion2, i, motion1) for i in range(num_sensors) ] logging.debug("Robot 1 Measurements: %s", robot1_measurements) logging.debug("Robot 2 Measurements: %s", robot2_measurements) # Stuff the data into the LongRangeMessages message1to2 = LongRangeMessage(message1to2_data, robot2_measurements) message2to1 = LongRangeMessage(message2to1_data, robot1_measurements) # And then transmit both of the messages. # Do it this way to ensure that receiving a message doesn't # modify some state inside the robot. robot1.receive_long_range_message(message2to1) robot2.receive_long_range_message(message1to2) # indicate whether these two robots communicated return True return False
def test_hyperparameter(): np.random.seed(10008) map_obj = MapReader('../data/map/wean.dat') occupancy_map = map_obj.get_map() # generate a random particle, then sent out beams from that location # indices = np.where(occupancy_map.flatten() == 0)[0] # ind = np.random.choice(indices, 1)[0] # y, x = ind // w, ind % w # theta = -np.pi / 2 # angle = np.pi * (40 / 180) X = init_particles_freespace(1, occupancy_map) sensor = SensorModel(occupancy_map) X = X.reshape(1, -1) z_t_star = sensor.ray_casting(X[:, :3], num_beams=10) print(z_t_star) print(z_t_star.max()) z = np.arange(sensor._max_range + 2).astype(np.float) p_hit, p_short, p_max, p_rand = sensor.estimate_density(z, z_t_star[0][5]) # plot(1, p_hit) # plot(2, p_short) # plot(3, p_max) # plot(4, p_rand) # w_hit = 3 # 99 / 2 / 2.5 / 4 # 1. # w_short = 0.05 # 2 * 198 / 4 / 2.5 / 4 # 1 # w_max = 0.1 # 49 / 2.5 / 4 # 0.5 # w_rand = 10 # 990 / 4 # 5 # self._z_hit = 99 / 2 / 2.5 / 4 # 1. # self._z_short = 198 / 4 // 2.5 / 4 # 1 # self._z_max = 49 / 2.5 / 4 # 0.5 # self._z_rand = 990 / 4 # 5 # w_hit = 1. # w_short = 0.1 # w_max = 0.5 # w_rand = 10 w_hit = 1000 # 99 / 2 / 2.5 / 4 # 1. w_short = 0.01 # 2 * 198 / 4 / 2.5 / 4 # 1 w_max = 0.03 # 49 / 4 / 4 # 0.5 w_rand = 12500 p = sensor._z_hit * p_hit + sensor._z_short * p_short + sensor._z_max * p_max + sensor._z_rand * p_rand plot(5, p) plt.show()
def __init__(self, init_p_occ=0.8, min_count=5): self.tmap_bin = None self.tmap = None self.tmap_aug = None self.pmap = None self.map_properties = {} self.tmap_loaded = False self.gplane_loaded = False self.gp_plane_map = None self.gp_emap = None self.gp_emap_prop = {} self.gp_ec = 0 # gamma model self.gamma_model_loaded = False self.gamma_rbin_ids = [] self.gamma_avgs = [] self.gamma_stds = [] self.count = [] # intensity spectrum of ground self.ground_spectrum = {} self.min_count = min_count # probability model variables self.init_p_occ = init_p_occ # saftey factor - the largest this should be is 1 - init_p_occ self.saftey_value = 1 - self.init_p_occ # sensor model self.sensor_model = SensorModel()
def __init__(self, map_file='../data/map/wean.dat'): self.map = Map(map_file) #self.map.display_gaussian([], 'Gaussian Blurred Map') self._number_particles = 1000 self.particles = list() self._particle_probabilities = [] for _ in range(0, self._number_particles): print 'Initializing particle', _ particle = Particle(self.map) #particle.x = 4080 + np.random.normal(scale=35) #particle.y = 3980 + np.random.normal(scale=15) #particle.theta = math.pi + 0.1 + np.random.normal(scale=.1) self.particles.append(particle) self._particle_probabilities.append(1) self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1) self._sensor_model = SensorModel(self.map) self._ranges = []
def __init__(self): # Get parameters self.particle_filter_frame = \ rospy.get_param("~particle_filter_frame") # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel()
class MyTestCase(unittest.TestCase): def setUp(self): self.sensor_model = SensorModel(map) self.particle = Particle(map) self.particle.x = 250 self.particle.y = 250 self.particle.theta = 0 def test_plot(self): self.sensor_model.plot_model() def test_laser_state(self): (x, y, theta) = _laser_state(self.particle) self.assertEqual(x, 275) self.assertEqual(y, 250) self.assertEqual(theta, 0) self.particle.theta = -math.pi / 2 (x, y, theta) = _laser_state(self.particle) self.assertEqual(x, 250) self.assertEqual(y, 225) self.assertEqual(theta, -math.pi / 2) def test_ray_cast(self): laser_state = _laser_state(self.particle) right = self.sensor_model._ray_cast(laser_state, -math.pi / 2) self.assertAlmostEqual(right, 250, places=0) # should hit edge of map straight = self.sensor_model._ray_cast(laser_state, 0) self.assertAlmostEqual( straight, 225, places=0) # should hit wall in front, minus 25 for laser offset left = self.sensor_model._ray_cast(laser_state, math.pi / 2) self.assertAlmostEqual(left, 750, places=0) # should hit edge of map on left left_diagonal = self.sensor_model._ray_cast(laser_state, math.pi / 4) self.assertAlmostEqual(left_diagonal, 225 / math.cos(math.pi / 4), places=0) # should hit wall off to left def test__get_probability(self): log_probabilities = [] particle = Particle(map) particle.x = 250 particle.y = 500 particle.theta = 0 for _ in range(0, 10): x = 200 + 10 * _ ranges = [] for i in range(-90, 91): side = abs(500.0 / (math.sin(i * math.pi / 180) + 0.000001)) wall = abs((500.0 - x - 25) / (math.cos(i * math.pi / 180) + 0.0000001)) ranges.append(min(side, wall)) log_probabilities.append( self.sensor_model.get_probability(particle, ranges)) most_likely_reading = max(enumerate(log_probabilities), key=lambda x: x[1])[0] self.assertEqual(most_likely_reading, 5)
def test_raycasting_vectorize(): np.random.seed(10008) map_obj = MapReader('../data/map/wean.dat') occupancy_map = map_obj.get_map() # generate a random particle, then sent out beams from that location h, w = occupancy_map.shape indices = np.where(occupancy_map.flatten() == 0)[0] ind = np.random.choice(indices, 1)[0] y, x = ind // w, ind % w theta = np.pi / 2 X = np.array([[x, y, theta]]) X = np.repeat(X, 2, axis=0) X[:, :2] *= 10 num_beams = 180 sensor = SensorModel(occupancy_map) z_t_star = sensor.ray_casting(X, num_beams=num_beams) x0, y0 = X[0, :2] angle = np.arange(num_beams) * (np.pi / num_beams) angle = theta + angle - np.pi / 2 x1 = x0 + z_t_star * np.cos(angle) y1 = y0 - z_t_star * np.sin(angle) x0, y0 = x0 / 10, y0 / 10 x1, y1 = x1 / 10, y1 / 10 # plot figure fig = plt.figure() plt.imshow(occupancy_map) plt.scatter(x0, y0, c='red') plt.scatter(x1, y1, c='yellow') print(f'(x0, y0): ({x0}, {y0}), (x1, y1): ({x1}, {y1})') # plt.plot((x0, x1), (y0, y1), color='yellow') plt.show() print(z_t_star)
class MyTestCase(unittest.TestCase): def setUp(self): self.sensor_model = SensorModel(map) self.particle = Particle(map) self.particle.x = 250 self.particle.y = 250 self.particle.theta = 0 def test_plot(self): self.sensor_model.plot_model() def test_laser_state(self): (x, y, theta) = _laser_state(self.particle) self.assertEqual(x, 275) self.assertEqual(y, 250) self.assertEqual(theta, 0) self.particle.theta = -math.pi/2 (x, y, theta) = _laser_state(self.particle) self.assertEqual(x, 250) self.assertEqual(y, 225) self.assertEqual(theta, -math.pi/2) def test_ray_cast(self): laser_state = _laser_state(self.particle) right = self.sensor_model._ray_cast(laser_state, -math.pi/2) self.assertAlmostEqual(right, 250, places=0) # should hit edge of map straight = self.sensor_model._ray_cast(laser_state, 0) self.assertAlmostEqual(straight, 225, places=0) # should hit wall in front, minus 25 for laser offset left = self.sensor_model._ray_cast(laser_state, math.pi/2) self.assertAlmostEqual(left, 750, places=0) # should hit edge of map on left left_diagonal = self.sensor_model._ray_cast(laser_state, math.pi/4) self.assertAlmostEqual(left_diagonal, 225/math.cos(math.pi/4), places=0) # should hit wall off to left def test__get_probability(self): log_probabilities = [] particle = Particle(map) particle.x = 250 particle.y = 500 particle.theta = 0 for _ in range(0, 10): x = 200 + 10*_ ranges = [] for i in range(-90, 91): side = abs(500.0/(math.sin(i*math.pi/180)+0.000001)) wall = abs((500.0-x-25)/(math.cos(i*math.pi/180)+0.0000001)) ranges.append(min(side, wall)) log_probabilities.append(self.sensor_model.get_probability(particle, ranges)) most_likely_reading = max(enumerate(log_probabilities), key=lambda x: x[1])[0] self.assertEqual(most_likely_reading, 5)
def __init__(self, map_file='../data/map/wean.dat'): self.map = Map(map_file) #self.map.display_gaussian([], 'Gaussian Blurred Map') self._number_particles = 1000 self.particles = list() self._particle_probabilities = [] for _ in range(0, self._number_particles): print 'Initializing particle', _ particle = Particle(self.map) #particle.x = 4080 + np.random.normal(scale=35) #particle.y = 3980 + np.random.normal(scale=15) #particle.theta = math.pi + 0.1 + np.random.normal(scale=.1) self.particles.append(particle) self._particle_probabilities.append(1) self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1) self._sensor_model = SensorModel(self.map) self._ranges = []
def __init__(self): # Get parameters self.particle_filter_frame = \ rospy.get_param("~particle_filter_frame") # Initialize publishers/subscribers # # *Important Note #1:* It is critical for your particle # filter to obtain the following topic names from the # parameters for the autograder to work correctly. Note # that while the Odometry message contains both a pose and # a twist component, you will only be provided with the # twist component, so you should rely only on that # information, and *not* use the pose component. scan_topic = rospy.get_param("~scan_topic", "/scan") odom_topic = rospy.get_param("~odom_topic", "/odom") self.laser_sub = rospy.Subscriber(scan_topic, LaserScan, YOUR_LIDAR_CALLBACK, # TODO: Fill this in queue_size=1) self.odom_sub = rospy.Subscriber(odom_topic, Odometry, YOUR_ODOM_CALLBACK, # TODO: Fill this in queue_size=1) # *Important Note #2:* You must respond to pose # initialization requests sent to the /initialpose # topic. You can test that this works properly using the # "Pose Estimate" feature in RViz, which publishes to # /initialpose. self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, YOUR_POSE_INITIALIZATION_CALLBACK, # TODO: Fill this in queue_size=1) # *Important Note #3:* You must publish your pose estimate to # the following topic. In particular, you must use the # pose field of the Odometry message. You do not need to # provide the twist part of the Odometry message. The # odometry you publish here should be with respect to the # "/map" frame. self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size = 1) # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel()
def __init__(self): rospy.init_node('pf') real_robot = False # create instances of two helper objects that are provided to you # as part of the project self.particle_filter = ParticleFilter() self.occupancy_field = OccupancyField() self.TFHelper = TFHelper() self.sensor_model = sensor_model = SensorModel( model_noise_rate=0.5, odometry_noise_rate=0.15, world_model=self.occupancy_field, TFHelper=self.TFHelper) self.position_delta = None # Pose, delta from current to previous odometry reading self.last_scan = None # list of ranges self.odom = None # Pose, current odometry reading self.x_y_spread = 0.3 # Spread constant for x-y initialization of particles self.z_spread = 0.2 # Spread constant for z initialization of particles self.n_particles = 150 # number of particles # pose_listener responds to selection of a new approximate robot # location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) rospy.Subscriber("odom", Odometry, self.update_position) rospy.Subscriber("stable_scan", LaserScan, self.update_scan) # publisher for the particle cloud for visualizing in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
def setUp(self): self.sensor_model = SensorModel(map) self.particle = Particle(map) self.particle.x = 250 self.particle.y = 250 self.particle.theta = 0
parser.add_argument('--path_to_log', default='../data/log/robotdata1.log') parser.add_argument('--output', default='results') parser.add_argument('--num_particles', default=500, type=int) parser.add_argument('--visualize', action='store_true') args = parser.parse_args() src_path_map = args.path_to_map src_path_log = args.path_to_log os.makedirs(args.output, exist_ok=True) map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() map_model = ReadTable() ray_map = map_model.return_Map() print(ray_map.shape) # ray_map = 0 bool_occ_map = (occupancy_map < 0.35) & (occupancy_map >= 0) num_particles = args.num_particles initial_num = num_particles # X_bar = init_particles_random(num_particles, occupancy_map) X_bar = init_particles_freespace(num_particles, occupancy_map)
# initialize mesh map module print('Load mesh map and initialize map module...') map_module = MapModule(map_poses, map_file) # initialize particles print('Monte Carlo localization initializing...') map_size, road_coords = gen_coords_given_poses(map_poses) if config['pose_tracking']: particles = init_particles_pose_tracking(numParticles, poses[start_idx]) else: particles = init_particles_given_coords(numParticles, road_coords) # initialize sensor model sensor_model = SensorModel(map_module, scan_folder, config['range_image']) if config['range_image']['render_instanced']: update_weights = sensor_model.update_weights_instanced else: update_weights = sensor_model.update_weights # generate odom commands commands = gen_commands(poses) # initialize a visualizer if visualize: plt.ion() visualizer = Visualizer(map_size, poses, map_poses, grid_res=grid_res,
parser.add_argument('--adaptive', action='store_true', help='Adaptively select particles or not') args = parser.parse_args() src_path_map = args.path_to_map src_path_log = args.path_to_log os.makedirs(args.output, exist_ok=True) map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() with open(src_path_log, 'r') as f: logfile = f.readlines() motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = args.num_particles X_bar = init_particles_freespace(num_particles, occupancy_map) num_beams = 10 if args.debug else 18 """ Monte Carlo Localization Algorithm : Main Loop """ if args.visualize: visualize_map(occupancy_map) visualize_timestep(X_bar, 0, args.output) first_time_idx = True total_time, cnt, time_id = 0., 0., 0
from readtable import ReadTable from matplotlib import pyplot as plt from matplotlib import figure as fig import time src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() bool_occ_map = (occupancy_map < 0.35) & (occupancy_map >= 0) print(bool_occ_map[500][400]) logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() def visualize_ray_cast(x_t1, bool_occ_map, tstep, output_path): walk_stride = 8 angle_stride = 10 x, y, theta = x_t1 laser_x_t1 = x + 25 * math.cos(theta) laser_y_t1 = y + 25 * math.sin(theta) xout = laser_x_t1 / 10 yout = laser_y_t1 / 10 for deg in range(-90, 90, angle_stride): laser_theta = (deg + theta * 180 / math.pi) * (math.pi / 180) calc_distance, x_end, y_end = ray_cast_visual(laser_x_t1, laser_y_t1, laser_theta, walk_stride)
def init_world_once(self, do_test=True): # create a blank world, PRM style num_nodes = self.config["num_nodes"] connection_radius = self.config["connection_radius"] environment_size = self.config["environment_size"] self.sensor_range = self.config["sensor_range"] # vertices self.vertices_surface = [] self.vertices = [] count = 0 for vertex_idx in xrange(num_nodes): x = random.uniform(0, environment_size[0]) y = random.uniform(0, environment_size[1]) for z in [-10, self.surface_level]: v = Vertex(x, y, z, count) count += 1 self.vertices.append(v) if z == self.surface_level: self.vertices_surface.append(True) else: self.vertices_surface.append(False) # edges, stored as a matrix indexed as [vertex_start, vertex_end] self.num_nodes = len( self.vertices ) #doubled the input num_nodes in creating two layers of vertices instead of one self.edge_matrix = [None] * self.num_nodes self.edge_adjacency_idx_lists = [None] * self.num_nodes self.edge_adjacency_edge_lists = [None] * self.num_nodes for vertex_start_idx in xrange(self.num_nodes): self.edge_matrix[vertex_start_idx] = [None] * self.num_nodes self.edge_adjacency_idx_lists[vertex_start_idx] = [] self.edge_adjacency_edge_lists[vertex_start_idx] = [] for vertex_end_idx in xrange(self.num_nodes): cost = distance(self.vertices[vertex_start_idx], self.vertices[vertex_end_idx]) if vertex_start_idx != vertex_end_idx and cost <= connection_radius and not ( self.vertices_surface[vertex_start_idx] and self.vertices_surface[vertex_end_idx]): exists = True else: exists = False edge = Edge(vertex_start_idx, vertex_end_idx, cost, exists) self.edge_matrix[vertex_start_idx][vertex_end_idx] = edge if exists: self.edge_adjacency_idx_lists[vertex_start_idx].append( vertex_end_idx) self.edge_adjacency_edge_lists[vertex_start_idx].append( edge) if do_test: self.test_indices() #self.vertex_target_idx = self.create_target_idx() #single robot # Define prior distribution self.num_classes = self.config["num_classes"] self.prob_of_class0 = self.config["prob_of_class0"] self.prob_of_other_classes = (1 - self.prob_of_class0) / ( self.num_classes - 1) self.prior = np.zeros(self.num_classes) #p_y0, p_y1, ... for i in range(self.num_classes): if i == 0: self.prior[i] = self.prob_of_class0 else: self.prior[i] = self.prob_of_other_classes self.randomize_targets() self.original_classes_y = copy.copy(self.classes_y) # Define single random drop-off location (on surface) per world temp = random.randint(0, len(self.vertices) - 1) while self.vertices_surface[temp] != True: temp = random.randint(0, len(self.vertices) - 1) self.drop_off_idx = temp ''' # Set all class 2 vertices to is_armed = True self.is_armed_array = np.zeros(self.num_nodes, dtype=bool) #default, False for all for v in xrange(len(self.classes_y)): if self.classes_y[v] == 2: #it is a mine self.is_armed_array[v] = True ''' ''' # Vertex classes # Each vertex has a number between 0 and n, which represents the class of the vertex num_v_y0 = int(self.num_nodes*self.prob_of_class0) num_v_other = self.num_nodes - num_v_y0 num_v_each = num_v_other/(self.num_classes-1) classes_y = [] classes_y.extend([0]*num_v_y0) # add the non-target vertex classes for i in range(1,self.num_classes): classes_y.extend([i]*num_v_each) # add the target vertex classes # G round truth vertex classes random.shuffle(classes_y) #shuffle the list to randomize location of targets ''' self.comms_range = self.config["comms_range"] self.vertices_in_comms_range = self.generateCommsRangeVertices() # Setup sensor model self.sensor_model = SensorModel(self.config, self.num_nodes, self)
class ParticleFilter(object): """ This is the main particle filter object. """ def __init__(self, map_file='../data/map/wean.dat'): self.map = Map(map_file) #self.map.display_gaussian([], 'Gaussian Blurred Map') self._number_particles = 1000 self.particles = list() self._particle_probabilities = [] for _ in range(0, self._number_particles): print 'Initializing particle', _ particle = Particle(self.map) #particle.x = 4080 + np.random.normal(scale=35) #particle.y = 3980 + np.random.normal(scale=15) #particle.theta = math.pi + 0.1 + np.random.normal(scale=.1) self.particles.append(particle) self._particle_probabilities.append(1) self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1) self._sensor_model = SensorModel(self.map) self._ranges = [] def log(self, file_handle): line = list() for particle in self.particles: loc = str(particle.x) + ',' + str(particle.y) line.append(loc) file_handle.write(';'.join(line)) file_handle.write('\n') def display(self): self.map.display(self.particles, ranges=self._ranges) def update(self, line): """ Update step. Reading is a single reading (i.e. line) from the log file Could be either an odometry or laser reading """ measurement_type = line[0] # O = odometry, L = laser scan measurements = np.fromstring(line[2:], sep=' ') odometry = measurements[0:3] time_stamp = measurements[-1] # last variable for particle in self.particles: self._motion_model.update(particle, odometry, time_stamp) if measurement_type == "L": odometry_laser = measurements[3:6] # coordinates of the laser in standard odometry frame self._ranges = measurements[6:-1] # exclude last variable, the time stamp self._particle_probabilities = list() # unnormalized sensor model probabilities of the particles for particle in self.particles: self._particle_probabilities.append(self._sensor_model.get_probability(particle, self._ranges)) argsorted_probabilities = np.argsort(-np.asarray(self._particle_probabilities)) self.particles[argsorted_probabilities[0]].debug = True self.particles[argsorted_probabilities[1]].debug = True self.particles[argsorted_probabilities[2]].debug = True def _resample(self): """ Resamples the particles given unnormalized particle probabilites """ particle_probabilities = np.asarray(self._particle_probabilities)/sum(self._particle_probabilities) # normalize indices = np.random.choice(range(0, self._number_particles), size=self._number_particles, replace=True, p=particle_probabilities) indices.sort() previous_index = -1 new_particles = list() for index in indices: if index != previous_index: new_particles.append(self.particles[index]) else: new_particles.append(deepcopy(self.particles[index])) previous_index = index self.particles = new_particles
def __init__( self, publish_tf, n_particles, n_viz_particles, odometry_topic, motor_state_topic, servo_state_topic, scan_topic, laser_ray_step, exclude_max_range_rays, max_range_meters, speed_to_erpm_offset, speed_to_erpm_gain, steering_angle_to_servo_offset, steering_angle_to_servo_gain, car_length, ): """ Initializes the particle filter publish_tf: Whether or not to publish the tf. Should be false in sim, true on real robot n_particles: The number of particles n_viz_particles: The number of particles to visualize odometry_topic: The topic containing odometry information motor_state_topic: The topic containing motor state information servo_state_topic: The topic containing servo state information scan_topic: The topic containing laser scans laser_ray_step: Step for downsampling laser scans exclude_max_range_rays: Whether to exclude rays that are beyond the max range max_range_meters: The max range of the laser speed_to_erpm_offset: Offset conversion param from rpm to speed speed_to_erpm_gain: Gain conversion param from rpm to speed steering_angle_to_servo_offset: Offset conversion param from servo position to steering angle steering_angle_to_servo_gain: Gain conversion param from servo position to steering angle car_length: The length of the car """ self.PUBLISH_TF = publish_tf # The number of particles in this implementation, the total number of particles is constant. self.N_PARTICLES = n_particles self.N_VIZ_PARTICLES = n_viz_particles # The number of particles to visualize # Cached list of particle indices self.particle_indices = np.arange(self.N_PARTICLES) # Numpy matrix of dimension N_PARTICLES x 3 self.particles = np.zeros((self.N_PARTICLES, 3)) # Numpy matrix containing weight for each particle self.weights = np.ones(self.N_PARTICLES) / float(self.N_PARTICLES) # A lock used to prevent concurrency issues. You do not need to worry about this self.state_lock = Lock() self.tfl = tf.TransformListener( ) # Transforms points between coordinate frames # Get the map map_msg = rospy.wait_for_message(MAP_TOPIC, OccupancyGrid) self.map_info = map_msg.info # Save info about map for later use # Create numpy array representing map for later use array_255 = np.array(map_msg.data).reshape( (map_msg.info.height, map_msg.info.width)) self.permissible_region = np.zeros_like(array_255, dtype=bool) # Numpy array of dimension (map_msg.info.height, map_msg.info.width), with values 0: not permissible, 1: permissible self.permissible_region[array_255 == 0] = 1 # Globally initialize the particles # Publish particle filter state # Used to create a tf between the map and the laser for visualization self.pub_tf = tf.TransformBroadcaster() # Publishes the expected pose self.pose_pub = rospy.Publisher(PUBLISH_PREFIX + "/inferred_pose", PoseStamped, queue_size=1) # Publishes a subsample of the particles self.particle_pub = rospy.Publisher(PUBLISH_PREFIX + "/particles", PoseArray, queue_size=1) # Publishes the most recent laser scan self.pub_laser = rospy.Publisher(PUBLISH_PREFIX + "/scan", LaserScan, queue_size=1) # Publishes the path of the car self.pub_odom = rospy.Publisher(PUBLISH_PREFIX + "/odom", Odometry, queue_size=1) rospy.sleep(1.0) self.initialize_global() # An object used for resampling self.resampler = ReSampler(self.particles, self.weights, self.state_lock) # An object used for applying sensor model self.sensor_model = SensorModel( scan_topic, laser_ray_step, exclude_max_range_rays, max_range_meters, map_msg, self.particles, self.weights, car_length, self.state_lock, ) # An object used for applying kinematic motion model self.motion_model = KinematicMotionModel( motor_state_topic, servo_state_topic, speed_to_erpm_offset, speed_to_erpm_gain, steering_angle_to_servo_offset, steering_angle_to_servo_gain, car_length, self.particles, self.state_lock, ) self.permissible_x, self.permissible_y = np.where( self.permissible_region == 1) # Parameters/flags/vars for global localization self.global_localize = False self.global_suspend = False self.ents = None self.ents_sum = 0.0 self.noisy_cnt = 0 # number of regions to partition. Simulation: 25, Real: 5. self.NUM_REGIONS = 25 # number of updates for regional localization. Simulation 5, Real: 3. self.REGIONAL_ROUNDS = 5 self.regions = [] self.click_mode = True self.debug_mode = False if self.debug_mode: self.global_localize = True # True when doing global localization # precompute regions. Each region is represented by arrays of x, y indices on the map region_size = len(self.permissible_x) / self.NUM_REGIONS idx = np.argsort(self.permissible_y) # column-major _px, _py = self.permissible_x[idx], self.permissible_y[idx] for i in range(self.NUM_REGIONS): self.regions.append(( _px[i * region_size:(i + 1) * region_size], _py[i * region_size:(i + 1) * region_size], )) # Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info # three different reactions to click on rviz self.click_sub = rospy.Subscriber( "/initialpose", PoseWithCovarianceStamped, self.clicked_pose_cb, queue_size=1, ) self.init_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.reinit_cb, queue_size=1) rospy.wait_for_message(scan_topic, LaserScan) print("Initialization complete")
class UWBDataFusion(object): def __init__(self, init_p_occ=0.8, min_count=5): self.tmap_bin = None self.tmap = None self.tmap_aug = None self.pmap = None self.map_properties = {} self.tmap_loaded = False self.gplane_loaded = False self.gp_plane_map = None self.gp_emap = None self.gp_emap_prop = {} self.gp_ec = 0 # gamma model self.gamma_model_loaded = False self.gamma_rbin_ids = [] self.gamma_avgs = [] self.gamma_stds = [] self.count = [] # intensity spectrum of ground self.ground_spectrum = {} self.min_count = min_count # probability model variables self.init_p_occ = init_p_occ # saftey factor - the largest this should be is 1 - init_p_occ self.saftey_value = 1 - self.init_p_occ # sensor model self.sensor_model = SensorModel() def set_tmap(self, tmap, map_properties): ''' Set the traversability map Input: Tmap, npfloat array ti is [0-1] negative value indicates unknown ti ''' self.map_properties = map_properties # initial traversability map self.tmap = tmap # determine the occupancy map - binary mask where ti value is 1 self.tmap_bin = (tmap == 1)*1 # init probability of occupancy map self.pmap = self.tmap_bin*self.init_p_occ # init tmap_aug self.agument_tmap() # set the initilised flag self.tmap_loaded = True def set_gplane(self, gplane, gplane_emap, gp_emap_prop, gp_ec): ''' Input: -gplane GroundPlane object -gplane_emap numpy elevation map, in cm -gp_emap_prop dictionary containing the map map_properties -gp_ec the error code for the elevation map in m; max_z value + 1 ''' self.gp_plane_map = gplane self.gp_emap = gplane_emap self.gp_emap_prop = gp_emap_prop self.gp_ec = gp_ec self.gplane_loaded = True assert('cell_size' in gp_emap_prop) def update_pocc(self, uwb_mask_pcd, uwb_scan ): if self.init() == False: return # update ground spectrum self.determine_ground_spec(uwb_mask_pcd) # evaluate measurement likelyhood self.sensor_model.prob_sen(uwb_mask_pcd, self.ground_spectrum, uwb_scan) # update pocc self.bayesian_update(uwb_mask_pcd) def determine_ground_spec(self, uwb_mask_pcd, radar_range_res=0.3): ''' Determine the ground spectrum vector for the current ground Input -uwb_pcd, pointcloud representation of where the uwb_beamwidth is, relative to the map frame Returns: Dictionary, contianing the ground intensity values indexed by the range bin Note not all range bins will have valid intensity values, if unknown -1 -1 will be present ''' if self.init() == False: return 0 ranges = uwb_mask_pcd.channels[0].values # angle scalar for the pdd angle_scalars = self._evaluate_angle_scalar(uwb_mask_pcd) # determine the sensor vector s_vec = self._evalaute_sensor_vector(uwb_mask_pcd) # get the incident angle mask angle_mask = self._evaluate_incident_angle(uwb_mask_pcd, s_vec) # determine the incident angles and weights for each range gate angle_weights_gated = {} # range gate the angles and determine the expected ground return for each range bin for i, angle in enumerate(angle_mask): if i > 0: bin_index = int(m.floor(ranges[i]/radar_range_res)) if bin_index in angle_weights_gated: angle_weights_gated[bin_index].append((angle, angle_scalars[i])) else: angle_weights_gated[bin_index] = [(angle, angle_scalars[i])] # for each range bin evaluate the expected ground return [iavg, imax, imin] # if unknown -1 intensity_spectrum = {} for bin_index, angles_weights in angle_weights_gated.iteritems(): # only request intensity if there are valid incident angles intensity_spectrum[bin_index] = self.expected_ground_return(angles_weights, bin_index) self.ground_spectrum = intensity_spectrum return 1 def bayesian_update(self, uwb_pcd): points = uwb_pcd.points ps_occ = uwb_pcd.channels[9].values ps_empty = uwb_pcd.channels[10].values region = uwb_pcd.channels[5].values for i, point in enumerate(points): if i > 0: if region[i] != -1 and ps_occ[i] != -1 and ps_empty[i] != -1: [row_index, col_index] = cartesian_2_index(point.x, point.y, self.map_properties) pri = self.pmap[row_index, col_index] # bayesian update pocc = (ps_occ[i]*pri)/(ps_occ[i]*pri + ps_empty[i]*(1-pri)) assert(pocc <= 1) self.pmap[row_index, col_index] = pocc # update tmap self.agument_tmap() def agument_tmap(self): # apply a saftey factor to evaluate the safe probability of occupancy map pocc_sf = self.pmap + self.saftey_value # apply piece wise function to saturate the probabilities procc_sf = np.piecewise(pocc_sf, [pocc_sf <= 1, pocc_sf > 1], [lambda pocc_sf:pocc_sf , 1]) # calculate the augmented traversability map self.tmap_aug = self.tmap*procc_sf def init(self): return self.tmap_loaded == True and self.gplane_loaded == True and self.gamma_model_loaded == True def update_ground_model(self, rbin_ids, gamma_avgs, gamma_stds, counts): ''' Update the ground gamma model. A gamma value will only be present if there has been at least one update to a range bin ''' self.gamma_model_loaded = True self.gamma_rbin_ids = rbin_ids self.gamma_avgs = gamma_avgs self.gamma_stds = gamma_stds self.count = counts def _evalaute_sensor_vector(self, uwb_fov_mask): ''' Evaluates the senor vector, which is a vector from the sensor orging to a point on the ground which is projected from the x,y positions of the UWB_FOV_Mask to the ground plane evaluate using 1x1m grids. Given UWB_MASK format sensor_msgs.Pointcloud, encoded with meta data such as state. evaluate the sensing vector and update the range channel of the UWB_MASK to the true_range ''' points = uwb_fov_mask.points sensor_pos = points[0] S = np.array([sensor_pos.x, sensor_pos.y, sensor_pos.z]) zero_vector = np.array([0.0,0.0,0.0]) s_list = [] s_list.append(zero_vector) for i, point in enumerate(points): if i > 0: # project the mask point into the ground using the higher resolution elevation map x_loc = point.x y_loc = point.y [row_index, col_index] = cartesian_2_index(x_loc, y_loc, self.gp_emap_prop) # determine the z projection of the mask point in meters z_proj = self.gp_emap[row_index, col_index]/100.0 # check z value of cell is known if z_proj == self.gp_ec: s_list.append(zero_vector) else: assert(z_proj != self.gp_ec) # therefore the point on the ground is G = np.array([x_loc, y_loc, z_proj]) # determine the sensing vector - originates from ground # vector from ground point to sensor origin v_gs = S - G s_list.append(v_gs) return s_list def _evaluate_incident_angle(self, uwb_fov_mask, s_list): ''' Evaluates the incident angle between the sensing vector and the ground Input: sensing vector, list of numpy vectors the first sensing vector is a zero vector, othe rest of the data vectors that orginate from the ground and travel to the radar position , unless the elevation value is unknown ''' zero_vector = np.array([0.0,0.0,0.0]) points = uwb_fov_mask.points i_angles = [] # to align the angles vector with all the points i_angles.append(-1) for i, point in enumerate(points): if i > 0: sensor_vector = s_list[i] if (sensor_vector != zero_vector).all(): x_loc = point.x y_loc = point.y normal_vector = self.gp_plane_map.get_normal(x_loc, y_loc) # check normal vector is known if (normal_vector == zero_vector).all(): i_angles.append(-1) else: aspect_angle = angle_between_vectors(normal_vector, sensor_vector) incident_angle = m.fabs(90 - aspect_angle) i_angles.append(incident_angle) else: i_angles.append(-1) return i_angles def _evaluate_angle_scalar(self, uwb_fov_mask, uwb_beamwidth=40): ''' Evaluates a scalar that compensates for the intensity variation in the UWB radar beam pattern ''' RAD2DEG = 180.0/m.pi points = uwb_fov_mask.points azi_channel = uwb_fov_mask.channels[1].values angle_scalars = [] # place holder scalar for the sensor origin point angle_scalars.append(0) for i, point in enumerate(points): if i > 0: # get azimuth position in degrees azimuth = azi_channel[i]*RAD2DEG a_scalar = inverse_parabola(azimuth, uwb_beamwidth) angle_scalars.append(a_scalar) return angle_scalars @staticmethod def eval_incident_angle_sum(angle_weights): ''' input: angle_weights, a list of tupples containing an incident angle and an assoicated weight angle, in degrees -1 if the incident angles is unknown returns: sum of the incident angle multiplied by its weight or -1 if there are invalid anlges in the list of angle_weights ''' incident_angle_sum = 0 DEG2RAD = m.pi/180.0 for i, angle_weight in enumerate(angle_weights): angle, weight = angle_weight[0]*DEG2RAD, angle_weight[1] if angle_weight[0] == -1: return -1 assert(weight > 0) incident_angle_sum = incident_angle_sum + m.sin(angle)*weight assert(m.sin(angle)*weight > 0) return incident_angle_sum def expected_ground_return(self, angle_weights, range_bin_index): ''' input: angle_weights, a list of tupples containing an incident angle and an assoicated weight angle, in degrees -1 if the incident angles is unknown ''' incident_angle_sum = self.eval_incident_angle_sum(angle_weights) # check the current mask does not lie in an unknown region if incident_angle_sum == -1: return [-1, -1] max_index = len(self.count) - 1 index_offset = self.gamma_rbin_ids[0] # index used to acess the gamma vectors which are aligned to particular range bins index = range_bin_index - index_offset # check there are measurements are in a valid range bin if index > max_index or index < 0: return [-1, -1] else: if self.count[index] >= self.min_count: mean_gamma = self.gamma_avgs[index] std_gamma = self.gamma_stds[index] ground_return = mean_gamma*incident_angle_sum ground_return_max = (mean_gamma + std_gamma)*incident_angle_sum return [ground_return, ground_return_max] else: return [-1, -1]
config = { 'sensor_sigma': 0.1, 'sensor_parameters': [ { 'delta': [1, 0], 'orientation': 0, 'fov': 180, }, { 'delta': [-1, 0], 'orientation': 180, 'fov': 180, }, ] } sm = SensorModel(config) config1 = { 'start': [0, 0], 'sigma_initial': [[0.1, 0], [0, 0.1]], 'id': 1 } motion1 = RobotMotion(config1) motion1.pos = np.array([0, 0]) motion1.th = math.radians(0) motion2 = RobotMotion(config1) motion2.pos = np.array([10, 0]) print(sm.get_measurement(motion1, 0, motion2))
class ParticleFilter: ''' Monte Carlo Localization based on odometry and a laser scanner. ''' def __init__(self): # Get parameters self.particle_filter_frame = rospy.get_param("~particle_filter_frame") self.MAX_PARTICLES = int(rospy.get_param("~num_particles")) self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.ANGLE_STEP = int(rospy.get_param("~angle_step")) self.DO_VIZ = bool(rospy.get_param("~do_viz")) # MCL algorithm self.iters = 0 self.lidar_initialized = False self.odom_initialized = False self.map_initialized = False self.last_odom_pose = None # last received odom pose self.last_stamp = None self.laser_angles = None self.downsampled_angles = None self.state_lock = Lock() # paritcles self.inferred_pose = None self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.particle_indices = np.arange(self.MAX_PARTICLES) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel() # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) self.initialize_global() # map self.permissible_region = None self.SHOW_FINE_TIMING = False # these topics are for visualization self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size=1) self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size=1) self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan", LaserScan, queue_size=1) self.path_pub = rospy.Publisher('/pf/viz/path', Path, queue_size=1) self.path = Path() if self.PUBLISH_ODOM: self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size=1) # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param( "~scan_topic", "/scan"), LaserScan, self.callback_lidar, queue_size=1) self.odom_sub = rospy.Subscriber(rospy.get_param( "~odom_topic", "/odom"), Odometry, self.callback_odom, queue_size=1) self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) print "Finished initializing, waiting on messages..." def initialize_global(self): ''' Spread the particle distribution over the permissible region of the state space. ''' print("GLOBAL INITIALIZATION") # randomize over grid coordinate space self.state_lock.acquire() print('Waiting for map..') while not self.sensor_model.map_set: continue self.map_initialized = True self.permissible_region = self.sensor_model.permissible_region self.map = self.sensor_model.map self.map_info = self.sensor_model.map_info permissible_x, permissible_y = np.where(self.permissible_region == 1) indices = np.random.randint(0, len(permissible_x), size=self.MAX_PARTICLES) permissible_states = np.zeros((self.MAX_PARTICLES, 3)) permissible_states[:, 0] = permissible_y[indices] permissible_states[:, 1] = permissible_x[indices] permissible_states[:, 2] = np.random.random( self.MAX_PARTICLES) * np.pi * 2.0 Utils.map_to_world(permissible_states, self.map_info) self.particles = permissible_states self.weights[:] = 1.0 / self.MAX_PARTICLES self.state_lock.release() def initialize_particles_pose(self, pose): ''' Initialize particles in the general region of the provided pose. ''' print "SETTING POSE" print pose self.state_lock.acquire() self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) self.particles[:, 0] = pose.position.x + np.random.normal( loc=0.0, scale=0.5, size=self.MAX_PARTICLES) self.particles[:, 1] = pose.position.y + np.random.normal( loc=0.0, scale=0.5, size=self.MAX_PARTICLES) self.particles[:, 2] = Utils.quaternion_to_angle( pose.orientation) + np.random.normal( loc=0.0, scale=0.4, size=self.MAX_PARTICLES) self.state_lock.release() def clicked_pose(self, msg): ''' Receive pose messages from RViz and initialize the particle distribution in response. ''' print('clicked_pose') if isinstance(msg, PointStamped): self.initialize_particles_pose(msg.pose.pose) #self.initialize_global() elif isinstance(msg, PoseWithCovarianceStamped): self.initialize_particles_pose(msg.pose.pose) def publish_tf(self, pose, stamp=None): """ Publish a tf for the car. This tells ROS where the car is with respect to the map. """ if stamp == None: stamp = rospy.Time.now() # this may cause issues with the TF tree. If so, see the below code. self.pub_tf.sendTransform( (pose[0], pose[1], 0), tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp, "/laser", "/map") # also publish odometry to facilitate getting the localization pose if self.PUBLISH_ODOM: odom = Odometry() odom.header = Utils.make_header("/map", stamp) odom.pose.pose.position.x = pose[0] odom.pose.pose.position.y = pose[1] odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2]) self.odom_pub.publish(odom) """ Our particle filter provides estimates for the "laser" frame since that is where our laser range estimates are measure from. Thus, We want to publish a "map" -> "laser" transform. However, the car's position is measured with respect to the "base_link" frame (it is the root of the TF tree). Thus, we should actually define a "map" -> "base_link" transform as to not break the TF tree. """ # Get map -> laser transform. map_laser_pos = np.array((pose[0], pose[1], 0)) map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2])) # Apply laser -> base_link transform to map -> laser transform # This gives a map -> base_link transform laser_base_link_offset = (0.265, 0, 0) map_laser_pos -= np.dot( tf.transformations.quaternion_matrix( tf.transformations.unit_vector(map_laser_rotation))[:3, :3], laser_base_link_offset).T # Publish transform self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp, self.particle_filter_frame, "/map") def publish_particles(self, particles): pa = PoseArray() pa.header = Utils.make_header("map") pa.poses = Utils.particles_to_poses(particles) self.particle_pub.publish(pa) def publish_scan(self, angles, ranges): ls = LaserScan() ls.header = Utils.make_header("laser", stamp=self.last_stamp) ls.angle_min = np.min(angles) ls.angle_max = np.max(angles) ls.angle_increment = np.abs(angles[0] - angles[1]) ls.range_min = 0 ls.range_max = np.max(ranges) ls.ranges = ranges self.pub_fake_scan.publish(ls) def visualize(self): ''' Publish various visualization messages. ''' if not self.DO_VIZ: return if self.pose_pub.get_num_connections() > 0 and isinstance( self.inferred_pose, np.ndarray): ps = PoseStamped() ps.header = Utils.make_header("map") ps.pose.position.x = self.inferred_pose[0] ps.pose.position.y = self.inferred_pose[1] ps.pose.orientation = Utils.angle_to_quaternion( self.inferred_pose[2]) self.pose_pub.publish(ps) if self.iters % 10 == 0: self.path.header = ps.header self.path.poses.append(ps) self.path_pub.publish(self.path) if self.particle_pub.get_num_connections() > 0: if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES: # randomly downsample particles proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights) # proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES) self.publish_particles(self.particles[proposal_indices, :]) else: self.publish_particles(self.particles) if self.pub_fake_scan.get_num_connections() > 0 and isinstance( self.sensor_model.particle_scans, np.ndarray): # generate the scan from the point of view of the inferred position for visualization inferred_scans = self.sensor_model.get_scans( self.inferred_pose[None, :]) self.publish_scan(self.downsampled_angles, inferred_scans[0, :]) def callback_lidar(self, msg): ''' Initializes reused buffers, and stores the relevant laser scanner data for later use. ''' if not isinstance(self.laser_angles, np.ndarray): print "...Received first LiDAR message" self.laser_angles = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) self.downsampled_angles = np.copy( self.laser_angles[0::self.ANGLE_STEP]).astype(np.float32) #self.viz_queries = np.zeros((self.downsampled_angles.shape[0],3), dtype=np.float32) #self.viz_ranges = np.zeros(self.downsampled_angles.shape[0], dtype=np.float32) #print self.downsampled_angles.shape[0] self.downsampled_ranges = np.array(msg.ranges[::self.ANGLE_STEP]) self.lidar_initialized = True def callback_odom(self, msg): ''' Store deltas between consecutive odometry messages in the coordinate space of the car. Odometry data is accumulated via dead reckoning, so it is very inaccurate on its own. ''' position = np.array( [msg.pose.pose.position.x, msg.pose.pose.position.y]) orientation = Utils.quaternion_to_angle(msg.pose.pose.orientation) pose = np.array([position[0], position[1], orientation]) if isinstance(self.last_odom_pose, np.ndarray): rot = Utils.rotation_matrix(-self.last_odom_pose[2]) delta = np.array([position - self.last_odom_pose[0:2]]).transpose() local_delta = (rot * delta).transpose() # changes in x,y,theta in local coordinate system of the car self.odometry_data = np.array([ local_delta[0, 0], local_delta[0, 1], orientation - self.last_odom_pose[2] ]) self.last_odom_pose = pose self.last_stamp = msg.header.stamp self.odom_initialized = True else: print "...Received first Odometry message" self.last_odom_pose = pose # this topic is slower than lidar, so update every time we receive a message self.update() def expected_pose(self, particles): # returns the expected value of the pose given the particle distribution return np.dot(particles.transpose(), self.weights) def MCL(self, odom, scans): # Implement the MCL algorithm # using the sensor model and the motion model # # Make sure you include some way to initialize # your particles, ideally with some sort # of interactive interface in rviz proposal_indices = np.random.choice(self.particle_indices, self.MAX_PARTICLES, p=self.weights) proposal_distribution = self.particles[proposal_indices, :] if self.SHOW_FINE_TIMING: t_propose = time.time() #print("resample",self.expected_pose(proposal_distribution)) # compute the motion model to update the proposal distribution proposal_distribution = self.motion_model.evaluate( proposal_distribution, odom) if self.SHOW_FINE_TIMING: t_motion = time.time() #print("motion model:", self.expected_pose(proposal_distribution)) # compute the sensor model self.weights = self.sensor_model.evaluate(proposal_distribution, scans) if self.SHOW_FINE_TIMING: t_sensor = time.time() # normalize importance weights self.weights /= np.sum(self.weights) #print("sensor model:", self.expected_pose(proposal_distribution)) if self.SHOW_FINE_TIMING: t_norm = time.time() t_total = (t_norm - t) / 100.0 if self.SHOW_FINE_TIMING and self.iters % 10 == 0: print "MCL: propose: ", np.round((t_propose-t)/t_total, 2), "motion:", np.round((t_motion-t_propose)/t_total, 2), \ "sensor:", np.round((t_sensor-t_motion)/t_total, 2), "norm:", np.round((t_norm-t_sensor)/t_total, 2) # save the particles self.particles = proposal_distribution #print(self.expected_pose(self.particles)) # Publish a transformation frame between the map # and the particle_filter_frame. def update(self): ''' Apply the MCL function to update particle filter state. Ensures the state is correctly initialized, and acquires the state lock before proceeding. ''' if self.lidar_initialized and self.odom_initialized and self.map_initialized: if self.state_lock.locked(): print("Concurrency error avoided") else: self.state_lock.acquire() self.timer.tick() self.iters += 1 t1 = time.time() scans = np.copy(self.downsampled_ranges).astype(np.float32) odom = np.copy(self.odometry_data) self.odometry_data = np.zeros(3) # run the MCL update algorithm self.MCL(odom, scans) # compute the expected value of the robot pose self.inferred_pose = self.expected_pose(self.particles) self.state_lock.release() t2 = time.time() # publish transformation frame based on inferred pose self.publish_tf(self.inferred_pose, self.last_stamp) # this is for tracking particle filter speed ips = 1.0 / (t2 - t1) self.smoothing.append(ips) if self.iters % 10 == 0: print "iters per sec:", int( self.timer.fps()), " possible:", int( self.smoothing.mean()) self.visualize()
class ParticleFilter(object): """ This is the main particle filter object. """ def __init__(self, map_file='../data/map/wean.dat'): self.map = Map(map_file) #self.map.display_gaussian([], 'Gaussian Blurred Map') self._number_particles = 1000 self.particles = list() self._particle_probabilities = [] for _ in range(0, self._number_particles): print 'Initializing particle', _ particle = Particle(self.map) #particle.x = 4080 + np.random.normal(scale=35) #particle.y = 3980 + np.random.normal(scale=15) #particle.theta = math.pi + 0.1 + np.random.normal(scale=.1) self.particles.append(particle) self._particle_probabilities.append(1) self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1) self._sensor_model = SensorModel(self.map) self._ranges = [] def log(self, file_handle): line = list() for particle in self.particles: loc = str(particle.x) + ',' + str(particle.y) line.append(loc) file_handle.write(';'.join(line)) file_handle.write('\n') def display(self): self.map.display(self.particles, ranges=self._ranges) def update(self, line): """ Update step. Reading is a single reading (i.e. line) from the log file Could be either an odometry or laser reading """ measurement_type = line[0] # O = odometry, L = laser scan measurements = np.fromstring(line[2:], sep=' ') odometry = measurements[0:3] time_stamp = measurements[-1] # last variable for particle in self.particles: self._motion_model.update(particle, odometry, time_stamp) if measurement_type == "L": odometry_laser = measurements[ 3:6] # coordinates of the laser in standard odometry frame self._ranges = measurements[ 6:-1] # exclude last variable, the time stamp self._particle_probabilities = list( ) # unnormalized sensor model probabilities of the particles for particle in self.particles: self._particle_probabilities.append( self._sensor_model.get_probability(particle, self._ranges)) argsorted_probabilities = np.argsort( -np.asarray(self._particle_probabilities)) self.particles[argsorted_probabilities[0]].debug = True self.particles[argsorted_probabilities[1]].debug = True self.particles[argsorted_probabilities[2]].debug = True def _resample(self): """ Resamples the particles given unnormalized particle probabilites """ particle_probabilities = np.asarray( self._particle_probabilities) / sum( self._particle_probabilities) # normalize indices = np.random.choice(range(0, self._number_particles), size=self._number_particles, replace=True, p=particle_probabilities) indices.sort() previous_index = -1 new_particles = list() for index in indices: if index != previous_index: new_particles.append(self.particles[index]) else: new_particles.append(deepcopy(self.particles[index])) previous_index = index self.particles = new_particles
def __init__(self): # using locks so threads can't access self.particles at the same time (avoiding race condition) self.lock = Lock() self.lock.acquire() # Get parameters self.particle_filter_frame = \ rospy.get_param("~particle_filter_frame") # should be '/base_link_pf' in sim and '/base_link' on car # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel() # Topics self.odom_topic = rospy.get_param("~odom_topic") self.scan_topic = rospy.get_param("~scan_topic") self.particle_marker_topic = '/particle_marker' self.click_topic = '/move_base_simple/goal' self.pose_array_topic = '/pose_array' self.inferred_pose_topic = '/inferred_pose' self.error_x_topic = '/error_x' self.error_y_topic = '/error_y' self.error_t_topic = '/error_t' # Subscribers and Publishers rospy.Subscriber( self.odom_topic, Odometry, self.odom_callback) # '/odom' for sim and 'vesc/odom' for car rospy.Subscriber(self.scan_topic, LaserScan, self.scan_callback) self.tf_broadcaster = TransformBroadcaster() # Listening to clicks in rviz and publishing particle markers to rviz rospy.Subscriber(self.click_topic, PoseStamped, self.click_callback) self.particle_marker_pub = rospy.Publisher(self.particle_marker_topic, Marker, queue_size=1) self.pose_array_pub = rospy.Publisher(self.pose_array_topic, PoseArray, queue_size=1) self.inferred_pose_pub = rospy.Publisher(self.inferred_pose_topic, Marker, queue_size=1) self.error_x_pub = rospy.Publisher(self.error_x_topic, Float32, queue_size=1) self.error_y_pub = rospy.Publisher(self.error_y_topic, Float32, queue_size=1) self.error_t_pub = rospy.Publisher(self.error_t_topic, Float32, queue_size=1) self.rate = rospy.Rate(30) # should be greater than 20 Hz # Implement the MCL algorithm # using the sensor model and the motion model # # Make sure you include some way to initialize # your particles, ideally with some sort # of interactive interface in rviz # # Publish a transformation frame between the map # and the particle_filter_frame. # Initializing particles and weights self.num_particles = rospy.get_param('~num_particles') # currently intializing all particles as origin self.particles = np.zeros((self.num_particles, 3)) # all particles are initially equally likely self.weights = np.full((self.num_particles), 1. / self.num_particles) # for error calculations self.groundtruth = [0, 0, 0] # send drive commands to test in sim self.drive_pub = rospy.Publisher('/drive', AckermannDriveStamped, queue_size=1) self.drive_msg = AckermannDriveStamped() self.drive_msg.drive.steering_angle = 0 self.drive_msg.drive.speed = 1 if self.SIM == False: # don't toggle on driving if working on real robot self.DRIVE = False self.publish_transform() self.lock.release()
class ParticleFilter: # toggle robot driving in a circle in the simulator DRIVING = True # set to True if working in the simulator, else False SIM = True """ *** IF WORKING ON THE REAL ROBOT: *** - in params.yaml: - change particle_filter_frame from 'base_link_pf' to 'base_link' - change odom_topic from '/odom' to '/vesc/odom' - in localize.launch: - uncomment out region that initializes map server to use stata basement map - use SIM = False and DRIVING = False """ def __init__(self): # using locks so threads can't access self.particles at the same time (avoiding race condition) self.lock = Lock() self.lock.acquire() # Get parameters self.particle_filter_frame = \ rospy.get_param("~particle_filter_frame") # should be '/base_link_pf' in sim and '/base_link' on car # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel() # Topics self.odom_topic = rospy.get_param("~odom_topic") self.scan_topic = rospy.get_param("~scan_topic") self.particle_marker_topic = '/particle_marker' self.click_topic = '/move_base_simple/goal' self.pose_array_topic = '/pose_array' self.inferred_pose_topic = '/inferred_pose' self.error_x_topic = '/error_x' self.error_y_topic = '/error_y' self.error_t_topic = '/error_t' # Subscribers and Publishers rospy.Subscriber( self.odom_topic, Odometry, self.odom_callback) # '/odom' for sim and 'vesc/odom' for car rospy.Subscriber(self.scan_topic, LaserScan, self.scan_callback) self.tf_broadcaster = TransformBroadcaster() # Listening to clicks in rviz and publishing particle markers to rviz rospy.Subscriber(self.click_topic, PoseStamped, self.click_callback) self.particle_marker_pub = rospy.Publisher(self.particle_marker_topic, Marker, queue_size=1) self.pose_array_pub = rospy.Publisher(self.pose_array_topic, PoseArray, queue_size=1) self.inferred_pose_pub = rospy.Publisher(self.inferred_pose_topic, Marker, queue_size=1) self.error_x_pub = rospy.Publisher(self.error_x_topic, Float32, queue_size=1) self.error_y_pub = rospy.Publisher(self.error_y_topic, Float32, queue_size=1) self.error_t_pub = rospy.Publisher(self.error_t_topic, Float32, queue_size=1) self.rate = rospy.Rate(30) # should be greater than 20 Hz # Implement the MCL algorithm # using the sensor model and the motion model # # Make sure you include some way to initialize # your particles, ideally with some sort # of interactive interface in rviz # # Publish a transformation frame between the map # and the particle_filter_frame. # Initializing particles and weights self.num_particles = rospy.get_param('~num_particles') # currently intializing all particles as origin self.particles = np.zeros((self.num_particles, 3)) # all particles are initially equally likely self.weights = np.full((self.num_particles), 1. / self.num_particles) # for error calculations self.groundtruth = [0, 0, 0] # send drive commands to test in sim self.drive_pub = rospy.Publisher('/drive', AckermannDriveStamped, queue_size=1) self.drive_msg = AckermannDriveStamped() self.drive_msg.drive.steering_angle = 0 self.drive_msg.drive.speed = 1 if self.SIM == False: # don't toggle on driving if working on real robot self.DRIVE = False self.publish_transform() self.lock.release() def odom_callback(self, odom_data): ''' whenever you get odometry data, use the motion model to update the particle positions determine the "average" particle pose and publish that transform :param odom_data: odometry data (type - Odometry) :return: no return, but updates self.particles positions and publishes transform ''' self.lock.acquire() #start = time() x = odom_data.pose.pose.position.x y = odom_data.pose.pose.position.y q_w = odom_data.pose.pose.orientation.w q_z = odom_data.pose.pose.orientation.z euler = transformations.euler_from_quaternion( np.array([0, 0, q_z, q_w])) self.groundtruth = [x, y, euler] self.particles = self.motion_model.evaluate( self.particles, self.motion_model.get_delta_x(odom_data), noise=0.2) self.publish_transform() #end = time() #print('odom_callback time:', end-start) self.lock.release() def scan_callback(self, scan_data): ''' whenever you get sensor data, use the sensor model to compute the particle probabilities. Then resample the particles based on these probabilities determine the "average" particle pose and publish that transform :param scan_data: scan data (type - LaserScan) :return: no return, but updates self.particles by resampling particles and publishes transform ''' self.lock.acquire() probabilities = self.sensor_model.evaluate(self.particles, scan_data, downsample=20, sim=self.SIM) # if map wasn't initialized in sensor_model, probabilites=None if isinstance(probabilities, type(None)): self.lock.release() rospy.loginfo('no probabilities to use in scan_callback') return smooth_p = np.power(probabilities, [1 / 3.] * len(probabilities)) self.weights = smooth_p / np.sum( smooth_p) # normalize weights so they sum to 1 # resample particles based on weights self.particles = self.resample_particles() self.publish_transform() self.lock.release() def resample_particles(self): ''' reseample particles based on their probability weights allow repeats of particles; to not allow, use replace=False ''' # choose with indices in particles array to keep using probabilities as weights indices = np.random.choice(self.num_particles, self.num_particles, p=self.weights) # return particles with chosen indices return self.particles[indices] def publish_transform(self): ''' anytime the particles are updated, determine the 'average' particle pose and broadcast that transform :return: no return, but broadcast transform between 'map' frame and self.particle_filter_frame ''' x_avg, y_avg, theta_avg = self.avg_pose() q = transformations.quaternion_from_euler(0, 0, theta_avg) # broadcast transformation self.tf_broadcaster.sendTransform( (x_avg, y_avg, 0), q, rospy.Time.now(), self.particle_filter_frame, 'map') self.publish_pose_array() #self.publish_particle_markers() # Driving in the simulator if self.DRIVING: self.drive_pub.publish(self.drive_msg) # publish inferred position in rviz as arrow if self.inferred_pose_pub.get_num_connections > 0: a = Marker() a.header.frame_id = 'map' a.type = Marker.ARROW a.color = ColorRGBA(0, 1, 1, 1) a.scale.x = 0.5 a.scale.y = 0.1 a.scale.z = 0.1 q = transformations.quaternion_from_euler(0, 0, theta_avg) quat = Quaternion() quat.x = q[0] quat.y = q[1] quat.z = q[2] quat.w = q[3] a.pose.position.x = x_avg a.pose.position.y = y_avg a.pose.orientation = quat self.inferred_pose_pub.publish(a) # for error plots try: x, y, theta = self.groundtruth x_error = x_avg - x y_error = y_avg - y t_error = theta_avg - theta[2] self.error_x_pub.publish(x_error) self.error_y_pub.publish(y_error) self.error_t_pub.publish(t_error) except Exception as e: rospy.loginfo(e) def avg_pose(self): ''' determine the average particle pose ** have to use mean of circular quantities for theta ** distirbution might be multi modal :param: no params, but uses instance variable self.particles :return: average particle pose as [x, y, theta] ''' x = np.mean(self.particles[:, 0]) y = np.mean(self.particles[:, 1]) # circular mean is angle between average x component and y component of the angles data theta = np.arctan2(np.mean(np.sin(self.particles[:, 2])), np.mean(np.cos(self.particles[:, 2]))) return [x, y, theta] def click_callback(self, pose_data): ''' use click in rviz to initialize particles :param pose_data: pose of rviz click (type - PoseStamped) use 2D Nav Goal to guess car location :return: no return, but updates self.particles ''' self.lock.acquire() x = pose_data.pose.position.x y = pose_data.pose.position.y q = pose_data.pose.orientation theta = transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])[2] # random points from normal distribution centered at [x, y, theta] self.particles = np.random.normal([x, y, theta], 1, (self.num_particles, 3)) self.weights = np.full((self.num_particles), 1. / self.num_particles) self.publish_pose_array() #self.publish_particle_markers() self.lock.release() def publish_particle_markers(self): ''' publish particles as markers in rviz ''' # if something is listening for marker publications if self.particle_marker_pub.get_num_connections > 0: m = Marker() m.header.frame_id = 'map' m.type = Marker.POINTS #m.color = ColorRGBA(1, 0, 0, 1) m.scale.x = 0.2 m.scale.y = 0.2 m.scale.z = 0.2 for i in range(len(self.particles)): p = self.particles[i] w = self.weights[i] m.points.append(Point(p[0], p[1], 0)) m.colors.append(self.prob_to_color(w)) self.particle_marker_pub.publish(m) def prob_to_color(self, prob): ''' convert a probability to a color red is high probability orange yellow green cyan light blue blue purple magenta black is low :param prob: probability between 0 and 1 :return: ColorRBGA corresponding to prob ''' m = max(self.weights) # max weight if prob > m * 0.9: return ColorRGBA(1, 0, 0, 1) if prob > m * 0.8: return ColorRGBA(1, 0.5, 0, 1) if prob > m * 0.7: return ColorRGBA(1, 1, 0, 1) if prob > m * 0.6: return ColorRGBA(0, 1, 0, 1) if prob > m * 0.5: return ColorRGBA(0, 1, 1, 1) if prob > m * 0.4: return ColorRGBA(0, 0.5, 1, 1) if prob > m * 0.3: return ColorRGBA(0, 0, 1, 1) if prob > m * 0.2: return ColorRGBA(0.5, 0, 1, 1) if prob > m * 0.1: return ColorRGBA(1, 0, 1, 1) else: return ColorRGBA(0, 0, 0, 1) def publish_pose_array(self): ''' publish arrows in rviz for to visualize particles ''' particles = np.copy(self.particles) if self.pose_array_topic > 0: pa = PoseArray() pa.header.frame_id = 'map' for p in particles[:20]: pose = Pose() pose.position = Point(p[0], p[1], 0) q = transformations.quaternion_from_euler(0, 0, p[2]) quat = Quaternion() quat.x = q[0] quat.y = q[1] quat.z = q[2] quat.w = q[3] pose.orientation = quat pa.poses.append(pose) self.pose_array_pub.publish(pa)
def setUp(self): self.sensor_model = SensorModel(map) self.particle = Particle(map) self.particle.x = 250 self.particle.y = 250 self.particle.theta = 0
def __init__(self): # Get parameters self.particle_filter_frame = rospy.get_param("~particle_filter_frame") self.MAX_PARTICLES = int(rospy.get_param("~num_particles")) self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.ANGLE_STEP = int(rospy.get_param("~angle_step")) self.DO_VIZ = bool(rospy.get_param("~do_viz")) # MCL algorithm self.iters = 0 self.lidar_initialized = False self.odom_initialized = False self.map_initialized = False self.last_odom_pose = None # last received odom pose self.last_stamp = None self.laser_angles = None self.downsampled_angles = None self.state_lock = Lock() # paritcles self.inferred_pose = None self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.particle_indices = np.arange(self.MAX_PARTICLES) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel() # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) self.initialize_global() # map self.permissible_region = None self.SHOW_FINE_TIMING = False # these topics are for visualization self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size=1) self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size=1) self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan", LaserScan, queue_size=1) self.path_pub = rospy.Publisher('/pf/viz/path', Path, queue_size=1) self.path = Path() if self.PUBLISH_ODOM: self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size=1) # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param( "~scan_topic", "/scan"), LaserScan, self.callback_lidar, queue_size=1) self.odom_sub = rospy.Subscriber(rospy.get_param( "~odom_topic", "/odom"), Odometry, self.callback_odom, queue_size=1) self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) print "Finished initializing, waiting on messages..."
class World(): CLASS_NONTARGET = 0 CLASS_WILDLIFE = 1 CLASS_MINE = 2 CLASS_BENIGN = 3 def __init__(self, config): # Create a world self.config = config self.surface_level = 0 self.fully_set_seed = rospy.get_param('~fully_set_seed') def init_world(self, seed1, do_test=True): self.seed = seed1 random.seed(seed1) # for repeatable trials if self.fully_set_seed: # True for plotting not training np.random.seed(seed1 + 1) # for repeatable trials print('init_world seed1 test: random.randint(25,50)', random.randint(25, 50)) self.init_world_once(do_test) while not self.is_connected(): print("World graph is disconnected. Reinitializing...") self.init_world_once(do_test) def init_world_once(self, do_test=True): # create a blank world, PRM style num_nodes = self.config["num_nodes"] connection_radius = self.config["connection_radius"] environment_size = self.config["environment_size"] self.sensor_range = self.config["sensor_range"] # vertices self.vertices_surface = [] self.vertices = [] count = 0 for vertex_idx in xrange(num_nodes): x = random.uniform(0, environment_size[0]) y = random.uniform(0, environment_size[1]) for z in [-10, self.surface_level]: v = Vertex(x, y, z, count) count += 1 self.vertices.append(v) if z == self.surface_level: self.vertices_surface.append(True) else: self.vertices_surface.append(False) # edges, stored as a matrix indexed as [vertex_start, vertex_end] self.num_nodes = len( self.vertices ) #doubled the input num_nodes in creating two layers of vertices instead of one self.edge_matrix = [None] * self.num_nodes self.edge_adjacency_idx_lists = [None] * self.num_nodes self.edge_adjacency_edge_lists = [None] * self.num_nodes for vertex_start_idx in xrange(self.num_nodes): self.edge_matrix[vertex_start_idx] = [None] * self.num_nodes self.edge_adjacency_idx_lists[vertex_start_idx] = [] self.edge_adjacency_edge_lists[vertex_start_idx] = [] for vertex_end_idx in xrange(self.num_nodes): cost = distance(self.vertices[vertex_start_idx], self.vertices[vertex_end_idx]) if vertex_start_idx != vertex_end_idx and cost <= connection_radius and not ( self.vertices_surface[vertex_start_idx] and self.vertices_surface[vertex_end_idx]): exists = True else: exists = False edge = Edge(vertex_start_idx, vertex_end_idx, cost, exists) self.edge_matrix[vertex_start_idx][vertex_end_idx] = edge if exists: self.edge_adjacency_idx_lists[vertex_start_idx].append( vertex_end_idx) self.edge_adjacency_edge_lists[vertex_start_idx].append( edge) if do_test: self.test_indices() #self.vertex_target_idx = self.create_target_idx() #single robot # Define prior distribution self.num_classes = self.config["num_classes"] self.prob_of_class0 = self.config["prob_of_class0"] self.prob_of_other_classes = (1 - self.prob_of_class0) / ( self.num_classes - 1) self.prior = np.zeros(self.num_classes) #p_y0, p_y1, ... for i in range(self.num_classes): if i == 0: self.prior[i] = self.prob_of_class0 else: self.prior[i] = self.prob_of_other_classes self.randomize_targets() self.original_classes_y = copy.copy(self.classes_y) # Define single random drop-off location (on surface) per world temp = random.randint(0, len(self.vertices) - 1) while self.vertices_surface[temp] != True: temp = random.randint(0, len(self.vertices) - 1) self.drop_off_idx = temp ''' # Set all class 2 vertices to is_armed = True self.is_armed_array = np.zeros(self.num_nodes, dtype=bool) #default, False for all for v in xrange(len(self.classes_y)): if self.classes_y[v] == 2: #it is a mine self.is_armed_array[v] = True ''' ''' # Vertex classes # Each vertex has a number between 0 and n, which represents the class of the vertex num_v_y0 = int(self.num_nodes*self.prob_of_class0) num_v_other = self.num_nodes - num_v_y0 num_v_each = num_v_other/(self.num_classes-1) classes_y = [] classes_y.extend([0]*num_v_y0) # add the non-target vertex classes for i in range(1,self.num_classes): classes_y.extend([i]*num_v_each) # add the target vertex classes # G round truth vertex classes random.shuffle(classes_y) #shuffle the list to randomize location of targets ''' self.comms_range = self.config["comms_range"] self.vertices_in_comms_range = self.generateCommsRangeVertices() # Setup sensor model self.sensor_model = SensorModel(self.config, self.num_nodes, self) ''' #old likelihood function we have replaced # it did not account for beyond sensor range -> 0 def robot_env_observations(self, vertex_robot, vertex_target): # need robot location, robot sensor model, and the actual target location # Do we want to have a field of view or assume 360 degree awareness for now? # Do we really need to get closer to a target ever once we are in range? # Where do we check if we have already sensed the particular target? # wouldn't want to just keep circling back to the same target and reporting it sensor_range = self.config["sensor_range"] distance_to_target = distance(vertex_robot, vertex_target) if distance_to_target <= sensor_range: prob_true_pos = 0.95 - 0.01*distance_to_target #prob_false_pos = 0.2 - 0.01*distance_to_target # True positive if random.random() <= prob_true_pos: robot_senses_target = True return vertex_target else: # False negative robot_senses_target = False else: # False positive if random.random() <= prob_false_pos: robot_senses_target = True # for vertex in range pick a random one vertex_near_robot = return vertex_near_robot # near defined as within sensor_range else: # True negative robot_senses_target = False if robot_senses_target: return True else: return False ''' def is_open_set_empty(self, open_set): for i in open_set: if i == True: return False return True def is_connected(self): num_vertices = len(self.vertices) open_set = [False] * num_vertices closed_set = [False] * num_vertices open_set[ 0] = True # Adding a vertex to open set, as our starting point while not self.is_open_set_empty(open_set): # find a vertex in open_set v_current = open_set.index(True) # remove it from the open set open_set[v_current] = False # add it to the closed set closed_set[v_current] = True # get the set of neighbours neighbours = self.edge_adjacency_edge_lists[v_current] # expand neighbouring nodes for e in neighbours: v_next = e.vertex_end_idx if not closed_set[v_next] == True: # If not in closed set, add to open set open_set[v_next] = True if False in closed_set: # you haven't visited at least one vertex return False return True def reset_world(self): print('original_classes_y', self.original_classes_y) self.classes_y = copy.copy(self.original_classes_y) print('self.classes_y after reset', self.classes_y) def reset_seed(self): # DO NOT USE DURING RUN OF ALG # For use when plotting so identical trees have same random numbers random.seed(self.seed) # for repeatable trials if self.fully_set_seed: #Safety check: This should always be true when this function is called because its true in plot_results.launch np.random.seed(self.seed + 1) # for repeatable trials def target_inclusion_test(self): target_inclusion_test_list = np.zeros( self.num_classes - 1) #minus 1 for the void of target class (i.e. 0) for vertex_class in self.classes_y: if vertex_class == 1: target_inclusion_test_list[0] = 1 elif vertex_class == 2: target_inclusion_test_list[1] = 1 elif vertex_class == 3: target_inclusion_test_list[2] = 1 if 0 in target_inclusion_test_list: #one target type not included, so we should randomize again return False return True def randomize_targets(self): # Randomize targets and ensure that all three target types are present #print('world.randomize_targets test, seed test: random.randint(25,50)', random.randint(25,50)) #test_count = 0 #print('world.randomize_targets test NUM NODES', self.num_nodes) #print('world.randomize_targets test len(self.vertices)',len(self.vertices)) # Vertex classes - ground truth all_targets_included = False while not all_targets_included: #test_count+= 1 self.classes_y = np.array([]) for i in range(self.num_nodes): #print('world.randomize_targets in-loop test, seed test: random.randint(25,50)', random.randint(25,50)) self.classes_y = np.append( self.classes_y, np.random.choice(a=len(self.prior), p=self.prior)) all_targets_included = self.target_inclusion_test() #print('world.randomize_targets TEST_COUNT', test_count) #print("classes_y",self.classes_y.shape) #self.original_classes = self.classes_y ''' def inclusive_randomize_targets(self): while not self.target_inclusion_test(): self.randomize_targets() ''' def disarm_target(self, vertex_idx, scorer): if self.classes_y[vertex_idx] == World.CLASS_MINE: self.classes_y[ vertex_idx] = World.CLASS_NONTARGET # Now is class 0 because target removed scorer.action_reward( vertex_idx, World.CLASS_MINE ) # Generate score #use reward_action_to_target from scorer return True return False def pickup_target(self, vertex_idx, scorer): if self.classes_y[vertex_idx] == World.CLASS_BENIGN: self.classes_y[ vertex_idx] = World.CLASS_NONTARGET # Now is class 0 because target removed scorer.action_reward(vertex_idx, World.CLASS_BENIGN) # Generate score return True return False def dropoff_target(self, vertex_idx, scorer, state): if self.drop_off_idx == vertex_idx: # Now is class 0 because target removed scorer.dropoff_reward( state.picked_up_target_count) # Generate score return True return False def report_target(self, vertex_idx, scorer, is_at_surface, is_in_comms): # Check with Graeme DONE #if classes_y[vertex_idx] == World.CLASS_WILDLIFE: response = scorer.submit_target(vertex_idx, World.CLASS_WILDLIFE, is_at_surface, is_in_comms) # Generate score print('reported something', response) if response == scorer.RESPONSE_CORRECT: print('report is correct') self.classes_y[ vertex_idx] = World.CLASS_NONTARGET # Now is class 0 because target removed return response def generateCommsRangeVertices(self): idx_list = [] for vertex in self.vertices: if distance_to_base( vertex ) < self.comms_range and vertex.position.z > self.surface_level - 0.0001: idx_list.append(vertex.vertex_idx) return idx_list #list of indices of vertices in comms range (and at surface) def robot_env_observations(self, vertex_robot_idx): #use for single target case #likelihoods = self.sensor_model.all_likelihoods(vertex_robot_idx, self.vertex_target_idx) #return a single observation, z based on the probability distribution #return np.random.choice(a=len(self.vertices)+1, p=likelihoods) v_in_range = np.array([], dtype=int) z_array = np.array([], dtype=int) for v in xrange(self.num_nodes): if self.sensor_model.distances[vertex_robot_idx][ v] < self.sensor_range: likelihoods = self.sensor_model.all_likelihoods( self.classes_y[v]) v_in_range = np.append(v_in_range, v) z_array = np.append( z_array, np.random.choice(a=self.num_classes, p=likelihoods)) return z_array, v_in_range def create_target_idx(self): #pick random vertex random_vertex_idx = random.randrange(len(self.vertices)) #random_vertex = self.vertices[random_vertex_idx] return random_vertex_idx def pickle_graph(self): graph = GraphPickle(self.vertices, self.edge_matrix) return pickle.dumps(graph) def test_indices(self): # sanity check that graph was constructed correctly rospy.loginfo("running graph test") num_nodes = len(self.vertices) if num_nodes <= 0: rospy.logerr("empty graph created") for vertex_idx in xrange(num_nodes): if self.vertices[vertex_idx].vertex_idx != vertex_idx: rospy.logerr("vertex index test failed") for vertex_start_idx in xrange(num_nodes): for vertex_end_idx in xrange(num_nodes): #print(self.edge_matrix[vertex_start_idx][vertex_end_idx].vertex_start_idx,vertex_start_idx) if self.edge_matrix[vertex_start_idx][ vertex_end_idx].vertex_start_idx != vertex_start_idx: rospy.logerr("edge index start test failed") if self.edge_matrix[vertex_start_idx][ vertex_end_idx].vertex_end_idx != vertex_end_idx: rospy.logerr("edge index end test failed") if self.edge_matrix[vertex_start_idx][vertex_end_idx].exists: if vertex_end_idx not in self.edge_adjacency_idx_lists[ vertex_start_idx]: rospy.logerr("edge_adjacency_idx_lists test failed") def get_edges_out(self, vertex_idx): return self.edge_matrix[vertex_idx] def plot_world(self, ax, target_belief): rospy.loginfo("plotting world") num_nodes = len(self.vertices) # plot possible edges for vertex_start_idx in xrange(num_nodes): for vertex_end_idx in xrange(num_nodes): if self.edge_matrix[vertex_start_idx][ vertex_end_idx].potentially_exists: vertex_start = self.vertices[vertex_start_idx] vertex_end = self.vertices[vertex_end_idx] ax.plot([vertex_start.position.x, vertex_end.position.x], [vertex_start.position.y, vertex_end.position.y], ':k', zorder=1) # plot actual edges for vertex_start_idx in xrange(num_nodes): for vertex_end_idx in xrange(num_nodes): if self.edge_matrix[vertex_start_idx][vertex_end_idx].exists: vertex_start = self.vertices[vertex_start_idx] vertex_end = self.vertices[vertex_end_idx] ax.plot([vertex_start.position.x, vertex_end.position.x], [vertex_start.position.y, vertex_end.position.y], '-r', zorder=10) # plot vertices xs = [] ys = [] size_list = [] for vertex_idx in xrange(num_nodes): xs.append(self.vertices[vertex_idx].position.x) ys.append(self.vertices[vertex_idx].position.y) #p = target_belief.prob_dist[vertex_idx] # Single target case #sp = 2+10*p #size_list.append(sp) #ax.scatter(xs, ys, s=5, zorder=20) # print(size_list) self.h_scatter_plot = ax.scatter(xs, ys, s=size_list, zorder=20) ''' # Plot goal location as blue star pos_target = self.vertices[self.vertex_target_idx].position ax.plot(pos_target.x, pos_target.y, 'b*', markersize=20, zorder=10) ''' # Plot targets as blue stars for v in xrange(len(self.classes_y)): if self.classes_y[ v]: #it's not 0, hence is a target of some kind (1,2,or 3) pos_target = self.vertices[v].position ax.plot(pos_target.x, pos_target.y, 'b*', markersize=20, zorder=10) # labels, axis etc ax.set_xlabel('x') ax.set_ylabel('y') ax.set_xlim(0, self.config["environment_size"][0]) ax.set_ylim(0, self.config["environment_size"][1]) ax.grid(True) ax.set_aspect('equal', 'box') def plot_world_update(self, ax, target_belief): # Check with Graeme: This is for size and color changes, and is not updated to account for prob_dist[v] being a list # Should I use this for anything? num_nodes = len(self.vertices) size_list = [] colors_list = [] max_p = 0 for vertex_idx in xrange(num_nodes): p = target_belief.prob_dist[vertex_idx] if p >= max_p: max_p = p for vertex_idx in xrange(num_nodes): p = target_belief.prob_dist[vertex_idx] sp = 1 + 100 * (p / max_p) size_list.append(sp) p /= max_p c = [0, p, 1 - p] colors_list.append(c) # size_list_round = [round(p, 1) for p in size_list] # print(size_list_round) self.h_scatter_plot.set_sizes(size_list) self.h_scatter_plot.set_color(colors_list)
class ParticleFilter: """ Implements particle filtering for estimating the state of the robot car """ def __init__( self, publish_tf, n_particles, n_viz_particles, odometry_topic, motor_state_topic, servo_state_topic, scan_topic, laser_ray_step, exclude_max_range_rays, max_range_meters, speed_to_erpm_offset, speed_to_erpm_gain, steering_angle_to_servo_offset, steering_angle_to_servo_gain, car_length, ): """ Initializes the particle filter publish_tf: Whether or not to publish the tf. Should be false in sim, true on real robot n_particles: The number of particles n_viz_particles: The number of particles to visualize odometry_topic: The topic containing odometry information motor_state_topic: The topic containing motor state information servo_state_topic: The topic containing servo state information scan_topic: The topic containing laser scans laser_ray_step: Step for downsampling laser scans exclude_max_range_rays: Whether to exclude rays that are beyond the max range max_range_meters: The max range of the laser speed_to_erpm_offset: Offset conversion param from rpm to speed speed_to_erpm_gain: Gain conversion param from rpm to speed steering_angle_to_servo_offset: Offset conversion param from servo position to steering angle steering_angle_to_servo_gain: Gain conversion param from servo position to steering angle car_length: The length of the car """ self.PUBLISH_TF = publish_tf # The number of particles in this implementation, the total number of particles is constant. self.N_PARTICLES = n_particles self.N_VIZ_PARTICLES = n_viz_particles # The number of particles to visualize # Cached list of particle indices self.particle_indices = np.arange(self.N_PARTICLES) # Numpy matrix of dimension N_PARTICLES x 3 self.particles = np.zeros((self.N_PARTICLES, 3)) # Numpy matrix containing weight for each particle self.weights = np.ones(self.N_PARTICLES) / float(self.N_PARTICLES) # A lock used to prevent concurrency issues. You do not need to worry about this self.state_lock = Lock() self.tfl = tf.TransformListener( ) # Transforms points between coordinate frames # Get the map map_msg = rospy.wait_for_message(MAP_TOPIC, OccupancyGrid) self.map_info = map_msg.info # Save info about map for later use # Create numpy array representing map for later use array_255 = np.array(map_msg.data).reshape( (map_msg.info.height, map_msg.info.width)) self.permissible_region = np.zeros_like(array_255, dtype=bool) # Numpy array of dimension (map_msg.info.height, map_msg.info.width), with values 0: not permissible, 1: permissible self.permissible_region[array_255 == 0] = 1 # Globally initialize the particles # Publish particle filter state # Used to create a tf between the map and the laser for visualization self.pub_tf = tf.TransformBroadcaster() # Publishes the expected pose self.pose_pub = rospy.Publisher(PUBLISH_PREFIX + "/inferred_pose", PoseStamped, queue_size=1) # Publishes a subsample of the particles self.particle_pub = rospy.Publisher(PUBLISH_PREFIX + "/particles", PoseArray, queue_size=1) # Publishes the most recent laser scan self.pub_laser = rospy.Publisher(PUBLISH_PREFIX + "/scan", LaserScan, queue_size=1) # Publishes the path of the car self.pub_odom = rospy.Publisher(PUBLISH_PREFIX + "/odom", Odometry, queue_size=1) rospy.sleep(1.0) self.initialize_global() # An object used for resampling self.resampler = ReSampler(self.particles, self.weights, self.state_lock) # An object used for applying sensor model self.sensor_model = SensorModel( scan_topic, laser_ray_step, exclude_max_range_rays, max_range_meters, map_msg, self.particles, self.weights, car_length, self.state_lock, ) # An object used for applying kinematic motion model self.motion_model = KinematicMotionModel( motor_state_topic, servo_state_topic, speed_to_erpm_offset, speed_to_erpm_gain, steering_angle_to_servo_offset, steering_angle_to_servo_gain, car_length, self.particles, self.state_lock, ) self.permissible_x, self.permissible_y = np.where( self.permissible_region == 1) # Parameters/flags/vars for global localization self.global_localize = False self.global_suspend = False self.ents = None self.ents_sum = 0.0 self.noisy_cnt = 0 # number of regions to partition. Simulation: 25, Real: 5. self.NUM_REGIONS = 25 # number of updates for regional localization. Simulation 5, Real: 3. self.REGIONAL_ROUNDS = 5 self.regions = [] self.click_mode = True self.debug_mode = False if self.debug_mode: self.global_localize = True # True when doing global localization # precompute regions. Each region is represented by arrays of x, y indices on the map region_size = len(self.permissible_x) / self.NUM_REGIONS idx = np.argsort(self.permissible_y) # column-major _px, _py = self.permissible_x[idx], self.permissible_y[idx] for i in range(self.NUM_REGIONS): self.regions.append(( _px[i * region_size:(i + 1) * region_size], _py[i * region_size:(i + 1) * region_size], )) # Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info # three different reactions to click on rviz self.click_sub = rospy.Subscriber( "/initialpose", PoseWithCovarianceStamped, self.clicked_pose_cb, queue_size=1, ) self.init_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.reinit_cb, queue_size=1) rospy.wait_for_message(scan_topic, LaserScan) print("Initialization complete") def initialize_global(self): """ Initialize the particles to cover the map """ self.state_lock.acquire() # Get in-bounds locations permissible_x, permissible_y = np.where(self.permissible_region == 1) # The number of particles at each location, each with different rotation angle_step = 4 # The sample interval for permissible states permissible_step = angle_step * len( permissible_x) / self.particles.shape[0] # Indices of permissible states to use indices = np.arange(0, len(permissible_x), permissible_step)[:(self.particles.shape[0] / angle_step)] # Proxy for the new particles permissible_states = np.zeros((self.particles.shape[0], 3)) # Loop through permissible states, each iteration drawing particles with # different rotation for i in range(angle_step): idx_start = i * (self.particles.shape[0] / angle_step) idx_end = (i + 1) * (self.particles.shape[0] / angle_step) permissible_states[idx_start:idx_end, 0] = permissible_y[indices] permissible_states[idx_start:idx_end, 1] = permissible_x[indices] permissible_states[idx_start:idx_end, 2] = i * (2 * np.pi / angle_step) # Transform permissible states to be w.r.t world utils.map_to_world(permissible_states, self.map_info) # Reset particles and weights self.particles[:, :] = permissible_states[:, :] self.weights[:] = 1.0 / self.particles.shape[0] # self.publish_particles(self.particles) self.state_lock.release() def reinit_cb(self, msg): if self.debug_mode: self.global_localize = True def publish_tf(self, pose, stamp=None): """ Publish a tf between the laser and the map This is necessary in order to visualize the laser scan within the map pose: The pose of the laser w.r.t the map stamp: The time at which this pose was calculated, defaults to None - resulting in using the time at which this function was called as the stamp """ if stamp is None: stamp = rospy.Time.now() try: # Lookup the offset between laser and odom delta_off, delta_rot = self.tfl.lookupTransform( "/laser_link", "/odom", rospy.Time(0)) # Transform offset to be w.r.t the map off_x = delta_off[0] * np.cos(pose[2]) - delta_off[1] * np.sin( pose[2]) off_y = delta_off[0] * np.sin(pose[2]) + delta_off[1] * np.cos( pose[2]) # Broadcast the tf self.pub_tf.sendTransform( (pose[0] + off_x, pose[1] + off_y, 0.0), quaternion_from_euler( 0, 0, pose[2] + euler_from_quaternion(delta_rot)[2]), stamp, "/odom", "/map", ) except (tf.LookupException ) as e: # Will occur if odom frame does not exist print(e) print("failed to find odom") def expected_pose(self): """ Uses cosine and sine averaging to more accurately compute average theta To get one combined value use the dot product of position and weight vectors https://en.wikipedia.org/wiki/Mean_of_circular_quantities returns: np array of the expected pose given the current particles and weights """ cosines = np.cos(self.particles[:, 2]) sines = np.sin(self.particles[:, 2]) theta = np.arctan2(np.dot(sines, self.weights), np.dot(cosines, self.weights)) position = np.dot(self.particles[:, 0:2].transpose(), self.weights) position[0] += (car_length / 2) * np.cos(theta) position[1] += (car_length / 2) * np.sin(theta) return np.array((position[0], position[1], theta), dtype=np.float) def clicked_pose_cb(self, msg): """ Reinitialize particles and weights according to the received initial pose Applies Gaussian noise to each particle's pose HINT: use Utils.quaternion_to_angle() Remember to use vectorized computation! msg: '/initialpose' topic. RVIZ publishes a message to this topic when you specify an initial pose using its GUI returns: nothing """ if self.click_mode: self.state_lock.acquire() pose = msg.pose.pose print("SETTING POSE") VAR_X = 0.001 VAR_Y = 0.001 VAR_THETA = 0.001 quat = pose.orientation theta = utils.quaternion_to_angle(quat) x = pose.position.x y = pose.position.y self.particles[:, 0] = np.random.normal(x, VAR_X, self.particles.shape[0]) self.particles[:, 1] = np.random.normal(y, VAR_Y, self.particles.shape[0]) self.particles[:, 2] = np.random.normal(theta, VAR_THETA, self.particles.shape[0]) self.weights.fill(1 / self.N_PARTICLES) self.state_lock.release() def visualize(self): """ Visualize the current state of the filter (1) Publishes a tf between the map and the laser. Necessary for visualizing the laser scan in the map (2) Publishes the most recent laser measurement. Note that the frame_id of this message should be '/laser' (3) Publishes a PoseStamped message indicating the expected pose of the car (4) Publishes a subsample of the particles (use self.N_VIZ_PARTICLES). Sample so that particles with higher weights are more likely to be sampled. """ self.state_lock.acquire() self.inferred_pose = self.expected_pose() if isinstance(self.inferred_pose, np.ndarray): if self.PUBLISH_TF: self.publish_tf(self.inferred_pose) ps = PoseStamped() ps.header = utils.make_header("map") ps.pose.position.x = self.inferred_pose[0] ps.pose.position.y = self.inferred_pose[1] ps.pose.orientation = utils.angle_to_quaternion( self.inferred_pose[2]) if self.pose_pub.get_num_connections() > 0: self.pose_pub.publish(ps) if self.pub_odom.get_num_connections() > 0: odom = Odometry() odom.header = ps.header odom.pose.pose = ps.pose self.pub_odom.publish(odom) if self.particle_pub.get_num_connections() > 0: if self.particles.shape[0] > self.N_VIZ_PARTICLES: # randomly downsample particles proposal_indices = np.random.choice(self.particle_indices, self.N_VIZ_PARTICLES, p=self.weights) self.publish_particles(self.particles[proposal_indices, :]) else: self.publish_particles(self.particles) if self.pub_laser.get_num_connections() > 0 and isinstance( self.sensor_model.last_laser, LaserScan): self.sensor_model.last_laser.header.frame_id = "/laser" self.sensor_model.last_laser.header.stamp = rospy.Time.now() self.pub_laser.publish(self.sensor_model.last_laser) self.state_lock.release() def publish_particles(self, particles): """ Helper function for publishing a pose array of particles particles: To particles to publish """ pa = PoseArray() pa.header = utils.make_header("map") pa.poses = utils.particles_to_poses(particles) self.particle_pub.publish(pa) def set_particles(self, region): self.state_lock.acquire() # Get in-bounds locations permissible_x, permissible_y = region assert len(permissible_x) >= self.particles.shape[0] # The number of particles at each location, each with different rotation angle_step = 4 # The sample interval for permissible states permissible_step = angle_step * len( permissible_x) / self.particles.shape[0] # Indices of permissible states to use indices = np.arange(0, len(permissible_x), permissible_step)[:(self.particles.shape[0] / angle_step)] # Proxy for the new particles permissible_states = np.zeros((self.particles.shape[0], 3)) # Loop through permissible states, each iteration drawing particles with # different rotation for i in range(angle_step): idx_start = i * (self.particles.shape[0] / angle_step) idx_end = (i + 1) * (self.particles.shape[0] / angle_step) permissible_states[idx_start:idx_end, 0] = permissible_y[indices] permissible_states[idx_start:idx_end, 1] = permissible_x[indices] permissible_states[idx_start:idx_end, 2] = i * (2 * np.pi / angle_step) # Transform permissible states to be w.r.t world utils.map_to_world(permissible_states, self.map_info) # Reset particles and weights self.particles[:, :] = permissible_states[:, :] self.weights[:] = 1.0 / self.particles.shape[0] self.publish_particles(self.particles) self.state_lock.release() def global_localization(self): self.sensor_model.reset_confidence() candidate_num = self.particles.shape[0] / self.NUM_REGIONS regional_particles = [] regional_weights = [] for i in range(self.NUM_REGIONS): self.set_particles(self.regions[i]) cnt_updates = 0 while cnt_updates < self.REGIONAL_ROUNDS: # each cluster update if self.sensor_model.do_resample: # if weights updated by sensor model self.resampler.resample_low_variance() self.visualize() self.sensor_model.do_resample = False cnt_updates += 1 self.state_lock.acquire() candidate_indices = np.argsort(self.weights)[-candidate_num:] candidates = self.particles[candidate_indices] candidates_weights = self.weights[candidate_indices] regional_particles.append(candidates.copy()) # save the particles regional_weights.append( candidates_weights) # save the particles' weights self.state_lock.release() self.state_lock.acquire() self.particles[:] = np.concatenate(regional_particles) self.weights[:] = np.concatenate(regional_weights) self.weights /= self.weights.sum() self.global_localize = False self.global_suspend = True self.sensor_model.do_confidence_update = True self.state_lock.release() def suspend_update(self): self.state_lock.acquire() ent = -((self.weights * np.log2(self.weights)).sum()) print("entropy ==", ent) self.ents_sum += ent self.ents.put(ent) if self.ents.qsize() > 10: self.ents_sum -= self.ents.get() self.state_lock.release() if self.ents_sum / 10 >= 8: self.global_suspend = False elif 2.0 < ent < 3.0: self.resampler.resample_low_variance()
parser.add_argument('--output', default='results') parser.add_argument('--num_particles', default=3000, type=int) parser.add_argument('--visualize', action='store_false') parser.add_argument('--path_to_raycast_map', default='raycast_map.npy') args = parser.parse_args() src_path_map = args.path_to_map src_path_log = args.path_to_log os.makedirs(args.output, exist_ok=True) map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = args.num_particles # X_bar = init_particles_random(num_particles, occupancy_map) X_bar = init_particles_freespace(num_particles, occupancy_map) """ Monte Carlo Localization Algorithm : Main Loop """ if not os.path.exists(args.path_to_raycast_map): print("Start pre-computing the ray cast map") raycast_map = sensor_model.precompute_raycast() np.save(args.path_to_raycast_map, raycast_map) print('Pre-compute of ray casting done!') else: raycast_map = np.load(args.path_to_raycast_map)