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