def initialize_global(self): print "INITIAL GLOBAL START" self.state_lock.acquire() # Use self.permissible_region to get in-bounds states # Uniformally sample from in-bounds regions # Convert map samples (which are in pixels) to world samples (in meters/radians) # Take a look at utils.py # Update particles in place # Update weights in place so that all particles have the same weight and the # sum of the weights is one. # YOUR CODE HERE perm_pos = np.argwhere(self.permissible_region == 1) perm_x = perm_pos[0] perm_y = perm_pos[1] # 1000 [N_PARTICLES] random angles between 0 and 2pi radians perm_rand_theta = np.random.uniform(0, 2.0 * np.pi, self.N_PARTICLES) # choose self.N_particles indices from 0,1,2,...,len(perm_x)) which is ~(296640,) but depends on map # choose 1000 indices from np.arange(0, 296440) using uniform distribution uniform_rand_indices = np.random.choice( len(perm_x), self.N_PARTICLES) # no 3rd param means uniform map_samples = np.zeros_like(self.particles) # shape (1000, 3) map_samples[:, 0] = perm_x[uniform_rand_indices] map_samples[:, 1] = perm_y[uniform_rand_indices] map_samples[:, 2] = perm_rand_theta Utils.map_to_world(map_samples, self.map_info) self.particles[:] = map_samples[:] self.weights[:] = 1.0 / self.N_PARTICLES self.state_lock.release() print "INITIALIZE GLOBAL COMPLETE"
def advance_to_next_goal(self): self.currentPlanWaypointIndex += 1 self.roi.tape_seen = False next_segment = plan[self.currentPlanWaypointIndex] next_goal_point = next_segment[0] gx = next_goal_point[0] gy = next_goal_point[1] gt = next_segment[1] self.theta_weight = next_segment[3] print("self.plan[current], ", next_segment) # delta_x = next_segment[1][0] - next_segment[0][0] # delta_y = next_segment[1][1] - next_segment[0][1] # # theta_radians = math.atan2(delta_y, delta_x) # theta_radians = wrap_pi_pi_number(theta_radians) # gt = 0#theta_radians goalPixelSpace = self.dtype([[gx, gy, gt]]) Utils.map_to_world(goalPixelSpace, self.map_info) self.goal = self.dtype( [goalPixelSpace[0][0], goalPixelSpace[0][1], goalPixelSpace[0][2]]) print("Next goal set to: ", self.goal, "which is", gx, ", ", gy)
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_global(self): self.state_lock.acquire() # get particles particles = np.zeros((self.N_PARTICLES, 3)) # Use self.permissible_region to get in-bounds states permissible_x, permissible_y = np.where(self.permissible_region == 1) # Uniformally sample from in-bounds regions particles_x = np.random.choice(permissible_x, self.N_PARTICLES) particles_y = np.random.choice(permissible_y, self.N_PARTICLES) #set particles x and y particles[:, 0] = particles_x particles[:, 1] = particles_y #convert to world samples and add to particles Utils.map_to_world(particles, self.map_info) # Update particles in place self.particles[:] = particles[:] # Update weights in place so that all particles have the same weight and the # sum of the weights is one. self.weights[:] = np.ones(self.N_PARTICLES) / float(self.N_PARTICLES) # YOUR CODE HERE self.state_lock.release()
def initialize_global(self): self.state_lock.acquire() # PP - priting for debug # print 'inside initialize_global' # Use self.permissible_region to get in-bounds states # Uniformally sample from in-bounds regions # Convert map samples (which are in pixels) to world samples (in meters/radians) # Take a look at utils.py # Update particles in place # Update weights in place so that all particles have the same weight and the # sum of the weights is one. # YOUR CODE HERE ''' ## PP - COPIED FROM SENSORMODEL.PY SKELETON CODE ## :P print 'Creating particles' angle_step = 25 particles = np.zeros((angle_step * permissible_x.shape[0],3)) for i in xrange(angle_step): particles[i*(particles.shape[0]/angle_step):(i+1)*(particles.shape[0]/angle_step),0] = permissible_y[:] particles[i*(particles.shape[0]/angle_step):(i+1)*(particles.shape[0]/angle_step),1] = permissible_x[:] particles[i*(particles.shape[0]/angle_step):(i+1)*(particles.shape[0]/angle_step),2] = i*(2*np.pi / angle_step) Utils.map_to_world(particles, map_info) weights = np.ones(particles.shape[0]) / float(particles.shape[0]) ''' # PP - use self.permissible_region - its an array of dimession map.height X map.width # PP - how to find which indices have 1 ?? - use np.where # PP - valid locations will be 2D array of Nx2 of valid pixels valid_locations_x, valid_locations_y = np.array( np.where(self.permissible_region == 1)) #print valid_locations_x, valid_locations_y # PP - uniformly sample from valid locations on map # PP - what will be theta here? sample uniformly between 0 to 2*pi? #length = np.random.uniform(0, 1) valid_locations_poses = np.zeros((self.N_PARTICLES, 3)) #print valid_locations_poses rand_idx = np.random.choice(len(valid_locations_x), self.N_PARTICLES) valid_locations_poses[:, 0] = valid_locations_x[rand_idx] valid_locations_poses[:, 1] = valid_locations_y[rand_idx] #valid_locations_poses[:, 2] = np.random.uniform(0, 2*np.pi) valid_locations_poses[:, 2] = np.random.random( self.N_PARTICLES) * np.pi * 2.0 #print valid_locations_poses # PP - convert map to world , valid_locations_poses should be Nx3 array Utils.map_to_world(valid_locations_poses, self.map_info) # PP - update particles self.particles[:, 0] = valid_locations_poses[:, 0] self.particles[:, 1] = valid_locations_poses[:, 1] self.particles[:, 2] = valid_locations_poses[:, 2] #print self.particles # PP - update weights self.weights[:] = 1.0 / self.N_PARTICLES self.state_lock.release()
def initialize_global(self): permissible_x, permissible_y = np.where(self.permissible_region == 1) angle_step = 4 #indices = np.random.randint(0, len(permissible_x), size=self.particles.shape[0]/angle_step) step = 4*len(permissible_x)/self.particles.shape[0] indices = np.arange(0, len(permissible_x), step)[:(self.particles.shape[0]/4)] permissible_states = np.zeros((self.particles.shape[0],3)) for i in xrange(angle_step): permissible_states[i*(self.particles.shape[0]/angle_step):(i+1)*(self.particles.shape[0]/angle_step),0] = permissible_y[indices] permissible_states[i*(self.particles.shape[0]/angle_step):(i+1)*(self.particles.shape[0]/angle_step),1] = permissible_x[indices] permissible_states[i*(self.particles.shape[0]/angle_step):(i+1)*(self.particles.shape[0]/angle_step),2] = i*(2*np.pi / angle_step)#np.random.random(self.particles.shape[0]) * np.pi * 2.0 Utils.map_to_world(permissible_states, self.map_info) self.particles[:,:] = permissible_states[:,:] self.weights[:] = 1.0 / self.particles.shape[0]
def generate_heatmap(map_resp, scan_msg): width = map_resp.map.info.width height = map_resp.map.info.height heatmap = np.zeros((height, width)) map_data = np.array(map_resp.map.data).reshape(heatmap.shape) # particles (in map pixels) where map is permissible pixel_particles = np.zeros((width * height, 3)) # populate all indices pixel_particles[:, 0] = np.tile(np.arange(width), height) pixel_particles[:, 1] = np.repeat(np.arange(height), width) # filter particles based on map pixel_particles = pixel_particles.reshape(height, width, 3)[map_data >= 0] # downsample particles downsampled_indices = np.arange(0, pixel_particles.shape[0], HEATMAP_XY_STEP) pixel_particles = pixel_particles[downsampled_indices] # transform particles into "world" frame (in meters, etc) meter_particles = np.copy(pixel_particles) utils.map_to_world(meter_particles, map_resp.map.info) num_particles = meter_particles.shape[0] weights = np.ones(num_particles) / float(num_particles) sensor_model = SensorModel(map_resp.map, meter_particles, weights) max_weights = np.zeros(num_particles) # iterate through all thetas for theta in np.linspace(-np.pi, np.pi, HEATMAP_NUM_THETA): meter_particles[:, 2] = theta sensor_model.lidar_cb(scan_msg) # update the max weights np.maximum.reduce((weights, max_weights), out=max_weights) max_weights /= np.sum(max_weights) # populate heatmap according to max_weights for i in range(num_particles): px, py = pixel_particles[i, 0:2] heatmap[int(py), int(px)] = max_weights[i] rospy.logerr("done computing heatmap") return heatmap
def initialize_global(self): self.state_lock.acquire() # Use self.permissible_region to get in-bounds states # Uniformally sample from in-bounds regions # Convert map samples (which are in pixels) to world samples (in meters/radians) # Take a look at utils.py # Update particles in place # Update weights in place so that all particles have the same weight and the # sum of the weights is one. # YOUR CODE HERE # get particles particles = np.zeros((self.N_PARTICLES, 3)) # Use self.permissible_region to get in-bounds states permissible_x, permissible_y = np.where(self.permissible_region == 1) num_perms = permissible_x.shape[0] # length of permissible array # Uniformally sample from in-bounds regions # Choosing n_particles number of permissible locations for the particles perm_iter = int(np.ceil(num_perms / self.N_PARTICLES) + 1) particles[:, 0] = permissible_y[0:num_perms:perm_iter] particles[:, 1] = permissible_x[0:num_perms:perm_iter] # Assigning theta of particles to random orientation between -pi and pi particles[:, 2] = -np.pi + 2 * np.pi * np.random.rand( self.N_PARTICLES, 1)[:, 0] #convert to world samples and add to particles Utils.map_to_world(particles, self.map_info) #np.set_printoptions(threshold=sys.maxsize) #np.set_printoptions(suppress=True) # Update particles in place self.particles[:] = particles[:] # Update weights in place so that all particles have the same weight and the # sum of the weights is one. self.weights[:] = np.ones(self.N_PARTICLES) / float(self.N_PARTICLES) self.state_lock.release()
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() 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_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 visualizePlan(self): if self.plan_pub.get_num_connections() > 0: frame_id = 'map' pa = Path() pa.header = Utils.make_header(frame_id) pa.poses = [] for i in range(self.currentPlanWaypointIndex,len(plan)): next_segment = plan[i] next_goal_point = next_segment[0] gx = next_goal_point[0] gy = next_goal_point[1] gt = next_segment[1] goalPixelSpace = self.dtype([[gx, gy, gt]]) Utils.map_to_world(goalPixelSpace, self.map_info) thegoal = [goalPixelSpace[0][0], goalPixelSpace[0][1], goalPixelSpace[0][2]] pa.poses.append(Utils.particle_to_posestamped(thegoal, frame_id)) self.plan_pub.publish(pa)
def initialize_global(self): ''' Spread the particle distribution over the permissible region of the state space. ''' print("GLOBAL INITIALIZATION") # randomize over grid coordinate space with self.state_lock: print('Waiting for map..') while not self.sensor_model.map_set: rospy.sleep(0.05) 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)) pose = Pose() pose.orientation.x = 0.0 pose.orientation.y = 0.0 pose.orientation.z = 0.823 pose.orientation.w = 0.568 pose.position.x = 4.96825 pose.position.y = -22.3357 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[:,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.particles = permissible_states self.weights[:] = 1.0 / self.MAX_PARTICLES
def initialize_global(self): self.state_lock.acquire() # Use self.permissible_region to get in-bounds states # Uniformally sample from in-bounds regions # Convert map samples (which are in pixels) to world samples (in meters/radians) # Take a look at utils.py # Update particles in place # Update weights in place so that all particles have the same weight and the # sum of the weights is one. # YOUR CODE HERE permissible_x, permissible_y = np.where(self.permissible_region == 1) angle_step = 4 permissible_step = angle_step * len( permissible_x) / self.particles.shape[0] indices = np.arange(0, len(permissible_x), permissible_step)[:(self.particles.shape[0] / angle_step)] permissible_states = np.zeros((self.particles.shape[0], 3)) for i in xrange(angle_step): permissible_states[i * (self.particles.shape[0] / angle_step):(i + 1) * (self.particles.shape[0] / angle_step), 0] = permissible_y[indices] permissible_states[i * (self.particles.shape[0] / angle_step):(i + 1) * (self.particles.shape[0] / angle_step), 1] = permissible_x[indices] permissible_states[i * (self.particles.shape[0] / angle_step):(i + 1) * (self.particles.shape[0] / angle_step), 2] = i * (2 * np.pi / angle_step) Utils.map_to_world(permissible_states, self.map_info) self.particles[:, :] = permissible_states[:, :] self.weights[:] = 1.0 / float(self.N_PARTICLES) self.state_lock.release()
def initialize_global(self): self.state_lock.acquire() # Use self.permissible_region to get in-bounds states # Uniformally sample from in-bounds regions # Convert map samples (which are in pixels) to world samples (in meters/radians) # Take a look at utils.py # Update particles in place # Update weights in place so that all particles have the same weight and the # sum of the weights is one. # YOUR CODE HERE permissible_poses = np.zeros((self.N_PARTICLES, 3)) permissible_x, permissible_y = np.where(self.permissible_region == 1) permissible_poses[:, 0] = np.random.choice(permissible_x, self.N_PARTICLES) permissible_poses[:, 1] = np.random.choice(permissible_y, self.N_PARTICLES) permissible_poses[:, 2] = 2.0 * np.pi * np.random.random(self.N_PARTICLES) utils.map_to_world(permissible_poses, self.map_info) self.particles[:] = permissible_poses[:] self.weights[:] = 1.0 / self.N_PARTICLES self.state_lock.release()
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] angle_step = 4 # The number of particles at each location, each with different rotation permissible_step = angle_step * len( permissible_x) / self.particles.shape[ 0] # The sample interval for permissible states indices = np.arange(0, len(permissible_x), permissible_step)[:( self.particles.shape[0] / angle_step)] # Indices of permissible states to use permissible_states = np.zeros( (self.particles.shape[0], 3)) # Proxy for the new particles # Loop through permissible states, each iteration drawing particles with # different rotation for i in xrange(angle_step): permissible_states[i * (self.particles.shape[0] / angle_step):(i + 1) * (self.particles.shape[0] / angle_step), 0] = permissible_y[indices] permissible_states[i * (self.particles.shape[0] / angle_step):(i + 1) * (self.particles.shape[0] / angle_step), 1] = permissible_x[indices] permissible_states[i * (self.particles.shape[0] / angle_step):(i + 1) * (self.particles.shape[0] / angle_step), 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 goalCB(self, msg): #rospy.loginfo('recieved goal msg') self.goal = np.array([(msg.pose.position.x, msg.pose.position.y, 0)]) Utils.world_to_map(self.goal, self.map_info) self.goal = np.array((self.goal[0][0], 1300 - self.goal[0][1], self.goal[0][2])).astype(int) #rospy.loginfo(self.goal) if self.environment[self.goal[1]][self.goal[0]] == 0: #rospy.loginfo(self.goal) if self.start != None and self.goal != None: #rospy.loginfo('getting path') path = np.array( self.rrt(self.environment, self.start, self.goal)).astype(float) #rospy.loginfo(path) Utils.map_to_world(path, self.map_info) path = path.astype(int) #rospy.loginfo('visualizing') self.visualize(path, self.start, self.goal) else: self.goal = None
def initialize_global(self): self.state_lock.acquire() # Use self.permissible_region to get in-bounds states # Uniformally sample from in-bounds regions # Convert map samples (which are in pixels) to world samples (in meters/radians) # Take a look at utils.py # Update particles in place # Update weights in place so that all particles have the same weight and the # sum of the weights is one. # YOUR CODE HERE for i in range(len(self.particles)): in_bounds = 0 w = 0 h = 0 while not in_bounds: w = np.random.randint(0, self.permissible_region.shape[0]) h = np.random.randint(0, self.permissible_region.shape[1]) in_bounds = self.permissible_region[w][h] self.particles[i] = [w, h, 0] Utils.map_to_world(self.particles, self.map_info) self.weights[:] = 1 / float(self.particles.shape[0]) self.state_lock.release()
def find_frontiers(self) -> List[Pose]: """Find free edge frontiers in a probabilistic grid map. Returns ------- frontiers : list of Pose A (possibly empty) List of free-edge frontiers. """ # construct a free edge cell mask free_edge_cells = self._find_free_edge_cells(self.gridmap) # find frontiers representatives frontiers = self._find_free_edge_centroids(free_edge_cells, self.gridmap.resolution) if not frontiers: return [] # convert to world coordinates frontiers = map_to_world(frontiers, self.gridmap) return [point_to_pose(x, y) for x, y in frontiers]
# Potentially downsample permissible_x and permissible_y here print 'Creating particles' angle_step = 25 particles = np.zeros((angle_step * permissible_x.shape[0], 3)) for i in xrange(angle_step): particles[i * (particles.shape[0] / angle_step):(i + 1) * (particles.shape[0] / angle_step), 0] = permissible_y[:] particles[i * (particles.shape[0] / angle_step):(i + 1) * (particles.shape[0] / angle_step), 1] = permissible_x[:] particles[i * (particles.shape[0] / angle_step):(i + 1) * (particles.shape[0] / angle_step), 2] = i * (2 * np.pi / angle_step) utils.map_to_world(particles, map_info) weights = np.ones(particles.shape[0]) / float(particles.shape[0]) print 'Initializing sensor model' sm = SensorModel(scan_topic, laser_ray_step, exclude_max_range_rays, max_range_meters, map_msg, particles, weights) # Give time to get setup rospy.sleep(1.0) # Load laser scan from bag bag = rosbag.Bag(bag_path) for _, msg, _ in bag.read_messages(topics=['/scan']): laser_msg = msg break
def plan_path( self, start: Pose, goal: Pose, radius: Optional[float] = None, simplify: bool = True, robust: bool = True, ) -> Tuple[Path, float]: """Plan a path from start to goal over a given map using A* algorithm. Parameters ---------- start : Pose Start position in world coordinates. goal : Pose Goal position in world coordinates. radius : float, optional The final point in the found path is within the neighborhood of a given radius from the goal. Default `2 * robot_size`. simplify : bool, optional If True (default), simplifies the found path by reducing the amount of waypoints to the minimal necessary so that the path still doesn't collide with obstacles. robust : bool, optional If True (default), steers A* heuristic to avoid paths that are close to obstacles. Returns ------- path : path The found path. length : float Path length (sum of distances between consecutive points). """ if radius is None: radius = ASTAR_TOLERANCE_FACTOR * self.robot_size radius /= self.gridmap.resolution # transform start and goal to the useful format start, goal = pose_to_point(start), pose_to_point(goal) # transform start and goal from world coordinates to map coordinates start, goal = world_to_map([start, goal], self.gridmap) # steer the A* heuristic for robust planning def steered_heuristic(a: Tuple[int, int], b: Tuple[int, int]) -> float: return euclidean_distance( a, b) + OBSTACLE_PENALTY_FACTOR / self.dist_to_obstacles[a[1], a[0]] self.astar.heuristic = steered_heuristic if robust else euclidean_distance # find a path using A* search path = self.astar.search(self.maze, start, goal, neighborhood="N8", radius=radius) # no path found if len(path) < 2: return Path(), np.inf # simplify the found path if simplify: path = self._simplify_path(path, self.maze) # transform the path back to the world coordinates path = map_to_world(path, self.gridmap) # create the Path message path = points_to_path(path) # compute the path length length = path_length(path) return path, length
def main(): rospy.init_node("sensor_model", anonymous=True) # Initialize the node bag_path = rospy.get_param( "~bag_path", '/home/tim/car_ws/src/lab2/bags/laser_scans/laser_scan1.bag') scan_topic = rospy.get_param("~scan_topic", "/scan") # The topic containing laser scans laser_ray_step = int(rospy.get_param("~laser_ray_step") ) # Step for downsampling laser scans (18 by default) # Whether to exclude rays that are beyond the max range (true by default) exclude_max_range_rays = bool(rospy.get_param("~exclude_max_range_rays")) max_range_meters = float(rospy.get_param( "~max_range_meters")) # The max range of the laser (5.6 by default) print 'Bag path: ' + bag_path # Use the 'static_map' service (launched by MapServer.launch) to get the map print("Getting map from service: ", MAP_TOPIC) rospy.wait_for_service(MAP_TOPIC) map_msg = rospy.ServiceProxy( MAP_TOPIC, GetMap)().map # The map, will get passed to init of sensor model map_info = map_msg.info # Save info about map for later use # map_info example: # map_load_time: # secs: 1541298847 # nsecs: 661081098 # resolution: 0.019999999553 # width: 3200 # height: 3200 # origin: # position: # x: -34.92 # y: -33.64 # z: 0.0 # orientation: # x: 0.0 # y: 0.0 # z: 0.0 # w: 1.0 print 'Creating permissible region' # Create numpy array representing map for later use # By default map has values 0: permissible, -1: unmapped, 100: blocked array_255 = np.array(map_msg.data).reshape( (map_msg.info.height, map_msg.info.width)) # shape (3200, 3200) permissible_region = np.zeros_like( array_255, dtype=bool) # zeros - shape (3200, 3200) # Set the values of the permissible region to 1 and everything else remains 0s permissible_region[ array_255 == 0] = 1 # Numpy array of dimension (map_msg.info.height, map_msg.info.width), # With values 0: not permissible, 1: permissible permissible_x, permissible_y = np.where( permissible_region == 1) # both shapes (521088,) # Potentially downsample permissible_x and permissible_y here print 'Creating particles' angle_step = 25 particles = np.zeros((angle_step * permissible_x.shape[0], 3)) # particles.shape (13027200, 3) for i in xrange(angle_step): particles[i * (particles.shape[0] / angle_step):(i + 1) * (particles.shape[0] / angle_step), 0] = permissible_y[:] particles[i * (particles.shape[0] / angle_step):(i + 1) * (particles.shape[0] / angle_step), 1] = permissible_x[:] particles[i * (particles.shape[0] / angle_step):(i + 1) * (particles.shape[0] / angle_step), 2] = i * (2.0 * np.pi / angle_step) Utils.map_to_world(particles, map_info) weights = np.ones(particles.shape[0]) / float( particles.shape[0]) # weights.shape (13027200,) print 'Initializing sensor model' sm = SensorModel(scan_topic, laser_ray_step, exclude_max_range_rays, max_range_meters, map_msg, particles, weights) # Give time to get setup rospy.sleep(1.0) # Load laser scan from bag bag = rosbag.Bag(bag_path) for _, msg, _ in bag.read_messages(topics=['/scan']): laser_msg = msg break w_min = np.amin(weights) w_max = np.amax(weights) pub_laser = rospy.Publisher( scan_topic, LaserScan, queue_size=1) # Publishes the most recent laser scan print("Starting analysis, this could take awhile...") while not isinstance(sm.queries, np.ndarray): pub_laser.publish(laser_msg) rospy.sleep(1.0) rospy.sleep(1.0) # Make sure there's enough time for laserscan to get lock print 'Going to wait for sensor model to finish' sm.state_lock.acquire() print 'Done, preparing to plot' print "weights before", weights weights = weights.reshape((angle_step, -1)) print "weights after", weights weights = np.amax(weights, axis=0) print map_msg.info.height print map_msg.info.width print weights.shape w_min = np.amin(weights) w_max = np.amax(weights) print 'w_min = %f' % w_min print 'w_max = %f' % w_max weights = 0.9 * (weights - w_min) / (w_max - w_min) + 0.1 img = np.zeros((map_msg.info.height, map_msg.info.width)) for i in xrange(len(permissible_x)): img[permissible_y[i], permissible_x[i]] = weights[i] plt.imshow(img) plt.show()
def get_plan(initial_pose, goal_pose, counter): # Create a publisher to publish the initial pose init_pose_pub = rospy.Publisher( INIT_POSE_TOPIC, PoseWithCovarianceStamped, queue_size=1) # to publish init position x=2500, y=640 # Create a publisher to publish the goal pose goal_pose_pub = rospy.Publisher( GOAL_POSE_TOPIC, PoseStamped, queue_size=1) # create a publisher for goal pose map_img, map_info = utils.get_map(MAP_TOPIC) # Get and store the map PWCS = PoseWithCovarianceStamped( ) # create a PoseWithCovarianceStamped() msg PWCS.header.stamp = rospy.Time.now() # set header timestamp value PWCS.header.frame_id = "map" # set header frame id value temp_pose = utils.map_to_world(initial_pose, map_info) # init pose PWCS.pose.pose.position.x = temp_pose[0] PWCS.pose.pose.position.y = temp_pose[1] PWCS.pose.pose.position.z = 0 PWCS.pose.pose.orientation = utils.angle_to_quaternion( temp_pose[2] ) # set msg orientation to [converted to queternion] value of the yaw angle in the look ahead pose from the path print "Pubplishing Initial Pose to topic", INIT_POSE_TOPIC print "Initial ", PWCS.pose init_pose_pub.publish( PWCS ) # publish initial pose, now you can add a PoseWithCovariance with topic of "/initialpose" in rviz if counter == 0: for i in range(0, 5): init_pose_pub.publish(PWCS) rospy.sleep(0.5) print "Initial Pose Set." # raw_input("Press Enter") PS = PoseStamped() # create a PoseStamped() msg PS.header.stamp = rospy.Time.now() # set header timestamp value PS.header.frame_id = "map" # set header frame id value temp_pose = utils.map_to_world(goal_pose, map_info) # init pose PS.pose.position.x = temp_pose[ 0] # set msg x position to value of the x position in the look ahead pose from the path PS.pose.position.y = temp_pose[ 1] # set msg y position to value of the y position in the look ahead pose from the path PS.pose.position.z = 0 # set msg z position to 0 since robot is on the ground PS.pose.orientation = utils.angle_to_quaternion(temp_pose[2]) print "Pubplishing Goal Pose to topic", GOAL_POSE_TOPIC print "\nGoal ", PS.pose goal_pose_pub.publish(PS) print "Goal Pose Set" # raw_input("Press Enter") print "\nwaiting for plan ", str(counter), "\n" raw_plan = rospy.wait_for_message(PLAN_POSE_ARRAY_TOPIC, PoseArray) print "\nPLAN COMPUTED! of type:", type(raw_plan) path_part_pub = rospy.Publisher( "/CoolPlan/plan" + str(counter), PoseArray, queue_size=1) # create a publisher for plan lookahead follower for i in range(0, 4): path_part_pub.publish(raw_plan) rospy.sleep(0.4) return raw_plan