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
last_pose = Pose2D()
for index, cur_pose in enumerate(path):
print("Loop {0}".format(str(index)))
if index == 0:
last_pose = cur_pose
for i in range(particle_num):
cur_particle_poses[i] = cur_pose
continue
# Sample Artificial Odometry Error
cur_particle_poses = \
err_model.sample_motion(cur_odom=cur_pose, \
last_odom=last_pose, \
last_particle_poses=cur_particle_poses)
last_pose = cur_pose
# Update Odometry Map
for i in range(particle_num):
GridMap2DDrawer.draw_point(grid2d_pose, \
cur_particle_poses[i].x, \
cur_particle_poses[i].y, \
0.0, 1.0)
grid2d_pose.show_heatmap(ax_main)
# Pause for Visualization Update
if (index % 30 == 0):
plt.pause(0.1)
grid2d_gt.show_heatmap(ax00)
grid2d_odom_disp.show_heatmap(ax10)
for index, pose in enumerate(path):
print("Loop : {0}".format(str(index)))
# Generate Artificial Scan
start_time = time.time()
scans = v_lidar2d.generate_scans(pose, grid2d_gt)
fake_scan_time = time.time() - start_time
# Register Scans
start_time = time.time()
#grid2d_scanmap.register_scan(pose=pose, scans=scans)
inv_sens_model.register_fixed_scan(pose=pose,
scans=scans,
map2d=grid2d_scanmap)
scan_registration_time = time.time() - start_time
# Visualization.
grid2d_scanmap.show_heatmap(ax_scanmap)
GridMap2DDrawer.draw_point(grid2d_pose, pose.x, pose.y, 0.2, 1.0)
grid2d_pose.show_heatmap(ax20)
plt.pause(0.1)
print("fake_scan_time : {0}".format(fake_scan_time) + "[sec]")
print("registration_time : {0}".format(scan_registration_time) +
"[sec]")
def create_sample_robot_path(map2d, trans_reso=1.0, angle_reso=math.pi / 2.0):
path = []
for i in range(34 * 4):
path.append(Pose2D(-49 + i * 0.25, -17.5, 0))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.1))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.2))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.3))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.4))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.5))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.6))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.7))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.9))
path.append(Pose2D(-15, -17.5, math.pi / 2.0))
for i in range(35 * 4):
path.append(Pose2D(-15, -17.5 + i * 0.25, math.pi / 2.0))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.1))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.2))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.3))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.4))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.5))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.6))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.7))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.8))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.9))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 1.0))
for i in range(22 * 4):
path.append(Pose2D(-15 - i * 0.25, 17.5, math.pi))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.1))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.2))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.3))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.4))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.5))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.6))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.7))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.8))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.9))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 1.0))
for i in range(35 * 4):
path.append(Pose2D(-37, 17.5 - i * 0.25, math.pi * 3.0 / 2.0))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.1))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.2))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.3))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.4))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.5))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.6))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.7))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.8))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.9))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 1.0))
for i in range(61 * 4):
path.append(Pose2D(-37 + i * 0.25, -17.5, 0))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.1))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.2))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.3))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.4))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.5))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.6))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.7))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.9))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 1.0))
for i in range(35 * 4):
path.append(Pose2D(24, -17.5 + i * 0.25, math.pi / 2.0))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.1))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.2))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.3))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.4))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.5))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.6))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.7))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.8))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.9))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 1.0))
for i in range(20 * 4):
path.append(Pose2D(24 - i * 0.25, 17.5, math.pi))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.1))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.2))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.3))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.4))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.5))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.6))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.7))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.8))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.9))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 1.0))
for i in range(32 * 4):
path.append(Pose2D(4, 17.5 - i * 0.25, math.pi * 3.0 / 2.0))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.1))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.2))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.3))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.4))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.5))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.6))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.7))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.8))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.9))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 1.0))
for i in range(18 * 4):
path.append(Pose2D(4 + i * 0.25, -14.5, 0))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.1))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.2))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.3))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.4))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.5))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.6))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.7))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.9))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 1.0))
for i in range(30 * 4):
path.append(Pose2D(22, -14.5 + i * 0.25, math.pi / 2.0))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.9))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.7))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.5))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.4))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.3))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.2))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.1))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.0))
for i in range(27 * 4):
path.append(Pose2D(22 + i * 0.25, 15.5, 0))
for pose in path:
GridMap2DDrawer.draw_point(map2d, pose.x, pose.y, 0.25, 1.0)
return path
def draw_path(path, map2d):
for pose in path:
GridMap2DDrawer.draw_point(map2d, pose.x, pose.y, 0.04, 1.0)
err_omg_v=err_omg_v, err_omg_omg=err_omg_omg,
err_r=0.01, err_phi=0.05, err_feat=1.0)
ekf_localizer = EKFLocalizerKC(config=ekf_conf, landmarks=poles, ini_pose=ellipse_path[0])
mm_pose_list = []
true_pose_list = []
ekf_pose_list = []
for index, true_pose in enumerate(ellipse_path):
ctrl = controls[index-1]
noised_ctrl = generate_noised_control(ctrl, err_v_v, err_v_omg, err_omg_v, err_omg_omg)
# Generate Detection
idx_list, meas_list = vir_lm_det.detect(pose=true_pose)
meas = EKFKCMeas(corresp_list=idx_list, meas_list=meas_list)
# Localize by EKF
ekf_pose, sig, likelihood = ekf_localizer.localize(control=noised_ctrl, meas=meas)
mm_pose = ekf_localizer.get_motion_model_pose()
if (index % 25 == 0):
GridMap2DDrawer.draw_point(grid2d_loc, ekf_pose.x, ekf_pose.y, 0.04, 1.0)
mm_pose_list.append(mm_pose)
true_pose_list.append(true_pose)
ekf_pose_list.append(ekf_pose)
plt.cla()
draw_result(plt, true_pose_list, mm_pose_list, ekf_pose_list, poles, idx_list)
plt.pause(0.1)
plt.show()