def __init__(self,
position: Position = Position(),
orientation: float = 0):
if type(position) is np.ndarray:
raise TypeError(
'You need to pass a Position to Pose. Use Position.from_array() to convert it.'
)
self._orientation = orientation
self._position = position.copy()
def create_basic_hallway(orientation=0):
return [
Wall(
Position(-2, 6).rotate(orientation),
Position(-2, -6).rotate(orientation)),
Wall(
Position(+2, 6).rotate(orientation),
Position(+2, -6).rotate(orientation))
]
def generate(self, pose: Pose, check_cache=False):
if check_cache:
if self.scan_exist(pose):
return self.load_scan(pose)
origin = pose.position
orientation = pose.orientation + np.pi / 2 # The first point taken by the sensor is parallel to the y axis
pts = []
for beam_id in range(0, self.nb_beam):
angle_rad = beam_id / self.nb_beam * 2 * math.pi + orientation
end_beam = origin + Position.from_angle(angle_rad,
norm=self.max_range_beam)
closest_inter = None
for wall in self.walls:
# inter = intersection_between_ray_and_segment(origin, end_beam - origin, wall.p1, wall.p2)
inter = intersection_between_segments(origin, end_beam,
wall.p1, wall.p2)
if inter is None:
continue
if closest_inter is None or (closest_inter - origin).norm > (
inter - origin).norm:
closest_inter = inter
if closest_inter is not None:
point = self.sensor_model.apply_noise(origin,
closest_inter) - origin
scan_frame_point = point.rotate(-pose.orientation)
pts.append(scan_frame_point.to_tuple())
scan = np.array(pts)
self.save_scan(pose, scan)
return scan
def intersection_between_lines(a1: Position, a2: Position, b1: Position,
b2: Position) -> Position:
s = np.vstack([a1.array, a2.array, b1.array, b2.array])
h = np.hstack((s, np.ones((4, 1))))
l1 = np.cross(h[0], h[1]) # first line
l2 = np.cross(h[2], h[3]) # second line
x, y, z = np.cross(l1, l2) # point of intersection
if z == 0:
raise ValueError('Parallel lines')
return Position(x / z, y / z)
def create_bumpy_hallway():
step_x = 0.5
step_y = 1
hallway_width = 3
walls_template = [
Wall(Position(hallway_width, 0), Position(hallway_width + step_x, 0)),
Wall(Position(hallway_width, 0), Position(hallway_width, step_y)),
Wall(Position(hallway_width, step_y),
Position(hallway_width + step_x, step_y)),
Wall(Position(hallway_width + step_x, step_y),
Position(hallway_width + step_x, 2 * step_y))
]
walls = []
walls += walls_template
walls += [w.copy().move(0, 4 * step_y) for w in walls_template]
walls += [w.copy().move(0, 2 * step_y) for w in walls_template]
walls += [w.copy().move(0, -2 * step_y) for w in walls_template]
walls += [w.copy().move(0, -4 * step_y) for w in walls_template]
walls += [w.copy().flix_x() for w in walls]
return walls
# poses = [origin,
# Pose(Position( 1, 0), 0),
# Pose(Position( 2, 0), 0),
# Pose(Position( 4, 0), 0),
# Pose(Position( 5, 0), 0),
# Pose(Position( 6, 0), 0),
#
# Pose(Position( 6, 1), np.deg2rad(90)),
# Pose(Position( 6, 4), np.deg2rad(90)),
# Pose(Position( 6, 8), np.deg2rad(90)),
# ]
orientation = np.deg2rad(0)
walls, poses = from_ascii_art(art,
origin_offset=Position(1, 1),
orientation=orientation)
# poses = [Pose.from_values(p.x, p.y, orientation) for p in poses]
# poses[0] = origin
sg = ScanGenerator(walls, nb_beam=180)
print("Generating map...")
scans = [sg.generate(p, check_cache=True).transpose() for p in poses]
print("done!")
# init_tf = move.to_tf()
icp = ICP()
#icp.set_default()
icp.load_from_dict(ICP.BASIC_CONFIG)
no_penalties = [[] for p in poses]
penalties_x = [
def normalize(vec: Position) -> Position:
if vec.norm == 0:
raise ZeroDivisionError
return vec.copy() / vec.norm
def position(self, position: Position):
self._position = position.copy()
def from_values(cls, x: float, y: float, orientation: float = 0) -> 'Pose':
return cls(Position(x, y), orientation)
def from_dict(cls, my_dict: Dict[str, float]) -> 'Pose':
return cls(Position(my_dict['x'], my_dict['y']),
my_dict['orientation'])
def from_ascii_art(ascii_art, origin_offset=None, orientation=0):
print(ascii_art)
if origin_offset is None:
origin_offset = Position()
grid = [[c for c in l] for l in ascii_art.splitlines()]
cardinals = [(-1, 0), (0, -1)]
w = len(grid[0])
h = len(grid)
cells = []
origin = None
checkpoints = {}
for y, l in enumerate(grid):
for x, val in enumerate(l):
is_free = val != WALL
if val == ORIGIN:
origin = x, y
checkpoints[0] = origin
if val in NUMBERS:
checkpoints[int(val)] = x, y
for cx, cy in cardinals:
px = cx + x
py = cy + y
if 0 <= px < w and 0 <= py < h:
is_free_c = grid[py][px] != WALL
if is_free != is_free_c:
# print(f"Add wall at {x},{y} in {cx},{cy}")
cells.append((x, y, cy == 0))
if origin is None:
raise RuntimeError("No origin found, missing the '*' in map")
ox, oy = origin
def to_real_world(cell_x, cell_y, offset=0.4):
ax = (cell_x - ox - offset) * cell_w
ay = (cell_y - oy - offset) * cell_h
x = ax * cos(orientation) - ay * sin(orientation)
y = ax * sin(orientation) + ay * cos(orientation)
return x + origin_offset.x, y + origin_offset.y
cell_w = 3
cell_h = -3 # Matplotlib has the origin in the lower left corner, not the upper left conner
walls = []
for x, y, is_vertical in cells:
if is_vertical:
w = Wall(Position(*to_real_world(x, y)),
Position(*to_real_world(x, y + 1)))
else:
w = Wall(Position(*to_real_world(x, y)),
Position(*to_real_world(x + 1, y)))
walls.append(w)
if len(checkpoints) > 1:
ordered_checkpoints = list(
collections.OrderedDict(sorted(checkpoints.items())).values())
# linear_path += [origin] # So it loop
path = [origin]
for a, b in zip(ordered_checkpoints, ordered_checkpoints[1:]):
path += find_shortest_path(ascii_art, a, b)
path = subsample_path(path)
# path = subsample_path(list(path))
pose_path = [
Pose.from_values(*to_real_world(x, y, offset=0.0), orientation)
for x, y in path
]
return walls, pose_path
def move(self, x, y):
m = Position.from_list([x, y])
self.p1 += m
self.p2 += m
return self