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]
def __init__(self, save):
# If save is NONE, __init__ is called for default values, otherwise data are loaded from file.
self.player = Player(save)
self.world = World(save)
#Build GUI
self.gui = GUIWindow(self.world.map)
self.gui.after(int(1000 / 20), self.call)
self.gui.bind('<Key>', self.keypress)
self.gui.map.bind('<B1-Motion>', self.map_pan)
self.gui.map.bind('<MouseWheel>', self.map_zoom)
self.gui.run()
def __init__(self, robot: cozmo.robot.Robot):
self.current_arena_pose = None
self.last_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()
print("Robot initialized!")
# 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=20, 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)
self.grid = CozGrid("map_arena.json")
self.pf = ParticleFilter(self.grid)
self.gui = GUIWindow(self.grid, show_camera=True)
self.gui.show_particles(self.pf.particles)
self.gui.show_mean(0, 0, 0)
self.gui.start()
# ---------- 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
last_x, last_y, last_h = last_pose.position.x, last_pose.position.y, \
def runGUI(self):
self.gui = GUIWindow(self.grid, show_camera=True)
self.gui.show_particles(self.pf.particles)
self.gui.show_mean(0, 0, 0)
self.gui.start()
threading.Thread.__init__(self, daemon=True)
self.filter = particle_filter
self.gui = gui
def run(self):
while True:
estimated = self.filter.update()
self.gui.show_particles(self.filter.particles)
self.gui.show_mean(estimated[0], estimated[1], estimated[2],
estimated[3])
self.gui.show_robot(self.filter.robbie)
self.gui.updated.set()
if __name__ == "__main__":
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)
if Use_GUI:
gui = GUIWindow(grid)
filter_thread = ParticleFilterThread(particlefilter, gui)
filter_thread.start()
gui.start()
else:
while True:
particlefilter.update()
def main(arg):
app = QApplication(arg)
gui = GUIWindow()
sys.exit(app.exec_())