def measure(self, state, debug=False):
"""
given the current robot pose, return the lidar data
:param state array(3)[float]: [x, y, theta (radians)] pose of the robot
:param debug bool: show debug plot?
:return Tuple[array(N)[float], array(N)[float]]: [lidar angles radians, lidar ranges meters] if ray did not hit obstacle, value is np.nan
"""
assert self._map is not None \
and "Please set the map using set_map() before calling measure, this will initialize lidar as well."
pose = state.copy()
pose_px = xy_to_rc(pose, self._map)
ranges = np.zeros_like(self._ray_angles)
for i, ego_ray in enumerate(self._ego_rays):
rotation_matrix = get_rotation_matrix_2d(-state[2])
rotated_ray = np.rint(ego_ray.dot(rotation_matrix))
ray = (pose_px[:2] + rotated_ray).astype(np.int)
ray = ray[which_coords_in_bounds(ray, self._map.get_shape())]
occupied_ind = np.argwhere(
self._map.data[ray[:, 0], ray[:, 1]] == Costmap.OCCUPIED)
if occupied_ind.shape[0]:
ranges[i] = np.linalg.norm(pose_px[:2] - ray[int(
occupied_ind[0])]) * self._map.resolution
else:
ranges[i] = np.nan
if debug:
points = scan_to_points(self._ray_angles + state[2],
ranges) + pose_px[:2]
plt.plot(points[:, 0], points[:, 1])
plt.show()
return self._ray_angles, ranges
def draw_scan_ranges(visualization_map, state, scan_angles, scan_ranges, color):
"""
Draw the occupied points of a lidar scan onto the map
:param visualization_map Costmap: costmap to draw on
:param state array(3)[float]: pose of the robot [x, y, theta]
:param scan_angles array(N)[float]: angles of the lidar scan
:param scan_ranges array(N)[float]: ranges of the lidar scan
:param color Union[int, array(3)[uint8]]: depending on the shape of visualization_map.data,
a valid value for the color to draw the path
"""
occupied_coords = scan_to_points(scan_angles + state[2], scan_ranges) + state[:2]
occupied_coords = xy_to_rc(occupied_coords, visualization_map).astype(np.int)
occupied_coords = occupied_coords[which_coords_in_bounds(occupied_coords, visualization_map.get_shape())]
visualization_map.data[occupied_coords[:, 0], occupied_coords[:, 1]] = color
def run_frontier_exploration(map_filename,
params_filename,
start_state,
sensor_range,
map_resolution,
completion_percentage,
render=True,
render_mode='exp',
render_interval=1,
render_size_scale=1.7,
completion_check_interval=1,
render_wait_for_key=True,
max_exploration_iterations=None):
"""
Interface for running frontier exploration on the grid world environment that is initialized via map_filename.. etc
:param map_filename str: path of the map to load into the grid world environment, needs to be a uint8 png with
values 127 for unexplored, 255 for free, 0 for occupied.
:param params_filename str: path of the params file for setting up the frontier agent etc.
See exploration/params/ for examples
:param start_state array(3)[float]: starting state of the robot in the map (in meters) [x, y, theta], if None the starting
state is random
:param sensor_range float: range of the sensor (lidar) in meters
:param map_resolution float: resolution of the map desired
:param completion_percentage float: decimal of the completion percentage desired i.e (.97), how much of the ground
truth environment to explore, note that 1.0 is not always reachable because of
footprint.
:param render bool: whether or not to visualize
:param render_mode str: specify rendering function (bc_exploration or bc_gym_planning_env)
:param render_interval int: visualize every render_interval iterations
:param render_size_scale Tuple(int): (h, w), the size of the render window in pixels
:param completion_check_interval int: check for exploration completion every completion_check_interval iterations
:param render_wait_for_key bool: if render is enabled, if render_wait_for_key is True then the exploration algorithm
will wait for key press to start exploration. Timing is not affected.
:param max_exploration_iterations int: number of exploration cycles to run before exiting
:return Costmap: occupancy_map, final map from exploration), percentage explored, time taken to explore
"""
# some parameters
frontier_agent = create_frontier_agent_from_params(params_filename)
footprint = frontier_agent.get_footprint()
# pick a sensor
sensor = Lidar(sensor_range=sensor_range,
angular_range=250 * np.pi / 180,
angular_resolution=1.0 * np.pi / 180,
map_resolution=map_resolution)
# setup grid world environment
env = GridWorld(map_filename=map_filename,
map_resolution=map_resolution,
sensor=sensor,
footprint=footprint,
start_state=start_state)
# setup corresponding gym environment
env_instance = RandomAisleTurnEnv()
render_size = (np.array(env.get_map_shape()[::-1]) *
render_size_scale).astype(np.int)
# setup log-odds mapper, we assume the measurements are very accurate,
# thus one scan should be enough to fill the map
padding = 1.
map_shape = np.array(env.get_map_shape()) + int(
2. * padding // map_resolution)
initial_map = Costmap(
data=Costmap.UNEXPLORED * np.ones(map_shape, dtype=np.uint8),
resolution=env.get_map_resolution(),
origin=[-padding - env.start_state[0], -padding - env.start_state[1]])
clearing_footprint_points = footprint.get_clearing_points(map_resolution)
clearing_footprint_coords = xy_to_rc(clearing_footprint_points,
initial_map).astype(np.int)
initial_map.data[clearing_footprint_coords[:, 0],
clearing_footprint_coords[:, 1]] = Costmap.FREE
mapper = LogOddsMapper(initial_map=initial_map,
sensor_range=sensor.get_sensor_range(),
measurement_certainty=0.8,
max_log_odd=8,
min_log_odd=-8,
threshold_occupied=.5,
threshold_free=.5)
# reset the environment to the start state, map the first observations
pose, [scan_angles, scan_ranges] = env.reset()
occupancy_map = mapper.update(state=pose,
scan_angles=scan_angles,
scan_ranges=scan_ranges)
if render:
if render_mode == 'exp':
visualize(occupancy_map=occupancy_map,
state=pose,
footprint=footprint,
start_state=start_state,
scan_angles=scan_angles,
scan_ranges=scan_ranges,
path=[],
render_size=render_size,
frontiers=[],
wait_key=0 if render_wait_for_key else 1)
elif render_mode == 'gym':
visualize_in_gym(gym_env=env_instance,
exp_map=occupancy_map,
path=[],
pose=pose)
else:
raise NameError(
"Invalid rendering method.\nPlease choose one of \"exp\" or \"gym\" methods."
)
iteration = 0
is_last_plan = False
was_successful = True
percentage_explored = 0.0
while True:
if iteration % completion_check_interval == 0:
percentage_explored = env.compare_maps(occupancy_map)
if percentage_explored >= completion_percentage:
is_last_plan = True
if max_exploration_iterations is not None and iteration > max_exploration_iterations:
was_successful = False
is_last_plan = True
# using the current map, make an action plan
path = frontier_agent.plan(state=pose,
occupancy_map=occupancy_map,
is_last_plan=is_last_plan)
# if we get empty lists for policy/path, that means that the agent was
# not able to find a path/frontier to plan to.
if not path.shape[0]:
print(
"No more frontiers! Either the map is 100% explored, or bad start state, or there is a bug!"
)
break
# if we have a policy, follow it until the end. update the map sparsely (speeds it up)
for j, desired_state in enumerate(path):
if footprint.check_for_collision(desired_state,
occupancy_map,
unexplored_is_occupied=True):
footprint_coords = footprint.get_ego_points(
desired_state[2], map_resolution) + desired_state[:2]
footprint_coords = xy_to_rc(footprint_coords,
occupancy_map).astype(np.int)
footprint_coords = footprint_coords[which_coords_in_bounds(
footprint_coords, occupancy_map.get_shape())]
occupancy_map.data[footprint_coords[:, 0],
footprint_coords[:, 1]] = Costmap.FREE
pose, [scan_angles, scan_ranges] = env.step(desired_state)
occupancy_map = mapper.update(state=pose,
scan_angles=scan_angles,
scan_ranges=scan_ranges)
# put the current laserscan on the map before planning
occupied_coords = scan_to_points(scan_angles + pose[2],
scan_ranges) + pose[:2]
occupied_coords = xy_to_rc(occupied_coords,
occupancy_map).astype(np.int)
occupied_coords = occupied_coords[which_coords_in_bounds(
occupied_coords, occupancy_map.get_shape())]
occupancy_map.data[occupied_coords[:, 0],
occupied_coords[:, 1]] = Costmap.OCCUPIED
# shows a live visualization of the exploration process if render is set to true
if render and j % render_interval == 0:
if render_mode == 'exp':
visualize(occupancy_map=occupancy_map,
state=pose,
footprint=footprint,
start_state=start_state,
scan_angles=scan_angles,
scan_ranges=scan_ranges,
path=path,
render_size=render_size,
frontiers=frontier_agent.get_frontiers(
compute=True, occupancy_map=occupancy_map),
wait_key=1,
path_idx=j)
elif render_mode == 'gym':
visualize_in_gym(gym_env=env_instance,
exp_map=occupancy_map,
path=path,
pose=pose)
else:
raise NameError(
"Invalid rendering method.\nPlease choose one of \"exp\" or \"gym\" methods."
)
if is_last_plan:
break
iteration += 1
if render:
cv2.waitKey(0)
return occupancy_map, percentage_explored, iteration, was_successful
def _scan_to_occupied_free_coords(self,
state,
angles,
ranges,
debug=False):
"""
converts laser scan to occupied and free coordinates for mapping
:param state array(3)[float]: current state of the robot
:param angles array(N)[float]: lidar angles of the robot
:param ranges array(N)[float]: lidar ranges corresponding to the angles
:param debug bool: show debug?
:return Tuple[array(N,2)[int], array(N,2)[int]]: occupied coordinates, and free coordinates (in row column)
"""
new_angles = wrap_angles(angles.copy() + state[2])
state_px = xy_to_rc(state, self._map)
position_px = state_px[:2].astype(np.int)
with np.errstate(invalid='ignore'):
free_inds = np.logical_or(np.isnan(ranges),
ranges >= self.sensor_range)
occ_inds = np.logical_not(free_inds)
ranges = ranges.copy()
ranges[free_inds] = self.sensor_range
occupied_coords = scan_to_points(new_angles[occ_inds],
ranges[occ_inds]) + state[:2]
occupied_coords = xy_to_rc(occupied_coords, self._map).astype(np.int)
free_endcoords = scan_to_points(new_angles[free_inds],
ranges[free_inds]) + state[:2]
free_endcoords = xy_to_rc(free_endcoords, self._map).astype(np.int)
if debug:
occupied_coords_vis = occupied_coords[which_coords_in_bounds(
occupied_coords, self._map.get_shape())]
free_coords_vis = free_endcoords[which_coords_in_bounds(
free_endcoords, self._map.get_shape())]
map_vis = np.repeat([self._map.data], repeats=3, axis=0).transpose(
(1, 2, 0)).copy()
map_vis[occupied_coords_vis[:, 0],
occupied_coords_vis[:, 1]] = [255, 0, 0]
map_vis[free_coords_vis[:, 0], free_coords_vis[:, 1]] = [0, 255, 0]
plt.imshow(map_vis)
plt.show()
free_coords = np.array([position_px])
for i in range(occupied_coords.shape[0]):
bresenham_line = bresenham2d_with_intensities(
position_px, occupied_coords[i, :], self._map.data.T)[:-1]
free_coords = np.vstack((free_coords, bresenham_line[:, :2]))
for i in range(free_endcoords.shape[0]):
bresenham_line = bresenham2d_with_intensities(
position_px, free_endcoords[i, :], self._map.data.T)
free_coords = np.vstack((free_coords, bresenham_line[:, :2]))
free_coords = free_coords.astype(np.int)
occupied_coords = occupied_coords[which_coords_in_bounds(
occupied_coords, self._map.get_shape())]
free_coords = free_coords[which_coords_in_bounds(
free_coords, self._map.get_shape())]
if debug:
map_vis = np.repeat([self._map.copy()], repeats=3,
axis=0).transpose((1, 2, 0))
map_vis[occupied_coords[:, 0], occupied_coords[:,
1]] = [10, 10, 127]
map_vis[free_coords[:, 0], free_coords[:, 1]] = [127, 10, 127]
map_vis[int(state[0]), int(state[1])] = [127, 122, 10]
plt.imshow(map_vis, interpolation='nearest')
plt.show()
return occupied_coords.astype(np.int), free_coords.astype(np.int)