def __init__(self, robot: cozmo.robot.Robot):
self.current_arena_pose = None
self.current_robot_pose = robot.pose
self.robot = robot
# start streaming
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
# Obtain the camera intrinsics matrix
fx, fy = robot.camera.config.focal_length.x_y
cx, cy = robot.camera.config.center.x_y
self.camera_settings = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]],
dtype=np.float)
self.pick_up_pose = Pose(x=4.5, y=13.75, z=0, angle_z=degrees(90))
self.drop_off_pose = Pose(x=21.75, y=13.75, z=0, angle_z=degrees(90))
self.drop_off_directions = [
Pose(x=3, y=4.5, z=0, angle_z=degrees(0)),
Pose(x=21.75, y=4.5, z=0, angle_z=degrees(90)), self.drop_off_pose
]
self.pick_up_directions = [
Pose(x=21.75, y=4.5, z=0, angle_z=degrees(90)),
Pose(x=3, y=4.5, z=0, angle_z=degrees(0)), self.pick_up_pose
]
self.drive_speed = speed_mmps(50)
print("Robot initialized!")
self.grid = CozGrid("map_arena.json")
self.pf = ParticleFilter(self.grid)
print("Robot initialized!")
threading.Thread(target=self.runGUI).start()
def __init__(self, robot: cozmo.robot.Robot):
self.current_arena_pose = None
self.last_robot_pose = robot.pose
self.robot = robot
# start streaming
await robot.set_head_angle(degrees(3)).wait_for_completed()
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
# Obtain the camera intrinsics matrix
fx, fy = robot.camera.config.focal_length.x_y
cx, cy = robot.camera.config.center.x_y
self.camera_settings = np.array([
[fx, 0, cx],
[ 0, fy, cy],
[ 0, 0, 1]
], dtype=np.float)
self.grid = CozGrid("map_arena.json")
self.pf = ParticleFilter(self.grid)
self.gui = GUIWindow(self.grid, show_camera=True)
self.pick_up_pose = Pose(x=4.5, y=20, 0, angle_z=degrees(90))
self.drop_off_pose = Pose(x=21.75, y=13.75, 0, angle_z=degrees(90))
self.drop_off_directions = [Pose(x=3, y=4.5, 0, angle_z=degrees(0)), Pose(x=21.75, y=4.5, 0, angle_z=degrees(90)), self.drop_off_pose]
self.pick_up_directions = [Pose(x=21.75, y=4.5, 0, angle_z=degrees(90)), Pose(x=3, y=4.5, 0, angle_z=degrees(0)), self.pick_up_pose]
# ---------- Sensor (markers) model update ----------
self.particles = measurement_update(self.particles, r_marker_list, self.grid)
# ---------- Show current state ----------
# Try to find current best estimate for display
m_x, m_y, m_h, m_confident = compute_mean_pose(self.particles)
return (m_x, m_y, m_h, m_confident)
# tmp cache
last_pose = cozmo.util.Pose(0,0,0,angle_z=cozmo.util.Angle(degrees=0))
flag_odom_init = False
# map
Map_filename = "map_arena.json"
grid = CozGrid(Map_filename)
gui = GUIWindow(grid, show_camera=True)
pf = ParticleFilter(grid)
def compute_odometry(curr_pose, cvt_inch=True):
'''
Compute the odometry given the current pose of the robot (use robot.pose)
Input:
- curr_pose: a cozmo.robot.Pose representing the robot's current location
- cvt_inch: converts the odometry into grid units
Returns:
- 3-tuple (dx, dy, dh) representing the odometry
'''
global last_pose, flag_odom_init
def run_test_case(Robot_init_pose, Dh_circular, Robot_speed):
grid = CozGrid(Map_filename)
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(setting.PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid)
score = 0
# 1. steps to build tracking
steps_built_track = 9999
for i in range(0, Steps_build_tracking):
est_pose = particlefilter.update()
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot \
and i+1 < steps_built_track:
steps_built_track = i + 1
#print("steps_built_track =", steps_built_track)
if steps_built_track < 50:
score = 45
elif steps_built_track < Steps_build_tracking:
score = Steps_build_tracking - steps_built_track
else:
score = 0
print("\nPhase 1")
print("Number of steps to build track :", steps_built_track, "/",
Steps_build_tracking)
acc_err_trans, acc_err_rot = 0.0, 0.0
max_err_trans, max_err_rot = 0.0, 0.0
step_track = 0
# 2. test tracking
score_per_track = 45.0 / Steps_stable_tracking
for i in range(0, Steps_stable_tracking):
est_pose = particlefilter.update()
err_trans = grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y)
acc_err_trans += err_trans
if max_err_trans < err_trans:
max_err_trans = err_trans
err_rot = math.fabs(diff_heading_deg(est_pose[2], robbie.h))
acc_err_rot += err_rot
if max_err_rot < err_rot:
max_err_rot = err_rot
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot:
step_track += 1
score += score_per_track
print("\nPhase 2")
print("Number of steps error in threshold :", step_track, "/",
Steps_stable_tracking)
print("Average translational error :",
acc_err_trans / Steps_stable_tracking)
print("Average rotational error :", acc_err_rot / Steps_stable_tracking,
"deg")
print("Max translational error :", max_err_trans)
print("Max rotational error :", max_err_rot, "deg")
return score
def grade(Robot_init_pose, Dh_circular, Robot_speed):
grid = CozGrid(Map_filename)
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid)
score = 0
# 1. steps to build tracking
steps_built_track = 9999
for i in range(0, Steps_build_tracking):
est_pose = particlefilter.update(Dh_circular, Robot_speed)
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot \
and i+1 < steps_built_track:
steps_built_track = i + 1
#print("steps_built_track =", steps_built_track)
if steps_built_track < 50:
score = 50
elif steps_built_track < 100:
score = 100 - steps_built_track
else:
score = 0
print("\nPhrase 1")
print("Number of steps to build track :", steps_built_track, "/",
Steps_build_tracking)
acc_err_trans, acc_err_rot = 0.0, 0.0
max_err_trans, max_err_rot = 0.0, 0.0
step_track = 0
# 2. test tracking
for i in range(0, Steps_stable_tracking):
est_pose = particlefilter.update(Dh_circular, Robot_speed)
err_trans = grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y)
acc_err_trans += err_trans
if max_err_trans < err_trans:
max_err_trans = err_trans
err_rot = math.fabs(diff_heading_deg(est_pose[2], robbie.h))
acc_err_rot += err_rot
if max_err_rot < err_rot:
max_err_rot = err_rot
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot:
step_track += 1
if step_track > 90:
score += 50
elif step_track > 40:
score += step_track - 40
else:
score += 0
print("\nPhrase 2")
print("Number of steps error in threshold :", step_track, "/",
Steps_stable_tracking)
print("Average translational error :{:.2f}".format(acc_err_trans /
Steps_stable_tracking))
print("Average rotational error :{:.2f} deg".format(acc_err_rot /
Steps_stable_tracking))
print("Max translational error :{:.2f}".format(max_err_trans))
print("Max rotational error :{:.2f} deg".format(max_err_rot))
print("\nexample score =", score)
return score
