def _load_map(self, filename, map_resolution, thicken_obstacles=True):
"""
Loads map from file into costmap object
:param filename str: location of the map file
:param map_resolution float: desired resolution of the loaded map
:param thicken_obstacles bool: thicken the obstacles in the map by 1 pixel, to avoid 1 pixel thick walls
"""
map_data = cv2.imread(filename)
assert map_data is not None and "map file not able to be loaded. Does the file exist?"
map_data = cv2.cvtColor(map_data, cv2.COLOR_RGB2GRAY)
map_data = map_data.astype(np.uint8)
assert np.max(map_data) == 255
if thicken_obstacles:
occupied_coords = np.argwhere(map_data == Costmap.OCCUPIED)
occupied_mask = np.zeros_like(map_data)
occupied_mask[occupied_coords[:, 0], occupied_coords[:, 1]] = 1
occupied_mask = cv2.dilate(
occupied_mask,
cv2.getStructuringElement(cv2.MORPH_CROSS, (3, 3)))
occupied_coords = np.argwhere(occupied_mask == 1)
map_data[occupied_coords[:, 0],
occupied_coords[:, 1]] = Costmap.OCCUPIED
self.map = Costmap(data=map_data,
resolution=map_resolution,
origin=[0., 0.])
def test_get_downscaled():
# todo grab a real case to test obstacle preservation
costmap = Costmap(data=np.zeros((100, 200), dtype=np.uint8),
resolution=0.05,
origin=np.array([-50., -50.]))
downscaled_costmap = costmap.get_downscaled(0.1)
assert downscaled_costmap.get_shape() == (50, 100)
assert downscaled_costmap.resolution == 0.1
assert np.all(downscaled_costmap.origin == [-50., -50.])
def test_compare_maps():
"""
Tests the compare maps function
---maps/test/test.png
0 0 0 0 0
0 255 255 255 0
0 255 0 255 0
0 255 255 255 0
0 0 0 0 0
---end file
"""
test_map_filename = os.path.join(get_maps_dir(), "test/circle.png")
footprint = CustomFootprint(np.array([[0., 0.]]), np.pi / 2)
env = GridWorld(map_filename=test_map_filename,
footprint=footprint,
sensor=Neighborhood(sensor_range=1, values=[0, 255]),
start_state=[1, 3, 0],
map_resolution=1,
thicken_obstacles=False)
map_data = np.array([[0, 0, 0, 0, 0], [0, 255, 255, 0, 0],
[0, 255, 0, 255, 0], [0, 0, 255, 0, 0],
[0, 0, 0, 0, 0]]).astype(np.uint8)
occupancy_map = Costmap(data=map_data, resolution=1, origin=[0, 0])
percentage_completed = env.compare_maps(occupancy_map)
assert percentage_completed == 0.625
def test_creation():
costmap = Costmap(data=np.zeros((100, 100), dtype=np.uint8),
resolution=1.0,
origin=np.array([-50., -50.]))
assert np.all(costmap.data == np.zeros((100, 100), dtype=np.uint8))
assert costmap.resolution == 1.0
assert np.all(costmap.origin == np.array([-50., -50.]))
def test_update():
initial_map = Costmap(127 * np.ones((11, 11), dtype=np.uint8), 1,
np.array([-5., -5.]))
sensor_range = 7.0
mapper = LogOddsMapper(initial_map=initial_map,
sensor_range=sensor_range,
measurement_certainty=.8,
max_log_odd=50,
min_log_odd=-8,
threshold_occupied=.7,
threshold_free=.3)
pose = np.array([0., 0., 0.])
scan_angles = np.linspace(-np.pi, np.pi, 360)
scan_ranges = sensor_range * np.ones_like(scan_angles)
scan_ranges[scan_ranges.shape[0] // 2:] = sensor_range / 2.
occupancy_map = mapper.update(state=pose,
scan_angles=scan_angles,
scan_ranges=scan_ranges)
ground_truth = np.array(
[[127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127],
[127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127],
[127, 127, 127, 0, 0, 0, 0, 0, 127, 127, 127],
[127, 127, 0, 0, 255, 255, 255, 0, 0, 127, 127],
[127, 127, 0, 255, 255, 255, 255, 255, 0, 127, 127],
[255, 255, 127, 255, 255, 255, 255, 255, 127, 255, 255],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]],
dtype=np.uint8)
assert np.all(occupancy_map.data == ground_truth)
def test_oriented_astar(debug=False):
occupancy_map_data = load_occupancy_map_data('brain', 'big_retail.png')
occupancy_map = Costmap(occupancy_map_data, 0.03, origin=[0., 0.])
footprint_points = get_tricky_circular_footprint()
footprint = CustomFootprint(footprint_points,
2. * np.pi / 180.,
inflation_scale=2.0)
angles = get_astar_angles()
footprint_masks = footprint.get_footprint_masks(occupancy_map.resolution,
angles=angles)
outline_coords = footprint.get_outline_coords(occupancy_map.resolution,
angles=angles)
start = rc_to_xy([1956, 137, 0], occupancy_map)
goal = rc_to_xy([841, 3403, 0.], occupancy_map)
start_time = time.time()
success, path = oriented_astar(goal=goal,
start=start,
occupancy_map=occupancy_map,
footprint_masks=footprint_masks,
outline_coords=outline_coords,
obstacle_values=[0, 127],
planning_scale=10)
time_elapsed = time.time() - start_time
assert success
if debug:
visualization_map = occupancy_map.copy()
visualization_map.data = np.dstack(
(occupancy_map.data, occupancy_map.data, occupancy_map.data))
draw_footprint_path(footprint, path, visualization_map, [255, 0, 0],
[0, 0, 255])
if success:
print("found solution in", time_elapsed, "seconds, with length:",
path.shape[0])
else:
print("failed to find solution")
plt.imshow(visualization_map.data, interpolation='nearest')
plt.show()
def test_extract_frontiers(debug=False):
occupancy_map_data = load_occupancy_map_data('test', 'vw_partly_explored_test.png')
occupancy_map = Costmap(data=occupancy_map_data, resolution=0.03, origin=np.array([0, 0]))
frontiers = extract_frontiers(occupancy_map=occupancy_map, approx=False,
kernel=cv2.getStructuringElement(cv2.MORPH_CROSS, (3, 3)))
frontier_sizes = np.array([frontier.shape[0] if len(frontier.shape) > 1 else 1 for frontier in frontiers])
significant_frontiers = [frontier for i, frontier in enumerate(frontiers) if frontier_sizes[i] > 70]
assert len(significant_frontiers) == 4
if debug:
frontier_map = np.repeat([occupancy_map.data], repeats=3, axis=0).transpose((1, 2, 0))
for frontier in significant_frontiers:
frontier_px = xy_to_rc(frontier, occupancy_map).astype(np.int)
frontier_px = frontier_px[which_coords_in_bounds(frontier_px, occupancy_map.get_shape())]
frontier_map[frontier_px[:, 0], frontier_px[:, 1]] = [255, 0, 0]
plt.imshow(frontier_map, cmap='gray', interpolation='nearest')
plt.show()
def test_plan(debug=False):
occupancy_map_data = load_occupancy_map_data('test', 'frontier_plan_map.png')
occupancy_map = Costmap(data=occupancy_map_data, resolution=0.03, origin=np.array([-6.305, -6.305]))
pose = np.array([.8, 0, -0.51759265])
frontier_agent = create_frontier_agent_from_params(os.path.join(get_exploration_dir(), "params/params.yaml"))
frontier_agent.is_first_plan = False
plan = frontier_agent.plan(pose, occupancy_map)
if debug:
visualize(occupancy_map, pose, np.array([]), np.array([]), frontier_agent.get_footprint(), [], (1000, 1000), pose,
frontiers=frontier_agent.get_frontiers(compute=True, occupancy_map=occupancy_map))
assert plan.shape[0] > 2
def test_compare_maps_with_floodfill():
ground_truth = cv2.imread(
get_maps_dir() + "/test/vw_ground_truth_test.png", cv2.COLOR_BGR2GRAY)
current_map = cv2.imread(
get_maps_dir() + "/test/vw_partly_explored_test.png",
cv2.COLOR_BGR2GRAY)
current_map = Costmap(current_map, 1, np.array([0., 0.]))
start_state = np.array([200, 230, 0])
num_free = compute_connected_pixels(start_state, ground_truth).shape[0]
measured_num_free = np.argwhere(current_map.data == Costmap.FREE).shape[0]
x = measured_num_free / float(num_free)
assert np.round(x, 4) == 0.2790
def test_footprint_orientation(debug=False):
state = np.array([0.71, 0.35, np.pi / 4])
map_data = load_occupancy_map_data('test', 'footprint_orientation_map.png')
occupancy_map = Costmap(data=map_data,
resolution=0.03,
origin=[-19.72, -1.54])
footprint_points = get_tricky_circular_footprint()
footprint = CustomFootprint(footprint_points,
2 * np.pi / 180.,
inflation_scale=1.3)
assert not footprint.check_for_collision(state, occupancy_map, debug=debug)
assert not footprint.check_for_collision(
state, occupancy_map, debug=debug, use_python=True)
def test_multithread_astar(debug=False):
num_instances = 10
pool = multiprocessing.Pool()
occupancy_map_data = load_occupancy_map_data('test', 'maze_small.png')
occupancy_map = Costmap(occupancy_map_data, 0.03, origin=[0., 0.])
obstacle_values = np.array([Costmap.OCCUPIED, Costmap.UNEXPLORED],
dtype=np.uint8)
start = rc_to_xy(
np.argwhere(occupancy_map.data == Costmap.FREE)[0, :], occupancy_map)
goals = rc_to_xy(
np.argwhere(occupancy_map.data == Costmap.FREE)[-num_instances:, :],
occupancy_map)
astar_fn = partial(astar,
start=start,
occupancy_map=occupancy_map,
obstacle_values=obstacle_values)
start_time = time.time()
results = pool.map(astar_fn, goals)
time_elapsed = time.time() - start_time
if debug:
print("ran", num_instances, "astar instances, finished in",
time_elapsed)
for i, [successful, path] in enumerate(results):
assert successful
if debug:
if successful:
print(" instance", i, "\b: found solution with length:",
path.shape[0])
else:
print(" instance", i, "\b: failed to find solution")
def test_one_astar(debug=False):
occupancy_map_data = load_occupancy_map_data('test', 'maze_small.png')
occupancy_map = Costmap(occupancy_map_data, 0.03, origin=[0., 0.])
obstacle_values = np.array([Costmap.OCCUPIED, Costmap.UNEXPLORED],
dtype=np.uint8)
start = rc_to_xy(
np.argwhere(occupancy_map.data == Costmap.FREE)[0, :], occupancy_map)
goal = rc_to_xy(
np.argwhere(occupancy_map.data == Costmap.FREE)[-1, :], occupancy_map)
start_time = time.time()
success, path = astar(goal=goal,
start=start,
occupancy_map=occupancy_map,
obstacle_values=obstacle_values,
allow_diagonal=False)
time_elapsed = time.time() - start_time
assert success
if debug:
map_solution = np.dstack(
(occupancy_map.data, occupancy_map.data, occupancy_map.data))
path_px = xy_to_rc(path, occupancy_map).astype(np.int)
map_solution[path_px[:, 0], path_px[:, 1]] = [0, 0, 255]
if success:
print("found solution in", time_elapsed, "seconds, with length:",
path.shape[0])
else:
print("failed to find solution")
plt.imshow(map_solution, interpolation='nearest')
plt.show()
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
class GridWorld:
"""
A simple grid world environment. Edges of the map are treated like obstacles
Map must be a image file whose values represent free (255, white), occupied (0, black).
"""
def __init__(self,
map_filename,
map_resolution,
sensor,
footprint,
start_state=None,
render_size=(500, 500),
thicken_obstacles=True):
"""
Creates an interactive grid world environment structured similar to open ai gym.
Allows for moving, sensing, and visualizing within the space. Map loaded is based off the map_filename.
:param map_filename str: path of image file whose values represent free (255, white), occupied (0, black).
:param map_resolution float: resolution of which to represent the map
:param sensor Sensor: sensor to use to sense the environment
:param footprint Footprint: footprint of the robot
:param start_state bool: if None, will randomly sample a free space point as the starting point
else it must be [row, column] of the position you want the robot to start.
it must be a valid location (free space and footprint must fit)
:param render_size Tuple(int): size of which to render the map (for visualization env.render() )
:param thicken_obstacles bool: thicken the obstacles in the map by 1 pixel, to avoid 1 pixel thick walls
"""
self.footprint = footprint
self.footprint_no_inflation = footprint.no_inflation()
self.map_resolution = map_resolution
self.render_size = render_size
assert render_size.shape[0] == 2 if isinstance(
render_size, np.ndarray) else len(render_size) == 2
self.state = None
self.map = None
self.truth_free_coords = None
self._load_map(map_filename,
map_resolution,
thicken_obstacles=thicken_obstacles)
self.start_state = np.array(start_state).astype(np.float) \
if start_state is not None else self._get_random_start_state()
assert self.start_state.shape[0] == 3
self.sensor = sensor
assert self.map is not None
self.sensor.set_map(occupancy_map=self.map)
self.reset()
assert self._is_state_valid(self.start_state)
def _get_random_start_state(self):
"""
Samples a random valid state from the map
:return array(3)[float]: state [x, y, theta] of the robot
"""
valid_points = self.map_resolution * np.argwhere(
self.map.data == Costmap.FREE)[:, ::-1]
choice = np.random.randint(valid_points.shape[0])
valid = False
while not valid:
choice = np.random.randint(valid_points.shape[0])
valid = self._is_state_valid(
np.concatenate((valid_points[choice], [0])))
# todo add random angle
return np.concatenate((valid_points[choice], [0])).astype(np.float)
def _is_state_valid(self, state, use_inflation=True):
"""
Make sure state is not out of bounds or on an obstacle (footprint check)
:param state array(3)[float]: [x, y, theta], the position/orientation of the robot
:param use_inflation bool: whether to use the inflated footprint to collision check, or the normal footprint.
usually the environment will need use the actual footprint of the robot (because thats the
physical limitation we want to simulate)
:return bool: whether it is valid or not
"""
return 0 <= state[0] < self.map.get_size()[0] and 0 <= state[1] < self.map.get_size()[1] \
and not (self.footprint.check_for_collision(state=state, occupancy_map=self.map)
if use_inflation else self.footprint_no_inflation.check_for_collision(state=state, occupancy_map=self.map))
def _load_map(self, filename, map_resolution, thicken_obstacles=True):
"""
Loads map from file into costmap object
:param filename str: location of the map file
:param map_resolution float: desired resolution of the loaded map
:param thicken_obstacles bool: thicken the obstacles in the map by 1 pixel, to avoid 1 pixel thick walls
"""
map_data = cv2.imread(filename)
assert map_data is not None and "map file not able to be loaded. Does the file exist?"
map_data = cv2.cvtColor(map_data, cv2.COLOR_RGB2GRAY)
map_data = map_data.astype(np.uint8)
assert np.max(map_data) == 255
if thicken_obstacles:
occupied_coords = np.argwhere(map_data == Costmap.OCCUPIED)
occupied_mask = np.zeros_like(map_data)
occupied_mask[occupied_coords[:, 0], occupied_coords[:, 1]] = 1
occupied_mask = cv2.dilate(
occupied_mask,
cv2.getStructuringElement(cv2.MORPH_CROSS, (3, 3)))
occupied_coords = np.argwhere(occupied_mask == 1)
map_data[occupied_coords[:, 0],
occupied_coords[:, 1]] = Costmap.OCCUPIED
self.map = Costmap(data=map_data,
resolution=map_resolution,
origin=[0., 0.])
def compare_maps(self, occupancy_map):
"""
Does a comparison of the ground truth map with the input map,
and will return a percentage completed
:param occupancy_map Costmap: input map to be compared with ground truth
:return float: percentage covered of the input map to the ground truth map
"""
if self.truth_free_coords is None:
start_state_px = xy_to_rc(self.start_state, self.map)
self.truth_free_coords = compute_connected_pixels(
start_state_px, self.map.data)
free_coords = np.argwhere(occupancy_map.data == Costmap.FREE)
return free_coords.shape[0] / float(self.truth_free_coords.shape[0])
def step(self, desired_state):
"""
Execute the given action with the robot in the environment, return the next robot position,
and the output of sensor.measure() (sensor data)
:param desired_state array(3)[float]: desired next state of the robot
:return array(3)[float]: new_state (new robot position), sensor_data (output from sensor)
"""
new_state = np.array(desired_state, dtype=np.float)
new_state[2] = wrap_angles(desired_state[2])
# # todo maybe keep angle of desired state
if not self._is_state_valid(new_state + self.start_state,
use_inflation=False):
new_state = self.state.copy()
# compute sensor_data
measure_state = new_state.copy()
measure_state[:2] += self.start_state[:2]
sensor_data = self.sensor.measure(measure_state)
# set state
self.state = new_state
return new_state.copy(), sensor_data
def reset(self):
"""
Resets the robot to the starting state in the environment
:return array(3)[float]: robot position, sensor data from start state
"""
self.state = np.array((0, 0, self.start_state[2]))
sensor_data = self.sensor.measure(self.start_state)
return self.state.copy(), sensor_data
def render(self, wait_key=0):
"""
Renders the environment and the robots position
:param wait_key int: the opencv waitKey arg
"""
# convert to colored image
map_vis = cv2.cvtColor(self.map.data.copy(), cv2.COLOR_GRAY2BGR)
state_px = xy_to_rc(self.state + self.start_state,
self.map)[:2].astype(np.int)
map_vis[state_px[0], state_px[1]] = [127, 122, 10]
# # todo figure out programmatic way to pick circle size (that works well)
# cv2.circle(map_vis, tuple(self.state[:2][::-1].astype(int)), 1, [127, 122, 10], thickness=-1)
# resize map
map_vis = cv2.resize(map_vis,
tuple(self.render_size),
interpolation=cv2.INTER_NEAREST)
# visualize map
cv2.namedWindow('map', cv2.WINDOW_GUI_NORMAL)
cv2.imshow('map', map_vis)
cv2.resizeWindow('map', *self.render_size)
cv2.waitKey(wait_key)
def get_sensor(self):
"""
Gets the sensor object
:return Sensor: the sensor used by the grid world
"""
return self.sensor
def get_map_shape(self):
"""
Returns the shape of the map
:return Tuple(int): map shape
"""
return self.map.get_shape()
def get_map_size(self):
"""
Returns the size of the map (x, y) in meters
:return Tuple(float): map size (meters)
"""
return self.map.get_size()
def get_map_resolution(self):
"""
Returns the resolution of the map
:return float: resolution
"""
return self.map_resolution
def __del__(self):
"""
Destructor, delete opencv windows
"""
cv2.destroyAllWindows()
def test_get_shape_size():
costmap = Costmap(data=np.zeros((100, 200), dtype=np.uint8),
resolution=0.05,
origin=np.array([-50., -50.]))
assert costmap.get_shape() == (100, 200)
assert costmap.get_size() == (200 * 0.05, 100 * 0.05)
def test_custom_footprint(debug=False):
map_data = load_occupancy_map_data('test', 'three_rooms.png')
occupancy_map = Costmap(data=map_data, resolution=0.03, origin=[0, 0])
free_state = rc_to_xy(np.array([110, 220, 0]), occupancy_map)
colliding_state = rc_to_xy(np.array([90, 215, 0]), occupancy_map)
footprint_points = get_tricky_oval_footprint()
footprint = CustomFootprint(footprint_points, 2 * np.pi / 180.)
assert not footprint.check_for_collision(
free_state, occupancy_map, debug=debug)
assert footprint.check_for_collision(colliding_state,
occupancy_map,
debug=debug)
assert not footprint.check_for_collision(
free_state, occupancy_map, debug=debug, use_python=True)
assert footprint.check_for_collision(colliding_state,
occupancy_map,
debug=debug,
use_python=True)
rotated_colliding_state = rc_to_xy(np.array([110, 220, -np.pi / 2]),
occupancy_map)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug,
use_python=True)
footprint_points = get_tricky_circular_footprint()
footprint = CustomFootprint(footprint_points, 2 * np.pi / 180.)
assert not footprint.check_for_collision(
free_state, occupancy_map, debug=debug)
assert footprint.check_for_collision(colliding_state,
occupancy_map,
debug=debug)
assert not footprint.check_for_collision(
free_state, occupancy_map, debug=debug, use_python=True)
assert footprint.check_for_collision(colliding_state,
occupancy_map,
debug=debug,
use_python=True)
rotated_colliding_state = rc_to_xy(np.array([115, 155, np.pi / 4]),
occupancy_map)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug,
use_python=True)
rotated_colliding_state = rc_to_xy(np.array([0, 0, np.pi / 2]),
occupancy_map)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug)
assert footprint.check_for_collision(rotated_colliding_state,
occupancy_map,
debug=debug,
use_python=True)
