def generate_scans(self, pose, map2d):
scans = Scans(max_range = self._lidar_config.range_max,
min_angle = self._lidar_config.min_angle, \
max_angle = self._lidar_config.max_angle, \
angle_res = self._lidar_config.angle_res)
conf = map2d.get_map_config()
min_map_coord = MapIntCoord2D(x=0, y=0)
max_map_coord = MapIntCoord2D(x=conf.x_tick_num, y=conf.y_tick_num)
x_ray_src_map, y_ray_src_map = \
GridMap2D.convert_global_2_map(pose.x, pose.y, conf.x_min, conf.y_min, conf.reso)
ray_src_map_coord = MapIntCoord2D(x=x_ray_src_map, y=y_ray_src_map)
for i in range(self._lidar_config.ray_cnt):
beam_angle = pose.theta + self._lidar_config.min_angle + self._lidar_config.angle_res * i
x_hit = self._lidar_config.range_max * math.cos(
beam_angle) + pose.x
y_hit = self._lidar_config.range_max * math.sin(
beam_angle) + pose.y
x_hit_map, y_hit_map = GridMap2D.convert_global_2_map(
x_hit, y_hit, conf.x_min, conf.y_min, conf.reso)
ray_hit_map_coord = MapIntCoord2D(x=x_hit_map, y=y_hit_map)
# 1. Generate path along this specific ray in map coords.
path = RayTracing2D.ray_tracing(ray_src_map_coord, \
ray_hit_map_coord, \
min_map_coord, \
max_map_coord)
# 2. Search area along point list.
for index, coord in enumerate(path):
x_map = int(coord.x)
y_map = int(coord.y)
# If this coord is outside or reaches last element of the path, set range_max and break.
if (not map2d.is_valid_map_coord(x_map, y_map)
or index == len(path) - 1):
scans.ranges[i] = self._lidar_config.range_max
break
# If the considered cell is occupied, set range and break.
if (map2d.get_val_via_map_coord(x_map=x_map, y_map=y_map) >
0.3):
scans.ranges[i] = \
math.sqrt((x_map - x_ray_src_map)**2 + (y_map - y_ray_src_map)**2) * conf.reso
break
return scans
def _convert_scan_hit_pnt_2_world_and_map_pnt(self, pose, scans, map_conf):
x_map, y_map = \
GridMap2D.convert_global_2_map(pose.x, pose.y, map_conf.x_min, map_conf.y_min, map_conf.reso)
end_pnts_world = [Coord2D() for i in range(scans.ray_cnt)]
end_pnts_map = [MapIntCoord2D() for i in range(scans.ray_cnt)]
for index in range(scans.ray_cnt):
beam_angle = pose.theta + scans.min_angle + scans.angle_res * index
coord_world = end_pnts_world[index]
coord_map = end_pnts_map[index]
# If scan is negative, invalid.
if (scans.ranges[index] > 0):
x = scans.ranges[index] * math.cos(beam_angle) + pose.x
y = scans.ranges[index] * math.sin(beam_angle) + pose.y
coord_world.x = x
coord_world.y = y
coord_map.x, coord_map.y = \
GridMap2D.convert_global_2_map(x, y, map_conf.x_min, map_conf.y_min, map_conf.reso)
else:
coord_world.x = pose.x
coord_world.y = pose.y
coord_map.x = x_map
coord_map.y = y_map
return end_pnts_world, end_pnts_map
def draw_line(map2d, x_st, y_st, x_en, y_en, value):
conf = map2d.get_map_config()
x_st_map, y_st_map = GridMap2D.convert_global_2_map(x_st, y_st, conf.x_min, conf.y_min, conf.reso)
x_en_map, y_en_map = GridMap2D.convert_global_2_map(x_en, y_en, conf.x_min, conf.y_min, conf.reso)
st_pnt_map = MapIntCoord2D(x=x_st_map, y=y_st_map)
end_pnt_map = MapIntCoord2D(x=x_en_map, y=y_en_map)
min_pnt_map = MapIntCoord2D(x=0, y=0)
max_pnt_map = MapIntCoord2D(x=conf.x_tick_num - 1, y=conf.y_tick_num - 1)
path = RayTracing2D.ray_tracing(st_pnt_map, end_pnt_map, min_pnt_map, max_pnt_map)
for coord in path:
map2d.update_val_via_map_coord(x_map=coord.x, y_map=coord.y, value=value)
def register_fixed_scan(self, *, pose, scans, map2d):
conf = map2d.get_map_config()
# Convert coordinate from global to map.
x_map_scan_st, y_map_scan_st = \
GridMap2D.convert_global_2_map(pose.x, pose.y, conf.x_min, conf.y_min, conf.reso)
map_scan_st = MapIntCoord2D(x=x_map_scan_st, y=y_map_scan_st)
map_scan_ends_world, map_scan_ends_map = self._convert_scan_hit_pnt_2_world_and_map_pnt(
pose, scans, conf)
min_pnt = MapIntCoord2D(x=0, y=0)
max_pnt = MapIntCoord2D(x=conf.x_tick_num - 1, y=conf.y_tick_num - 1)
for scan_idx, end_coord in enumerate(map_scan_ends_map):
if (map_scan_st.x == end_coord.x and map_scan_st.y == end_coord.y):
continue
path = RayTracing2D.ray_tracing(map_scan_st, end_coord, min_pnt,
max_pnt)
for index, coord in enumerate(path):
# When reacnes last element, break.
if (index == len(path) - 1):
break
x_map = my_round(coord.x)
y_map = my_round(coord.y)
if (not map2d.is_valid_map_coord(x_map, y_map)):
break
val = map2d.get_val_via_map_coord(x_map=x_map, y_map=y_map)
if (val != 1.0):
map2d.update_val_via_map_coord(x_map=x_map,
y_map=y_map,
value=0.0)
x_hit_map = my_round(path[len(path) - 1].x)
y_hit_map = my_round(path[len(path) - 1].y)
if (map2d.is_valid_map_coord(x_hit_map, y_hit_map)
and scans.ranges[scan_idx] < scans.max_range):
map2d.update_val_via_map_coord(x_map=x_hit_map,
y_map=y_hit_map,
value=1.0)
def register_scan(self, *, pose, scans, map2d):
conf = map2d.get_map_config()
# Convert coordinate from global to map.
x_map_scan_st, y_map_scan_st = \
GridMap2D.convert_global_2_map(pose.x, pose.y, conf.x_min, conf.y_min, conf.reso)
map_scan_st = MapIntCoord2D(x=x_map_scan_st, y=y_map_scan_st)
map_scan_ends_world, map_scan_ends_map = self._convert_scan_hit_pnt_2_world_and_map_pnt(
pose, scans, conf)
min_pnt = MapIntCoord2D(x=0, y=0)
max_pnt = MapIntCoord2D(x=conf.x_tick_num - 1, y=conf.y_tick_num - 1)
for scan_idx, end_coord in enumerate(map_scan_ends_map):
if (map_scan_st.x == end_coord.x and map_scan_st.y == end_coord.y):
continue
path = RayTracing2D.ray_tracing(map_scan_st, end_coord, min_pnt,
max_pnt)
for coord in path:
x_world, y_world = \
GridMap2D.convert_map_2_global(\
coord.x, coord.y, conf.x_min, conf.y_min, conf.reso)
tgt_world = Coord2D(x=x_world, y=y_world)
add_log = self.log_likelihood(\
pose=pose, \
tgt=tgt_world, \
scanned_range=scans.ranges[scan_idx])
val = map2d.get_val_via_map_coord(x_map=coord.x, y_map=coord.y) \
+ add_log - conf.init_val
map2d.update_val_via_map_coord(x_map=coord.x,
y_map=coord.y,
value=val)
def ray_tracing(st_pnt, end_pnt, min_pnt, max_pnt):
next_t_dist_ver, dt_dx, proc_dir_x, dist_x =\
RayTracing2D.__calc_t_dist_to_next_border(st_pnt.x, end_pnt.x)
next_t_dist_hor, dt_dy, proc_dir_y, dist_y =\
RayTracing2D.__calc_t_dist_to_next_border(st_pnt.y, end_pnt.y)
total_dist = dist_x + dist_y + 1
x = my_math.my_round(st_pnt.x)
y = my_math.my_round(st_pnt.y)
path = np.zeros((2, total_dist), dtype=int)
path = [MapIntCoord2D() for i in range(total_dist)]
path[0].x = x
path[0].y = y
cnt = 1
for i in range(total_dist - 1, 0, -1):
if (next_t_dist_ver < next_t_dist_hor):
x = x + proc_dir_x
next_t_dist_ver = next_t_dist_ver + dt_dx
else:
y = y + proc_dir_y
next_t_dist_hor = next_t_dist_hor + dt_dy
if ((x < min_pnt.x or max_pnt.x < x) or \
(y < min_pnt.y or max_pnt.y < y)):
break
path[cnt].x = x
path[cnt].y = y
cnt = cnt + 1
return path[0:cnt]
cnt = 1
for i in range(total_dist - 1, 0, -1):
if (next_t_dist_ver < next_t_dist_hor):
x = x + proc_dir_x
next_t_dist_ver = next_t_dist_ver + dt_dx
else:
y = y + proc_dir_y
next_t_dist_hor = next_t_dist_hor + dt_dy
if ((x < min_pnt.x or max_pnt.x < x) or \
(y < min_pnt.y or max_pnt.y < y)):
break
path[cnt].x = x
path[cnt].y = y
cnt = cnt + 1
return path[0:cnt]
if __name__ == "__main__":
st_pnt = MapIntCoord2D(x=0, y=0)
end_pnt = MapIntCoord2D(x=20, y=5)
min_pnt = MapIntCoord2D(x=0, y=0)
max_pnt = MapIntCoord2D(x=10, y=10)
path = RayTracing2D.ray_tracing(st_pnt, end_pnt, min_pnt, max_pnt)
for item in path:
print(item)