PYTHON   19

lab3a

Guest on 2nd August 2022 12:40:50 AM

  1. from cozmo_fsm import *
  2. from cozmo.util import degrees, Pose
  3.  
  4. ARUCO_MARKER_SIZE = 50   # millimeters
  5.  
  6. class Lab3a(StateMachineProgram):
  7.     def __init__(self):
  8.         landmarks = {
  9.             'Aruco-0' : Pose(-55, 160, 0, angle_z=degrees(-90)),
  10.             'Aruco-1' : Pose( 55, 160, 0, angle_z=degrees(-90)),
  11.             'Aruco-2' : Pose(160,  55, 0, angle_z=degrees(180)),
  12.             'Aruco-3' : Pose(160, -55, 0, angle_z=degrees(180))
  13.         }
  14.  
  15.         pf = ParticleFilter(robot,
  16.                             landmarks = landmarks,
  17.                             #sensor_model = ArucoDistanceSensorModel(robot)
  18.                             sensor_model = ArucoBearingSensorModel(robot)
  19.                             #sensor_model = ArucoCombinedSensorModel(robot)
  20.         )
  21.  
  22.         super().__init__(aruco_marker_size=ARUCO_MARKER_SIZE,
  23.                          particle_filter=pf)

Raw Paste


Login or Register to edit or fork this paste. It's free.