def test_conv_pnt_global_2_local1(self):
local_fr = Pose2D(1, 1, 45.0/180.0 * math.pi)
glo_pnt = Coord2D(x=3.0, y=1.0)
loc_pnt = conv_pnt_global_2_local(local_fr, glo_pnt)
self.assertAlmostEqual(loc_pnt.x, math.sqrt(2), places=5)
self.assertAlmostEqual(loc_pnt.y, -math.sqrt(2), places=5)
def test_conv_pnt_local_2_global(self):
local_fr = Pose2D(1, 1, 45.0/180.0 * math.pi)
loc_pnt = Coord2D(x=math.sqrt(2), y=-math.sqrt(2))
glo_pnt = conv_pnt_local_2_global(local_fr, loc_pnt)
self.assertAlmostEqual(glo_pnt.x, 3.0, places=5)
self.assertAlmostEqual(glo_pnt.y, 1.0, places=5)
def conv_pnt_local_2_global(local_fr, loc_pnt): theta = math.atan2(loc_pnt.y, loc_pnt.x) + local_fr.theta r = math.sqrt(loc_pnt.x**2 + loc_pnt.y**2) dx = r * math.cos(theta) dy = r * math.sin(theta) return Coord2D(x=local_fr.x + dx, y=local_fr.y + dy)
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 conv_pnt_global_2_local(local_fr, glob_pnt): dx = glob_pnt.x - local_fr.x dy = glob_pnt.y - local_fr.y phi = math.atan2(dy, dx) - local_fr.theta r = math.sqrt(dx**2 + dy**2) x = r * math.cos(phi) y = r * math.sin(phi) return Coord2D(x=x, y=y)
def create_robocup_field(map2d):
poles = [Coord2D(x=2.2, y=1.45),\
Coord2D(x=0, y=1.45), \
Coord2D(x=-2.2, y=1.45), \
Coord2D(x=-2.2, y=-1.45), \
Coord2D(x=0, y=-1.45), \
Coord2D(x=2.2, y=-1.45)]
for pole in poles:
GridMap2DDrawer.draw_point(map2d, pole.x, pole.y, 0.10, 1.0)
return poles
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)