├── .gitignore ├── cozmo_fsm ├── .gitignore ├── docs │ └── perched_cameras_v0.3.pdf ├── README.md ├── examples │ ├── __init__.py │ ├── Look5.fsm │ ├── Greet.fsm │ ├── Texting.fsm │ ├── PF_Cube.py │ ├── BackItUp.fsm │ ├── Iteration.fsm │ ├── Nested.fsm │ ├── Greet.py │ ├── Look5.py │ ├── PF_Aruco.py │ ├── TapSpeak.fsm │ ├── CV_Canny.py │ ├── CV_GoodFeatures.py │ ├── BackItUp.py │ ├── TapSpeak.py │ ├── Randomness.fsm │ ├── Nested.py │ ├── Iteration.py │ ├── CV_Contour.py │ ├── Texting.py │ ├── Boo.fsm │ ├── CV_OpticalFlow.py │ ├── Randomness.py │ ├── CV_Hough.py │ └── Boo.py ├── __init__.py ├── path-planner-issue.txt ├── trace.py ├── TODO.txt ├── opengl.py ├── sim_robot.py ├── pilot0.py ├── custom_objs.py ├── kine.py ├── events.py ├── aruco.py ├── speech.py ├── wall_defs.py ├── cozmo_kin.py ├── perched.py ├── rrt_shapes.py ├── cam_viewer.py ├── evbase.py ├── doorpass.fsm ├── base.py └── transitions.py ├── walls ├── Wall39.odp ├── Wall39.pdf ├── Wall45.odp └── Wall45.pdf ├── aruco ├── tags │ ├── aruco_4x4_100.zip │ └── aruco_4x4_100 │ │ ├── aruco4x4_100_0.jpg │ │ ├── aruco4x4_100_1.jpg │ │ ├── aruco4x4_100_10.jpg │ │ ├── aruco4x4_100_11.jpg │ │ ├── aruco4x4_100_12.jpg │ │ ├── aruco4x4_100_13.jpg │ │ ├── aruco4x4_100_14.jpg │ │ ├── aruco4x4_100_15.jpg │ │ ├── aruco4x4_100_16.jpg │ │ ├── aruco4x4_100_17.jpg │ │ ├── aruco4x4_100_18.jpg │ │ ├── aruco4x4_100_19.jpg │ │ ├── aruco4x4_100_2.jpg │ │ ├── aruco4x4_100_20.jpg │ │ ├── aruco4x4_100_21.jpg │ │ ├── aruco4x4_100_22.jpg │ │ ├── aruco4x4_100_23.jpg │ │ ├── aruco4x4_100_24.jpg │ │ ├── aruco4x4_100_25.jpg │ │ ├── aruco4x4_100_26.jpg │ │ ├── aruco4x4_100_27.jpg │ │ ├── aruco4x4_100_28.jpg │ │ ├── aruco4x4_100_29.jpg │ │ ├── aruco4x4_100_3.jpg │ │ ├── aruco4x4_100_30.jpg │ │ ├── aruco4x4_100_31.jpg │ │ ├── aruco4x4_100_32.jpg │ │ ├── aruco4x4_100_33.jpg │ │ ├── aruco4x4_100_34.jpg │ │ ├── aruco4x4_100_35.jpg │ │ ├── aruco4x4_100_36.jpg │ │ ├── aruco4x4_100_37.jpg │ │ ├── aruco4x4_100_38.jpg │ │ ├── aruco4x4_100_39.jpg │ │ ├── aruco4x4_100_4.jpg │ │ ├── aruco4x4_100_40.jpg │ │ ├── aruco4x4_100_41.jpg │ │ ├── aruco4x4_100_42.jpg │ │ ├── aruco4x4_100_43.jpg │ │ ├── aruco4x4_100_44.jpg │ │ ├── aruco4x4_100_45.jpg │ │ ├── aruco4x4_100_46.jpg │ │ ├── aruco4x4_100_47.jpg │ │ ├── aruco4x4_100_48.jpg │ │ ├── aruco4x4_100_49.jpg │ │ ├── aruco4x4_100_5.jpg │ │ ├── aruco4x4_100_50.jpg │ │ ├── aruco4x4_100_51.jpg │ │ ├── aruco4x4_100_52.jpg │ │ ├── aruco4x4_100_53.jpg │ │ ├── aruco4x4_100_54.jpg │ │ ├── aruco4x4_100_55.jpg │ │ ├── aruco4x4_100_56.jpg │ │ ├── aruco4x4_100_57.jpg │ │ ├── aruco4x4_100_58.jpg │ │ ├── aruco4x4_100_59.jpg │ │ ├── aruco4x4_100_6.jpg │ │ ├── aruco4x4_100_60.jpg │ │ ├── aruco4x4_100_61.jpg │ │ ├── aruco4x4_100_62.jpg │ │ ├── aruco4x4_100_63.jpg │ │ ├── aruco4x4_100_64.jpg │ │ ├── aruco4x4_100_65.jpg │ │ ├── aruco4x4_100_66.jpg │ │ ├── aruco4x4_100_67.jpg │ │ ├── aruco4x4_100_68.jpg │ │ ├── aruco4x4_100_69.jpg │ │ ├── aruco4x4_100_7.jpg │ │ ├── aruco4x4_100_70.jpg │ │ ├── aruco4x4_100_71.jpg │ │ ├── aruco4x4_100_72.jpg │ │ ├── aruco4x4_100_73.jpg │ │ ├── aruco4x4_100_74.jpg │ │ ├── aruco4x4_100_75.jpg │ │ ├── aruco4x4_100_76.jpg │ │ ├── aruco4x4_100_77.jpg │ │ ├── aruco4x4_100_78.jpg │ │ ├── aruco4x4_100_79.jpg │ │ ├── aruco4x4_100_8.jpg │ │ ├── aruco4x4_100_80.jpg │ │ ├── aruco4x4_100_81.jpg │ │ ├── aruco4x4_100_82.jpg │ │ ├── aruco4x4_100_83.jpg │ │ ├── aruco4x4_100_84.jpg │ │ ├── aruco4x4_100_85.jpg │ │ ├── aruco4x4_100_86.jpg │ │ ├── aruco4x4_100_87.jpg │ │ ├── aruco4x4_100_88.jpg │ │ ├── aruco4x4_100_89.jpg │ │ ├── aruco4x4_100_9.jpg │ │ ├── aruco4x4_100_90.jpg │ │ ├── aruco4x4_100_91.jpg │ │ ├── aruco4x4_100_92.jpg │ │ ├── aruco4x4_100_93.jpg │ │ ├── aruco4x4_100_94.jpg │ │ ├── aruco4x4_100_95.jpg │ │ ├── aruco4x4_100_96.jpg │ │ ├── aruco4x4_100_97.jpg │ │ ├── aruco4x4_100_98.jpg │ │ └── aruco4x4_100_99.jpg ├── sheets │ └── aruco_4x4_100_44mm │ │ ├── aruco-44mm-001-020.odg │ │ ├── aruco-44mm-001-020.pdf │ │ ├── aruco-44mm-021-040.odg │ │ ├── aruco-44mm-021-040.pdf │ │ ├── aruco-44mm-041-060.odg │ │ ├── aruco-44mm-041-060.pdf │ │ ├── aruco-44mm-061-080.odg │ │ ├── aruco-44mm-061-080.pdf │ │ ├── aruco-44mm-081-099.odg │ │ ├── aruco-44mm-081-099.pdf │ │ └── README.txt └── generatetags.py ├── custom_markers ├── SDK_2Circles.png ├── SDK_2Diamonds.png ├── SDK_2Hexagons.png ├── SDK_3Circles.png ├── SDK_3Diamonds.png ├── SDK_3Hexagons.png ├── SDK_4Circles.png ├── SDK_4Diamonds.png ├── SDK_4Hexagons.png ├── SDK_5Circles.png ├── SDK_5Diamonds.png ├── SDK_5Hexagons.png ├── SDK_2Triangles.png ├── SDK_3Triangles.png ├── SDK_4Triangles.png └── SDK_5Triangles.png ├── requirements.txt ├── README.md ├── INSTALL.txt └── event_monitor.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | *.pyc 3 | *.swp 4 | *.swo 5 | *~ 6 | 7 | -------------------------------------------------------------------------------- /cozmo_fsm/.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | *.pyc 3 | *.swp 4 | *.swo 5 | *~ 6 | cozmo 7 | 8 | -------------------------------------------------------------------------------- /walls/Wall39.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/walls/Wall39.odp -------------------------------------------------------------------------------- /walls/Wall39.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/walls/Wall39.pdf -------------------------------------------------------------------------------- /walls/Wall45.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/walls/Wall45.odp -------------------------------------------------------------------------------- /walls/Wall45.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/walls/Wall45.pdf -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100.zip -------------------------------------------------------------------------------- /custom_markers/SDK_2Circles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_2Circles.png -------------------------------------------------------------------------------- /custom_markers/SDK_2Diamonds.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_2Diamonds.png -------------------------------------------------------------------------------- /custom_markers/SDK_2Hexagons.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_2Hexagons.png -------------------------------------------------------------------------------- /custom_markers/SDK_3Circles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_3Circles.png -------------------------------------------------------------------------------- /custom_markers/SDK_3Diamonds.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_3Diamonds.png -------------------------------------------------------------------------------- /custom_markers/SDK_3Hexagons.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_3Hexagons.png -------------------------------------------------------------------------------- /custom_markers/SDK_4Circles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_4Circles.png -------------------------------------------------------------------------------- /custom_markers/SDK_4Diamonds.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_4Diamonds.png -------------------------------------------------------------------------------- /custom_markers/SDK_4Hexagons.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_4Hexagons.png -------------------------------------------------------------------------------- /custom_markers/SDK_5Circles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_5Circles.png -------------------------------------------------------------------------------- /custom_markers/SDK_5Diamonds.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_5Diamonds.png -------------------------------------------------------------------------------- /custom_markers/SDK_5Hexagons.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_5Hexagons.png -------------------------------------------------------------------------------- /custom_markers/SDK_2Triangles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_2Triangles.png -------------------------------------------------------------------------------- /custom_markers/SDK_3Triangles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_3Triangles.png -------------------------------------------------------------------------------- /custom_markers/SDK_4Triangles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_4Triangles.png -------------------------------------------------------------------------------- /custom_markers/SDK_5Triangles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/custom_markers/SDK_5Triangles.png -------------------------------------------------------------------------------- /cozmo_fsm/docs/perched_cameras_v0.3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/cozmo_fsm/docs/perched_cameras_v0.3.pdf -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_0.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_0.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_1.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_10.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_11.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_12.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_13.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_14.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_14.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_15.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_16.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_16.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_17.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_17.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_18.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_18.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_19.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_19.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_2.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_20.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_20.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_21.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_21.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_22.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_22.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_23.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_23.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_24.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_24.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_25.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_25.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_26.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_26.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_27.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_27.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_28.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_28.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_29.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_29.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_3.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_30.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_30.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_31.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_31.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_32.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_32.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_33.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_33.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_34.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_34.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_35.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_35.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_36.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_36.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_37.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_37.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_38.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_38.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_39.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_39.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_4.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_40.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_40.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_41.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_41.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_42.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_42.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_43.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_43.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_44.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_44.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_45.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_45.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_46.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_46.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_47.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_47.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_48.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_48.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_49.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_49.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_5.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_50.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_50.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_51.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_51.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_52.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_52.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_53.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_53.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_54.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_54.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_55.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_55.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_56.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_56.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_57.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_57.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_58.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_58.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_59.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_59.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_6.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_60.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_60.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_61.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_61.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_62.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_62.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_63.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_63.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_64.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_64.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_65.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_65.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_66.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_66.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_67.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_67.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_68.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_68.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_69.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_69.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_7.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_70.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_70.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_71.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_71.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_72.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_72.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_73.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_73.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_74.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_74.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_75.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_75.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_76.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_76.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_77.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_77.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_78.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_78.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_79.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_79.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_8.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_80.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_80.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_81.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_81.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_82.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_82.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_83.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_83.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_84.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_84.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_85.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_85.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_86.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_86.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_87.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_87.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_88.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_88.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_89.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_89.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_9.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_9.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_90.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_90.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_91.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_91.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_92.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_92.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_93.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_93.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_94.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_94.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_95.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_95.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_96.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_96.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_97.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_97.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_98.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_98.jpg -------------------------------------------------------------------------------- /aruco/tags/aruco_4x4_100/aruco4x4_100_99.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/tags/aruco_4x4_100/aruco4x4_100_99.jpg -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-001-020.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-001-020.odg -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-001-020.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-001-020.pdf -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-021-040.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-021-040.odg -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-021-040.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-021-040.pdf -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-041-060.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-041-060.odg -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-041-060.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-041-060.pdf -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-061-080.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-061-080.odg -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-061-080.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-061-080.pdf -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-081-099.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-081-099.odg -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-081-099.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/touretzkyds/cozmo-tools/HEAD/aruco/sheets/aruco_4x4_100_44mm/aruco-44mm-081-099.pdf -------------------------------------------------------------------------------- /aruco/sheets/aruco_4x4_100_44mm/README.txt: -------------------------------------------------------------------------------- 1 | These markers are 44 mm square and are designed to be printed on 2 | 2 inch square adhesive labels. Marker 00 appears after Marker 99 3 | on the last sheet. 4 | -------------------------------------------------------------------------------- /cozmo_fsm/README.md: -------------------------------------------------------------------------------- 1 | # cozmo_fsm 2 | 3 | ## Finite State Machine package for Cozmo, inspired by Tekkotsu. 4 | 5 | This package is still in development. More documentation will be 6 | provided soon. 7 | 8 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | cozmo 2 | numpy 3 | Pillow<10.0.0 4 | opencv-python 5 | opencv-contrib-python 6 | colorama 7 | termcolor 8 | PyOpenGL 9 | PyOpenGL_accelerate 10 | pyreadline 11 | SpeechRecognition 12 | PyAudio 13 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example demos for the cozmo_fsm finite state machine package 3 | """ 4 | 5 | from . import BackItUp 6 | from . import Boo 7 | from . import Greet 8 | from . import Look5 9 | from . import Nested 10 | from . import TapSpeak 11 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Look5.fsm: -------------------------------------------------------------------------------- 1 | """ 2 | Example of starting a behavior and indicating that it should not be 3 | automatically stopped when the behavior node is exited. We later 4 | use StopBehavior() to stop the behavior. 5 | """ 6 | 7 | from cozmo_fsm import * 8 | 9 | class Look5(StateMachineProgram): 10 | $setup { 11 | LookAroundInPlace(stop_on_exit=False) 12 | =T(5)=> Say("I'm almost done") 13 | =T(5)=> StopBehavior() 14 | } 15 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Greet.fsm: -------------------------------------------------------------------------------- 1 | """ 2 | The Greet demo illustrates the use of CompletionTrans and TimerTrans 3 | transitions. 4 | 5 | Behavior: Cozmo starts out by saying 'Greetings, human!'. After his 6 | speech has completed, he waits 5 seconds, then says 'Bye-bye now'. 7 | """ 8 | 9 | from cozmo_fsm import * 10 | 11 | class Greet(StateMachineProgram): 12 | $setup { 13 | say: Say('Greetings, human!') =C=> 14 | wait: StateNode() =T(5)=> 15 | say2: Say('Bye-bye now.') 16 | } 17 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Texting.fsm: -------------------------------------------------------------------------------- 1 | from cozmo_fsm import * 2 | 3 | class Texting(StateMachineProgram): 4 | $setup{ 5 | startnode: StateNode() 6 | startnode =TM('1')=> do_null 7 | startnode =TM('2')=> do_time 8 | startnode =TM('3')=> do_comp 9 | 10 | do_null: Say("Full steam ahead") =N=> Forward(20) =C=> startnode 11 | 12 | do_time: Say("Full steam ahead") =T(2)=> Forward(20) =C=> startnode 13 | 14 | do_comp: Say("Full steam ahead") =C=> Forward(20) =C=> startnode 15 | } 16 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/PF_Cube.py: -------------------------------------------------------------------------------- 1 | from cozmo_fsm import * 2 | from cozmo.util import degrees, Pose 3 | 4 | class PF_Cube(StateMachineProgram): 5 | def __init__(self): 6 | landmarks = { 7 | cube1 : Pose( 55, 160, 0, angle_z=degrees(90)), 8 | cube2 : Pose(160, 55, 0, angle_z=degrees( 0)), 9 | cube3 : Pose(160, -55, 0, angle_z=degrees( 0)) 10 | } 11 | pf = ParticleFilter(robot, 12 | landmarks = landmarks, 13 | sensor_model = CubeSensorModel(robot)) 14 | super().__init__(particle_filter=pf, particle_viewer=True) 15 | -------------------------------------------------------------------------------- /cozmo_fsm/__init__.py: -------------------------------------------------------------------------------- 1 | from cozmo.util import radians, degrees, Pose, Rotation 2 | 3 | from . import base 4 | from . import program 5 | base.program = program 6 | 7 | from .nodes import * 8 | from .transitions import * 9 | from .program import * 10 | from .trace import tracefsm 11 | from .particle import * 12 | from .particle_viewer import ParticleViewer 13 | from .path_planner import PathPlanner 14 | from .cozmo_kin import * 15 | from .rrt import * 16 | from .path_viewer import PathViewer 17 | from .speech import * 18 | from .worldmap import WorldMap 19 | from .worldmap_viewer import WorldMapViewer 20 | from .cam_viewer import CamViewer 21 | from .pilot import * 22 | from .pickup import * 23 | from .doorpass import * 24 | from . import wall_defs 25 | from . import custom_objs 26 | from .sim_robot import SimRobot 27 | 28 | del base 29 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/BackItUp.fsm: -------------------------------------------------------------------------------- 1 | """ 2 | The BackItUp demo illustrates the use of fork/join to launch 3 | parallel actions and synchronize them again. The fork is performed 4 | by the NullTrans transition with two destinations, while the join is 5 | performed by the CompletionTrans transition with two sources. 6 | 7 | Behavior: Cozmo backs up by 100 mm while simultaneously beeping. He 8 | uses DriveForward instead of Forward to avoid conflict with the Say 9 | action. When he's done backing up, he stops beeping and says 'Safety first'. 10 | """ 11 | 12 | from cozmo_fsm import * 13 | 14 | class BackItUp(StateMachineProgram): 15 | $setup { 16 | launcher: StateNode() =N=> {driver, speaker} 17 | 18 | driver: Forward(-100,10) 19 | speaker: Say('beep',duration_scalar=0.8,abort_on_stop=True) =C=> speaker 20 | 21 | {driver,speaker} =C=> finisher: Say('Safety first!') 22 | 23 | } 24 | -------------------------------------------------------------------------------- /cozmo_fsm/path-planner-issue.txt: -------------------------------------------------------------------------------- 1 | RRT planner should not try to use doorways as maneuverable space. Use 2 | wavefront for doorway passage. Instead of giving the doorways wider 3 | openings when RRT planning, we should block them off. 4 | 5 | Wavefront planner is failing when a sideways cube is close to the 6 | wall, even if there is room for the robot to fit. Perhaps walls are 7 | being inflated too thick? 8 | 9 | Should we restrict the RRT algorithm to a smaller box, since we only 10 | use it for short-range planning now? This might help prevent it from 11 | exceeding max_iter by exploring areas far from the robot and the goal. 12 | 13 | DriveContinuous should turn more slowly when the robot is carrying a 14 | cube, because it tends to overshoot. 15 | 16 | In CalypsoMachine.fsm, in the first line of the ExpressFrustration 17 | fsm, change =N=> to => and you'll get an error from genfsm that 18 | reports the wrong line as the location of the syntax error. 19 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Iteration.fsm: -------------------------------------------------------------------------------- 1 | """ 2 | Iteration.fsm demonstrates nested iteration using the Iterate node 3 | and the =CNext=> transition, which waits for completion before advancing 4 | the iterator. Use =Next=> if the source nodes don't need to 5 | complete. 6 | """ 7 | 8 | from cozmo_fsm import * 9 | 10 | class PrintIt(StateNode): 11 | def start(self,event=None): 12 | if self.running: return 13 | super().start(event) 14 | if isinstance(event,DataEvent): 15 | print('I got some data: ', event.data) 16 | 17 | class Iteration(StateMachineProgram): 18 | $setup{ 19 | outer_loop: Iterate(['alpha', 'bravo', 'charlie']) 20 | outer_loop =SayData=> Say() =C=> inner_loop 21 | 22 | inner_loop: Iterate(4) =D=> PrintIt() =Next=> inner_loop 23 | # When inner iteration is done, it posts a completion event. 24 | inner_loop =CNext=> outer_loop 25 | 26 | outer_loop =C=> Say('Done') 27 | } 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # cozmo-tools 2 | 3 | See the INSTALL.txt file for installation instructions. 4 | 5 | For a radically different approach to Cozmo programming more suited to beginners, try Calypso at https://Calypso.software 6 | 7 | ## Tools for programming Anki's Cozmo robot via the Python SDK. 8 | 9 | * __simple_cli__ provides a _Command Line Interface_ for the Cozmo SDK 10 | so you can evaluate expressions in the context of an active SDK connection 11 | to a robot. It also provides a variety of visualization tools, such as a 12 | camera viewer, worldmap viewer, particle viewer, and path viewer. 13 | Run it by typing: `python3 simple_cli` 14 | 15 | * __cozmo_fsm__ is a Finite State Machine package for Cozmo programming. 16 | 17 | * __genfsm__ is a preprocessor that converts .fsm files written in 18 | the cozmo_fsm notation to .py files that are ready to run. 19 | 20 | __Note__: you can install most of the Python dependencies by simply running `pip3 install -r requirements.txt`, 21 | but see the INSTALL.txt file for some exceptions. 22 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Nested.fsm: -------------------------------------------------------------------------------- 1 | """ 2 | The Nested demo shows the use of nested state machines. We define a 3 | new node class DingDong that has a three-node state machine inside 4 | it. We then define the main class, Nested, whose state machine 5 | contains two instances of DingDong. DingDong uses a ParentCompletes 6 | node to cause DinDong to post a completion event, which allows 7 | Nested's first DingDong instance 'dd1' to move on to the next state, 8 | which is 'bridge'. (The 'dd2' instance of DingDong also posts a 9 | completion event, but nothing is listening for it.) 10 | 11 | Behavior: Cozmo says 'ding', then 'dong', then says 'once again' 12 | (that's the bridge), then 'ding', and then 'dong'. 13 | """ 14 | 15 | from cozmo_fsm import * 16 | 17 | class DingDong(StateNode): 18 | $setup { 19 | ding: Say('ding') =C=> dong: Say('dong') =C=> ParentCompletes() 20 | } 21 | 22 | class Nested(StateMachineProgram): 23 | $setup { 24 | dd1: DingDong() =C=> bridge: Say('once again') =C=> dd2: DingDong() 25 | } 26 | -------------------------------------------------------------------------------- /cozmo_fsm/trace.py: -------------------------------------------------------------------------------- 1 | """ 2 | Constants for defining tracing levels. 3 | """ 4 | 5 | class TRACE: 6 | def __init__(self): 7 | self._trace_level = 0 8 | 9 | @property 10 | def trace_level(self): return TRACE._trace_level 11 | @trace_level.setter 12 | def trace_level(self,val): 13 | TRACE._trace_level = val 14 | 15 | @property 16 | def no_tracing(self): return 0 17 | @property 18 | def statenode_start(self): return 1 19 | @property 20 | def statenode_startstop(self): return 2 21 | @property 22 | def transition_fire(self): return 3 23 | @property 24 | def transition_startstop(self): return 4 25 | @property 26 | def listener_invocation(self): return 5 27 | @property 28 | def polling(self): return 6 29 | @property 30 | def await_satisfied(self): return 7 31 | @property 32 | def event_posted(self): return 8 33 | @property 34 | def task_cancel(self): return 9 35 | 36 | TRACE = TRACE() 37 | 38 | def tracefsm(level=None): 39 | if level is not None: 40 | type(TRACE).trace_level = level 41 | else: 42 | return TRACE.trace_level 43 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Greet.py: -------------------------------------------------------------------------------- 1 | """ 2 | The Greet demo illustrates the use of CompletionTrans and TimerTrans 3 | transitions. 4 | 5 | Behavior: Cozmo starts out by saying 'Greetings, human!'. After his 6 | speech has completed, he waits 5 seconds, then says 'Bye-bye now'. 7 | """ 8 | 9 | from cozmo_fsm import * 10 | 11 | class Greet(StateMachineProgram): 12 | def setup(self): 13 | """ 14 | say: Say('Greetings, human!') =C=> 15 | wait: StateNode() =T(5)=> 16 | say2: Say('Bye-bye now.') 17 | """ 18 | 19 | # Code generated by genfsm on Mon Feb 17 03:12:53 2020: 20 | 21 | say = Say('Greetings, human!') .set_name("say") .set_parent(self) 22 | wait = StateNode() .set_name("wait") .set_parent(self) 23 | say2 = Say('Bye-bye now.') .set_name("say2") .set_parent(self) 24 | 25 | completiontrans1 = CompletionTrans() .set_name("completiontrans1") 26 | completiontrans1 .add_sources(say) .add_destinations(wait) 27 | 28 | timertrans1 = TimerTrans(5) .set_name("timertrans1") 29 | timertrans1 .add_sources(wait) .add_destinations(say2) 30 | 31 | return self 32 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Look5.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example of starting a behavior and indicating that it should not be 3 | automatically stopped when the behavior node is exited. We later 4 | use StopBehavior() to stop the behavior. 5 | """ 6 | 7 | from cozmo_fsm import * 8 | 9 | class Look5(StateMachineProgram): 10 | def setup(self): 11 | """ 12 | LookAroundInPlace(stop_on_exit=False) 13 | =T(5)=> Say("I'm almost done") 14 | =T(5)=> StopBehavior() 15 | """ 16 | 17 | # Code generated by genfsm on Mon Feb 17 03:13:56 2020: 18 | 19 | lookaroundinplace1 = LookAroundInPlace(stop_on_exit=False) .set_name("lookaroundinplace1") .set_parent(self) 20 | say1 = Say("I'm almost done") .set_name("say1") .set_parent(self) 21 | stopbehavior1 = StopBehavior() .set_name("stopbehavior1") .set_parent(self) 22 | 23 | timertrans1 = TimerTrans(5) .set_name("timertrans1") 24 | timertrans1 .add_sources(lookaroundinplace1) .add_destinations(say1) 25 | 26 | timertrans2 = TimerTrans(5) .set_name("timertrans2") 27 | timertrans2 .add_sources(say1) .add_destinations(stopbehavior1) 28 | 29 | return self 30 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/PF_Aruco.py: -------------------------------------------------------------------------------- 1 | """ 2 | PF_Aruco demonstrates a particle filter using ArUco markers. 3 | There are three sensor models provided: 4 | ArucoDistanceSensorModel -- distances only 5 | ArucoBearingSensorModel -- bearings only 6 | ArucoCombinedSensorModel -- combined distances + bearings 7 | 8 | In the particle viewer window: 9 | the WASD keys move the robot 10 | 'e' forces an evaluation step 11 | 'r' forces a resampling 12 | 'v' displays the weight statistics 13 | 'z' re-randomizes the particles. 14 | """ 15 | 16 | from cozmo_fsm import * 17 | from cozmo.util import degrees, Pose 18 | 19 | class PF_Aruco(StateMachineProgram): 20 | def __init__(self): 21 | landmarks = { 22 | 'Aruco-0' : Pose(-55, 160, 0, angle_z=degrees(90)), 23 | 'Aruco-1' : Pose( 55, 160, 0, angle_z=degrees(90)), 24 | 'Aruco-2' : Pose(160, 55, 0, angle_z=degrees( 0)), 25 | 'Aurco-3' : Pose(160, -55, 0, angle_z=degrees( 0)) 26 | } 27 | pf = ParticleFilter(robot, 28 | landmarks = landmarks, 29 | sensor_model = ArucoCombinedSensorModel(robot)) 30 | super().__init__(particle_filter=pf, particle_viewer=True) 31 | 32 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/TapSpeak.fsm: -------------------------------------------------------------------------------- 1 | """ 2 | The TapSpeak demo shows Cozmo responding to cube tap events. A 3 | TapTrans transition is used to set up a handler for taps. The 4 | example also illustrates how the TapTrans transition does wildcard 5 | matching if not given an argument. By passing a cube as an argument 6 | to the TapTrans constructor can use it to look for taps on a 7 | specific cube. 8 | 9 | Behavior: Cozmo starts out by saying 'Tap a cube'. Then, every time 10 | a cube is tapped, Cozmo says the cube name and goes back to 11 | listening for more tap events. 12 | """ 13 | 14 | from cozmo_fsm import * 15 | 16 | from cozmo_fsm import * 17 | 18 | class SayCube(Say): 19 | """Say the name of a cube.""" 20 | def start(self, event=None, \ 21 | cube_names = ['paperclip', 'anglepoise lamp', 'deli slicer']): 22 | cube_number = next(k for k,v in self.robot.world.light_cubes.items() \ 23 | if v == event.source) 24 | self.text = cube_names[cube_number-1] 25 | super().start(event) 26 | 27 | class TapSpeak(StateMachineProgram): 28 | $setup{ 29 | intro: Say('Tap a cube.') =C=> wait 30 | 31 | wait: StateNode() =Tap()=> speak 32 | 33 | speak: SayCube() =C=> wait 34 | } 35 | -------------------------------------------------------------------------------- /cozmo_fsm/TODO.txt: -------------------------------------------------------------------------------- 1 | Remove arucos from particle landmark lists if the aruco is part of a 2 | wall. No need to maintain separate mu/sigma info for wall arucos. 3 | Should still allow solo arucos to be used as landmarks if they are not 4 | part of a wall definition. 5 | 6 | This will speed up the particle filter because we won't be calling 7 | process_landmark on all the wall arucos for every particle. 8 | 9 | **** Current bug: if we kidnap the robot so pf state is 'lost', 10 | make_particles_from_landmarks is punting because it only looks for 11 | solo aruco landmarks now; doesn't construct walls. 12 | 13 | 14 | ================================================================ 15 | 16 | Bug: If we see markers 43 and 44 we create Wall-43. If we then pick 17 | up the robot so it delocalizes, and put it down so that it only sees 18 | marker 45, it will not add the marker or re-localize, even though 19 | it should know where it is because marker 45 gives it its 20 | position with respect to Wall-43. 21 | 22 | ================================================================ 23 | 24 | Should replace integer Aruco marker ids with 'Aruco-###' strings 25 | throughout. 26 | 27 | ================================================================ 28 | 29 | Intersection of two line segments needed for path planner: 30 | https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect 31 | http://www.cs.swan.ac.uk/~cssimon/line_intersection.html 32 | 33 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/CV_Canny.py: -------------------------------------------------------------------------------- 1 | """ 2 | CV_Canny demonstrates image thresholding in OpenCV, and 3 | independently, the Canny edge detector. 4 | """ 5 | 6 | import cv2 7 | import numpy as np 8 | from cozmo_fsm import * 9 | 10 | class CV_Canny(StateMachineProgram): 11 | def __init__(self): 12 | super().__init__(aruco=False, particle_filter=False, cam_viewer=False, 13 | annotate_sdk=False) 14 | 15 | def start(self): 16 | dummy = numpy.array([[0]], dtype='uint8') 17 | super().start() 18 | 19 | cv2.namedWindow('edges') 20 | cv2.imshow('edges',dummy) 21 | 22 | cv2.namedWindow('threshold') 23 | cv2.imshow('threshold',dummy) 24 | 25 | cv2.createTrackbar('thresh','threshold',0,255,lambda self: None) 26 | cv2.setTrackbarPos('thresh', 'threshold', 100) 27 | 28 | cv2.createTrackbar('thresh1','edges',0,255,lambda self: None) 29 | cv2.createTrackbar('thresh2','edges',0,255,lambda self: None) 30 | cv2.setTrackbarPos('thresh1', 'edges', 50) 31 | cv2.setTrackbarPos('thresh2', 'edges', 150) 32 | 33 | def user_image(self,image,gray): 34 | cv2.waitKey(1) 35 | # Thresholding 36 | self.thresh = cv2.getTrackbarPos('thresh','threshold') 37 | ret, self.im_thresh = cv2.threshold(gray, self.thresh, 255, cv2.THRESH_BINARY) 38 | 39 | # Canny edge detection 40 | self.thresh1 = cv2.getTrackbarPos('thresh1','edges') 41 | self.thresh2 = cv2.getTrackbarPos('thresh2','edges') 42 | self.im_edges = cv2.Canny(gray, self.thresh1, self.thresh2, apertureSize=3) 43 | 44 | cv2.imshow('threshold',self.im_thresh) 45 | cv2.imshow('edges',self.im_edges) 46 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/CV_GoodFeatures.py: -------------------------------------------------------------------------------- 1 | """ 2 | CV_GoodFeatures demonstrates the Shi and Tomasi (1994) feature 3 | extractor built in to OpenCV. 4 | """ 5 | 6 | import cv2 7 | import numpy as np 8 | from cozmo_fsm import * 9 | 10 | class CV_GoodFeatures(StateMachineProgram): 11 | def __init__(self): 12 | super().__init__(aruco=False, cam_viewer=False, annotate_sdk=False) 13 | 14 | def start(self): 15 | self.colors = np.random.randint(0,255,(101,3),dtype=np.uint8) 16 | dummy = numpy.array([[0]],dtype='uint8') 17 | super().start() 18 | 19 | cv2.namedWindow('features') 20 | cv2.imshow('features',dummy) 21 | cv2.createTrackbar('maxCorners','features',50,100,lambda self: None) 22 | cv2.createTrackbar('qualityLevel','features',10,1000,lambda self: None) 23 | cv2.createTrackbar('minDistance','features',5,50,lambda self: None) 24 | 25 | def user_image(self,image,gray): 26 | cv2.waitKey(1) 27 | maxCorners = max(1,cv2.getTrackbarPos('maxCorners','features')) 28 | quality = max(1,cv2.getTrackbarPos('qualityLevel','features')) 29 | cv2.setTrackbarPos('qualityLevel', 'features', quality) # don't allow zero 30 | minDist = max(1,cv2.getTrackbarPos('minDistance','features')) 31 | cv2.setTrackbarPos('minDistance', 'features', minDist) # don't allow zero 32 | qualityLevel = quality / 1000 33 | corners = cv2.goodFeaturesToTrack(gray, maxCorners, qualityLevel, minDist) 34 | (x,y,_) = image.shape 35 | image = cv2.resize(image,(y*2,x*2)) 36 | i = 0 37 | for corner in corners: 38 | x,y = corner.ravel() 39 | x = int(x*2); y = int(y*2) 40 | color_index = (x+y) % self.colors.shape[0] 41 | color = self.colors[color_index].tolist() 42 | cv2.circle(image, (x, y), 4, color, -1) 43 | i += 1 44 | cv2.imshow('features',image) 45 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/BackItUp.py: -------------------------------------------------------------------------------- 1 | """ 2 | The BackItUp demo illustrates the use of fork/join to launch 3 | parallel actions and synchronize them again. The fork is performed 4 | by the NullTrans transition with two destinations, while the join is 5 | performed by the CompletionTrans transition with two sources. 6 | 7 | Behavior: Cozmo backs up by 100 mm while simultaneously beeping. He 8 | uses DriveForward instead of Forward to avoid conflict with the Say 9 | action. When he's done backing up, he stops beeping and says 'Safety first'. 10 | """ 11 | 12 | from cozmo_fsm import * 13 | 14 | class BackItUp(StateMachineProgram): 15 | def setup(self): 16 | """ 17 | launcher: StateNode() =N=> {driver, speaker} 18 | 19 | driver: Forward(-100,10) 20 | speaker: Say('beep',duration_scalar=0.8,abort_on_stop=True) =C=> speaker 21 | 22 | {driver,speaker} =C=> finisher: Say('Safety first!') 23 | 24 | """ 25 | 26 | # Code generated by genfsm on Mon Feb 17 03:10:16 2020: 27 | 28 | launcher = StateNode() .set_name("launcher") .set_parent(self) 29 | driver = Forward(-100,10) .set_name("driver") .set_parent(self) 30 | speaker = Say('beep',duration_scalar=0.8,abort_on_stop=True) .set_name("speaker") .set_parent(self) 31 | finisher = Say('Safety first!') .set_name("finisher") .set_parent(self) 32 | 33 | nulltrans1 = NullTrans() .set_name("nulltrans1") 34 | nulltrans1 .add_sources(launcher) .add_destinations(driver,speaker) 35 | 36 | completiontrans1 = CompletionTrans() .set_name("completiontrans1") 37 | completiontrans1 .add_sources(speaker) .add_destinations(speaker) 38 | 39 | completiontrans2 = CompletionTrans() .set_name("completiontrans2") 40 | completiontrans2 .add_sources(driver,speaker) .add_destinations(finisher) 41 | 42 | return self 43 | -------------------------------------------------------------------------------- /cozmo_fsm/opengl.py: -------------------------------------------------------------------------------- 1 | """ 2 | Common code for OpenGL window management 3 | """ 4 | 5 | try: 6 | from OpenGL.GLUT import * 7 | from OpenGL.GL import * 8 | from OpenGL.GLU import * 9 | except: 10 | pass 11 | 12 | import time 13 | 14 | from threading import Thread # for backgrounding window 15 | 16 | INIT_DONE = False 17 | MAIN_LOOP_LAUNCHED = False 18 | 19 | # Maintain a registry of display functions for our windows 20 | WINDOW_REGISTRY = [] 21 | 22 | # List of window creation requests that need to be satisfied 23 | CREATION_QUEUE = [] 24 | 25 | def init(): 26 | global INIT_DONE, robot 27 | if not INIT_DONE: 28 | INIT_DONE = True 29 | glutInit() 30 | glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH) 31 | 32 | # Killing window should not directly kill main program 33 | glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_CONTINUE_EXECUTION) 34 | launch_event_loop() 35 | 36 | def create_window(name,size=(500,500)): 37 | global WINDOW_REGISTRY 38 | glutInitWindowSize(*size) 39 | w = glutCreateWindow(name) 40 | #print('request creation of window',w) 41 | WINDOW_REGISTRY.append(w) 42 | return w 43 | 44 | def event_loop(): 45 | while True: 46 | for window in WINDOW_REGISTRY: 47 | glutSetWindow(window) 48 | glutPostRedisplay() 49 | glutMainLoopEvent() 50 | process_requests() 51 | time.sleep(0.1) 52 | 53 | def process_requests(): 54 | global CREATION_QUEUE 55 | # Process any requests for new windows 56 | queue = CREATION_QUEUE 57 | CREATION_QUEUE = [] 58 | for req in queue: 59 | req() # invoke the window creator 60 | 61 | 62 | def launch_event_loop(): 63 | global MAIN_LOOP_LAUNCHED 64 | if MAIN_LOOP_LAUNCHED: return 65 | MAIN_LOOP_LAUNCHED = True 66 | print('launching opengl event loop') 67 | thread = Thread(target=event_loop) 68 | thread.daemon = True #ending fg program will kill bg program 69 | thread.start() 70 | -------------------------------------------------------------------------------- /cozmo_fsm/sim_robot.py: -------------------------------------------------------------------------------- 1 | """ 2 | Create a dummy robot and world so we can use cozmo-tools 3 | classes without having to connect to a real robot. 4 | """ 5 | 6 | import asyncio 7 | 8 | try: 9 | import cv2 10 | ARUCO_DICT_4x4_100 = cv2.aruco.DICT_4X4_100 11 | except: 12 | ARUCO_DICT_4x4_100 = None 13 | 14 | import cozmo 15 | from cozmo.util import Distance, Angle, Pose 16 | 17 | from .cozmo_kin import CozmoKinematics 18 | from .evbase import EventRouter 19 | from .aruco import Aruco 20 | from .particle import SLAMParticleFilter 21 | from .rrt import RRT, RRTNode 22 | from .worldmap import WorldMap 23 | 24 | class SimWorld(): 25 | def __init__(self): 26 | self.path_viewer = None 27 | self.particle_viewer = None 28 | self.worldmap_viewer = None 29 | 30 | class SimServer(): 31 | def __init__(self): 32 | self.started = False 33 | 34 | class SimRobot(): 35 | def __init__(self, run_in_cloud=False): 36 | robot = self 37 | 38 | robot.loop = asyncio.get_event_loop() 39 | 40 | if not run_in_cloud: 41 | robot.erouter = EventRouter() 42 | robot.erouter.robot = robot 43 | robot.erouter.start() 44 | 45 | robot.head_angle = Angle(radians=0) 46 | robot.shoulder_angle = Angle(radians=0) 47 | robot.lift_height = Distance(distance_mm=0) 48 | robot.pose = Pose(0,0,0,angle_z=Angle(degrees=0)) 49 | robot.camera = None 50 | robot.carrying = None 51 | 52 | robot.world = SimWorld() 53 | robot.world.aruco = Aruco(robot, ARUCO_DICT_4x4_100) 54 | robot.world.light_cubes = dict() 55 | robot.world._faces = dict() 56 | robot.world.charger = None 57 | robot.world.server = SimServer() 58 | robot.world.path_viewer = None 59 | 60 | robot.world.particle_filter = SLAMParticleFilter(robot) 61 | robot.kine = CozmoKinematics(robot) # depends on particle filter 62 | robot.world.rrt = RRT(robot) # depends on kine 63 | robot.world.world_map = WorldMap(robot) 64 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/TapSpeak.py: -------------------------------------------------------------------------------- 1 | """ 2 | The TapSpeak demo shows Cozmo responding to cube tap events. A 3 | TapTrans transition is used to set up a handler for taps. The 4 | example also illustrates how the TapTrans transition does wildcard 5 | matching if not given an argument. By passing a cube as an argument 6 | to the TapTrans constructor can use it to look for taps on a 7 | specific cube. 8 | 9 | Behavior: Cozmo starts out by saying 'Tap a cube'. Then, every time 10 | a cube is tapped, Cozmo says the cube name and goes back to 11 | listening for more tap events. 12 | """ 13 | 14 | from cozmo_fsm import * 15 | 16 | from cozmo_fsm import * 17 | 18 | class SayCube(Say): 19 | """Say the name of a cube.""" 20 | def start(self, event=None, \ 21 | cube_names = ['paperclip', 'anglepoise lamp', 'deli slicer']): 22 | cube_number = next(k for k,v in self.robot.world.light_cubes.items() \ 23 | if v == event.source) 24 | self.text = cube_names[cube_number-1] 25 | super().start(event) 26 | 27 | class TapSpeak(StateMachineProgram): 28 | def setup(self): 29 | """ 30 | intro: Say('Tap a cube.') =C=> wait 31 | 32 | wait: StateNode() =Tap()=> speak 33 | 34 | speak: SayCube() =C=> wait 35 | """ 36 | 37 | # Code generated by genfsm on Mon Feb 17 03:16:53 2020: 38 | 39 | intro = Say('Tap a cube.') .set_name("intro") .set_parent(self) 40 | wait = StateNode() .set_name("wait") .set_parent(self) 41 | speak = SayCube() .set_name("speak") .set_parent(self) 42 | 43 | completiontrans1 = CompletionTrans() .set_name("completiontrans1") 44 | completiontrans1 .add_sources(intro) .add_destinations(wait) 45 | 46 | taptrans1 = TapTrans() .set_name("taptrans1") 47 | taptrans1 .add_sources(wait) .add_destinations(speak) 48 | 49 | completiontrans2 = CompletionTrans() .set_name("completiontrans2") 50 | completiontrans2 .add_sources(speak) .add_destinations(wait) 51 | 52 | return self 53 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Randomness.fsm: -------------------------------------------------------------------------------- 1 | """ 2 | Randomness.fsm demonstrates three ways to introduce randomness in 3 | state machine behavior. 4 | 5 | 1) Use the =RND=> transition to select a destination node at random. 6 | 7 | 2) Pass a list of utterances to Say(), and it will choose one at 8 | random. 9 | 10 | 3) Specialize a node class such as Forward or Turn and use Python's 11 | random() function to generate a random value for the node's 12 | parameter. 13 | 14 | """ 15 | 16 | import random 17 | 18 | from cozmo_fsm import * 19 | 20 | class RandomForward(Forward): 21 | """Move forward a random distance.""" 22 | def __init__(self,mindist=10,maxdist=50,**kwargs): 23 | super().__init__(**kwargs) 24 | self.mindist = mindist if isinstance(mindist,Distance) else distance_mm(mindist) 25 | self.maxdist = maxdist if isinstance(maxdist,Distance) else distance_mm(maxdist) 26 | 27 | def start(self,event=None): 28 | self.distance = distance_mm(self.mindist.distance_mm + 29 | random.random() * (self.maxdist.distance_mm - self.mindist.distance_mm)) 30 | super().start(event) 31 | 32 | class RandomTurn(Turn): 33 | """Turn by a random amount.""" 34 | def __init__(self,minangle=20,maxangle=170,**kwargs): 35 | super().__init__(**kwargs) 36 | self.minangle = minangle if isinstance(minangle,Angle) else degrees(minangle) 37 | self.maxangle = maxangle if isinstance(maxangle,Angle) else degrees(maxangle) 38 | 39 | def start(self,event=None): 40 | angle = self.minangle.degrees + random.random()*(self.maxangle.degrees - self.minangle.degrees) 41 | self.angle = degrees(angle) if random.random()>=0.5 else degrees(-angle) 42 | super().start(event) 43 | 44 | class Randomness(StateMachineProgram): 45 | $setup{ 46 | startnode: StateNode() =RND=> {fwd, fwd, turn, turn, joke} 47 | 48 | fwd: Say(["Forward", "Straight", "Full steam ahead"]) 49 | =T(2)=> RandomForward() =T(2)=> startnode 50 | 51 | turn: Say(["Turn", "Rotate", "Yaw"]) 52 | =T(2)=> RandomTurn() =C=> startnode 53 | 54 | joke: Say(["Watch this", "Hold my beer", "I'm not lost", 55 | "Be cool", "Wanna race?"]) 56 | =C=> StateNode() =T(2)=> startnode 57 | } 58 | 59 | -------------------------------------------------------------------------------- /aruco/generatetags.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2,os,sys,time 3 | import cv2.aruco as aruco 4 | 5 | def make_folder(name): 6 | try: 7 | os.system("mkdir "+name) 8 | except: 9 | pass 10 | 11 | 12 | def getBottomLeftWhite(im): 13 | (width,height) = im.shape 14 | for y in range(height-1,-1,-1): 15 | for x in range(width): 16 | if(im[y][x] == 255): 17 | return (x,y) 18 | return None 19 | 20 | def getTag(num,aruco_dict=aruco.Dictionary_get(aruco.DICT_4X4_100),size=500): 21 | return aruco.drawMarker(aruco_dict,num,size) 22 | 23 | def save_tags(aruco_dict,name,num,size=500,flip=False,label=False): 24 | for i in range(num): 25 | if i%20==0: print("tag %d generated." % (i)) 26 | im = getTag(i,aruco_dict,size) 27 | if flip: 28 | im = cv2.flip(im,1) 29 | pos = getBottomLeftWhite(im) 30 | pos = (pos[0]+5,pos[1]-5) #shift up a little 31 | final=cv2.putText(im,str(i),pos,cv2.FONT_HERSHEY_COMPLEX_SMALL,1,128) #write num in gray 32 | 33 | if label: 34 | #add label 35 | final = np.concatenate((final,255*np.ones((int(size/10),size)))) 36 | msg = "Tag %d" % (i) 37 | pos = (int(size/2)-20*int(len(msg)/2),size+int(size/20)) 38 | final = cv2.putText(final,"Tag %d" % (i),pos,cv2.FONT_HERSHEY_COMPLEX_SMALL,1,0) 39 | 40 | cv2.imwrite(name+str(i)+".jpg",final) 41 | 42 | 43 | def generate_tags(dict_name,outfilename,quantity=100,flip=False,label=False): 44 | aruco_dict = aruco.Dictionary_get(dict_name) 45 | save_tags(aruco_dict,outfilename,quantity,flip=flip,label=label) 46 | 47 | 48 | if(__name__ == "__main__"): 49 | print("you are running this file as a standalone program.") 50 | label = len(sys.argv)>1 51 | if(label): 52 | print("You have chosen to label images of all tags.") 53 | print("tags being outputed will be saved to autogenerated folders in your current directory. Press enter to continue?") 54 | input() #wait for user to press enter 55 | make_folder("aruco_4x4_100") 56 | #make_folder("aruco_4x4_1000") 57 | #make_folder("aruco_5x5_100") 58 | #make_folder("aruco_5x5_1000"), 59 | generate_tags(aruco.DICT_4X4_100,"aruco_4x4_100/aruco4x4_100_",flip=False) 60 | #generate_tags(aruco.DICT_4X4_1000,"aruco_4x4_1000/aruco4x4_1000_",1000,flip=flip) 61 | #generate_tags(aruco.DICT_5X5_100,"aruco_5x5_100/aruco5x5_100_",flip=flip) 62 | #generate_tags(aruco.DICT_5X5_1000,"aruco_5x5_1000/aruco5x5_1000_",1000,flip=flip) 63 | print("complete!") 64 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Nested.py: -------------------------------------------------------------------------------- 1 | """ 2 | The Nested demo shows the use of nested state machines. We define a 3 | new node class DingDong that has a three-node state machine inside 4 | it. We then define the main class, Nested, whose state machine 5 | contains two instances of DingDong. DingDong uses a ParentCompletes 6 | node to cause DinDong to post a completion event, which allows 7 | Nested's first DingDong instance 'dd1' to move on to the next state, 8 | which is 'bridge'. (The 'dd2' instance of DingDong also posts a 9 | completion event, but nothing is listening for it.) 10 | 11 | Behavior: Cozmo says 'ding', then 'dong', then says 'once again' 12 | (that's the bridge), then 'ding', and then 'dong'. 13 | """ 14 | 15 | from cozmo_fsm import * 16 | 17 | class DingDong(StateNode): 18 | def setup(self): 19 | """ 20 | ding: Say('ding') =C=> dong: Say('dong') =C=> ParentCompletes() 21 | """ 22 | 23 | # Code generated by genfsm on Mon Feb 17 03:14:24 2020: 24 | 25 | ding = Say('ding') .set_name("ding") .set_parent(self) 26 | dong = Say('dong') .set_name("dong") .set_parent(self) 27 | parentcompletes1 = ParentCompletes() .set_name("parentcompletes1") .set_parent(self) 28 | 29 | completiontrans1 = CompletionTrans() .set_name("completiontrans1") 30 | completiontrans1 .add_sources(ding) .add_destinations(dong) 31 | 32 | completiontrans2 = CompletionTrans() .set_name("completiontrans2") 33 | completiontrans2 .add_sources(dong) .add_destinations(parentcompletes1) 34 | 35 | return self 36 | 37 | class Nested(StateMachineProgram): 38 | def setup(self): 39 | """ 40 | dd1: DingDong() =C=> bridge: Say('once again') =C=> dd2: DingDong() 41 | """ 42 | 43 | # Code generated by genfsm on Mon Feb 17 03:14:24 2020: 44 | 45 | dd1 = DingDong() .set_name("dd1") .set_parent(self) 46 | bridge = Say('once again') .set_name("bridge") .set_parent(self) 47 | dd2 = DingDong() .set_name("dd2") .set_parent(self) 48 | 49 | completiontrans3 = CompletionTrans() .set_name("completiontrans3") 50 | completiontrans3 .add_sources(dd1) .add_destinations(bridge) 51 | 52 | completiontrans4 = CompletionTrans() .set_name("completiontrans4") 53 | completiontrans4 .add_sources(bridge) .add_destinations(dd2) 54 | 55 | return self 56 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Iteration.py: -------------------------------------------------------------------------------- 1 | """ 2 | Iteration.fsm demonstrates nested iteration using the Iterate node 3 | and the =CNext=> transition, which waits for completion before advancing 4 | the iterator. Use =Next=> if the source nodes don't need to 5 | complete. 6 | """ 7 | 8 | from cozmo_fsm import * 9 | 10 | class PrintIt(StateNode): 11 | def start(self,event=None): 12 | if self.running: return 13 | super().start(event) 14 | if isinstance(event,DataEvent): 15 | print('I got some data: ', event.data) 16 | 17 | class Iteration(StateMachineProgram): 18 | def setup(self): 19 | """ 20 | outer_loop: Iterate(['alpha', 'bravo', 'charlie']) 21 | outer_loop =SayData=> Say() =C=> inner_loop 22 | 23 | inner_loop: Iterate(4) =D=> PrintIt() =Next=> inner_loop 24 | # When inner iteration is done, it posts a completion event. 25 | inner_loop =CNext=> outer_loop 26 | 27 | outer_loop =C=> Say('Done') 28 | """ 29 | 30 | # Code generated by genfsm on Mon Feb 17 03:13:49 2020: 31 | 32 | outer_loop = Iterate(['alpha', 'bravo', 'charlie']) .set_name("outer_loop") .set_parent(self) 33 | say1 = Say() .set_name("say1") .set_parent(self) 34 | inner_loop = Iterate(4) .set_name("inner_loop") .set_parent(self) 35 | printit1 = PrintIt() .set_name("printit1") .set_parent(self) 36 | say2 = Say('Done') .set_name("say2") .set_parent(self) 37 | 38 | saydatatrans1 = SayDataTrans() .set_name("saydatatrans1") 39 | saydatatrans1 .add_sources(outer_loop) .add_destinations(say1) 40 | 41 | completiontrans1 = CompletionTrans() .set_name("completiontrans1") 42 | completiontrans1 .add_sources(say1) .add_destinations(inner_loop) 43 | 44 | datatrans1 = DataTrans() .set_name("datatrans1") 45 | datatrans1 .add_sources(inner_loop) .add_destinations(printit1) 46 | 47 | nexttrans1 = NextTrans() .set_name("nexttrans1") 48 | nexttrans1 .add_sources(printit1) .add_destinations(inner_loop) 49 | 50 | cnexttrans1 = CNextTrans() .set_name("cnexttrans1") 51 | cnexttrans1 .add_sources(inner_loop) .add_destinations(outer_loop) 52 | 53 | completiontrans2 = CompletionTrans() .set_name("completiontrans2") 54 | completiontrans2 .add_sources(outer_loop) .add_destinations(say2) 55 | 56 | return self 57 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/CV_Contour.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | from cozmo_fsm import * 4 | 5 | class CV_Contour(StateMachineProgram): 6 | def __init__(self): 7 | self.colors = [(0,0,255), (0,255,0), (255,0,0), 8 | (255,255,0), (255,0,255), (0,255,255), 9 | (0,0,128), (0,128,0), (128,0,0), 10 | (128,128,0), (0,128,128), (128,0,128), 11 | (255,255,255)] 12 | super().__init__(aruco=False, particle_filter=False, cam_viewer=False, 13 | force_annotation=True, annotate_sdk=False) 14 | 15 | def start(self): 16 | super().start() 17 | dummy = numpy.array([[0]*320], dtype='uint8') 18 | cv2.namedWindow('contour') 19 | cv2.imshow('contour',dummy) 20 | 21 | cv2.createTrackbar('thresh1','contour',0,255,lambda self: None) 22 | cv2.setTrackbarPos('thresh1','contour',100) 23 | 24 | cv2.createTrackbar('minArea','contour',1,1000,lambda self: None) 25 | cv2.setTrackbarPos('minArea','contour',50) 26 | 27 | def user_image(self,image,gray): 28 | thresh1 = cv2.getTrackbarPos('thresh1','contour') 29 | ret, thresholded = cv2.threshold(gray, thresh1, 255, 0) 30 | #cv2.imshow('contour',thresholded) 31 | if cv2.__version__[0] >= '4': 32 | contours, hierarchy = \ 33 | cv2.findContours(thresholded, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) 34 | else: # in OpenCV 3.x there was an additional return value 35 | dummy, contours, hierarchy = \ 36 | cv2.findContours(thresholded, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) 37 | areas = [(i, cv2.contourArea(contours[i])) for i in range(len(contours))] 38 | areas.sort(key=lambda x: x[1]) 39 | areas.reverse() 40 | self.areas = areas 41 | self.contours = contours 42 | self.hierarchy = hierarchy 43 | 44 | def user_annotate(self,annotated_image): 45 | minArea = cv2.getTrackbarPos('minArea','contour') 46 | scale = self.annotated_scale_factor 47 | for area_entry in self.areas: 48 | if area_entry[1] < minArea: 49 | break 50 | temp = index = area_entry[0] 51 | depth = -1 52 | while temp != -1 and depth < len(self.colors)-1: 53 | depth += 1 54 | temp = self.hierarchy[0,temp,3] 55 | contour = scale * self.contours[index] 56 | cv2.drawContours(annotated_image, [contour], 0, self.colors[depth], 2) 57 | cv2.imshow('contour',annotated_image) 58 | cv2.waitKey(1) 59 | return annotated_image 60 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Texting.py: -------------------------------------------------------------------------------- 1 | from cozmo_fsm import * 2 | 3 | class Texting(StateMachineProgram): 4 | def setup(self): 5 | """ 6 | startnode: StateNode() 7 | startnode =TM('1')=> do_null 8 | startnode =TM('2')=> do_time 9 | startnode =TM('3')=> do_comp 10 | 11 | do_null: Say("Full steam ahead") =N=> Forward(20) =C=> startnode 12 | 13 | do_time: Say("Full steam ahead") =T(2)=> Forward(20) =C=> startnode 14 | 15 | do_comp: Say("Full steam ahead") =C=> Forward(20) =C=> startnode 16 | """ 17 | 18 | # Code generated by genfsm on Mon Feb 17 03:17:21 2020: 19 | 20 | startnode = StateNode() .set_name("startnode") .set_parent(self) 21 | do_null = Say("Full steam ahead") .set_name("do_null") .set_parent(self) 22 | forward1 = Forward(20) .set_name("forward1") .set_parent(self) 23 | do_time = Say("Full steam ahead") .set_name("do_time") .set_parent(self) 24 | forward2 = Forward(20) .set_name("forward2") .set_parent(self) 25 | do_comp = Say("Full steam ahead") .set_name("do_comp") .set_parent(self) 26 | forward3 = Forward(20) .set_name("forward3") .set_parent(self) 27 | 28 | textmsgtrans1 = TextMsgTrans('1') .set_name("textmsgtrans1") 29 | textmsgtrans1 .add_sources(startnode) .add_destinations(do_null) 30 | 31 | textmsgtrans2 = TextMsgTrans('2') .set_name("textmsgtrans2") 32 | textmsgtrans2 .add_sources(startnode) .add_destinations(do_time) 33 | 34 | textmsgtrans3 = TextMsgTrans('3') .set_name("textmsgtrans3") 35 | textmsgtrans3 .add_sources(startnode) .add_destinations(do_comp) 36 | 37 | nulltrans1 = NullTrans() .set_name("nulltrans1") 38 | nulltrans1 .add_sources(do_null) .add_destinations(forward1) 39 | 40 | completiontrans1 = CompletionTrans() .set_name("completiontrans1") 41 | completiontrans1 .add_sources(forward1) .add_destinations(startnode) 42 | 43 | timertrans1 = TimerTrans(2) .set_name("timertrans1") 44 | timertrans1 .add_sources(do_time) .add_destinations(forward2) 45 | 46 | completiontrans2 = CompletionTrans() .set_name("completiontrans2") 47 | completiontrans2 .add_sources(forward2) .add_destinations(startnode) 48 | 49 | completiontrans3 = CompletionTrans() .set_name("completiontrans3") 50 | completiontrans3 .add_sources(do_comp) .add_destinations(forward3) 51 | 52 | completiontrans4 = CompletionTrans() .set_name("completiontrans4") 53 | completiontrans4 .add_sources(forward3) .add_destinations(startnode) 54 | 55 | return self 56 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Boo.fsm: -------------------------------------------------------------------------------- 1 | """ 2 | Peek-A-Boo game inspired by the Boo game of Pablo Barros. 3 | 4 | This version is coded using the cozmo_fsm package and illustrates 5 | features such as repetitive polling, nested state machines, and a 6 | completion transition that uses one completing source node to 7 | terminate a second source (MoveHead) that doesn't complete. 8 | """ 9 | 10 | from cozmo_fsm import * 11 | 12 | class WaitForPlayer(StateMachineProgram): 13 | """Wait for player's face to appear and remain visible for a little while.""" 14 | def start(self,event=None): 15 | self.set_polling_interval(0.2) 16 | self.faces_found = 0 # initialize before polling starts 17 | super().start(event) 18 | 19 | def poll(self): 20 | if self.robot.world.visible_face_count() == 0: return 21 | self.faces_found += 1 22 | if self.faces_found > 3: 23 | self.post_completion() 24 | 25 | class WaitForHide(StateNode): 26 | """Wait for player's face to disappear and remain not visible for a little while.""" 27 | def start(self,event=None): 28 | self.set_polling_interval(0.2) 29 | self.faces_not_found = 0 # initialize before polling starts 30 | super().start(event) 31 | 32 | def poll(self): 33 | if self.robot.world.visible_face_count() > 0: return 34 | self.faces_not_found += 1 35 | if self.faces_not_found > 2: 36 | self.post_completion() 37 | 38 | class HeadAndLiftGesture(StateNode): 39 | """Move head and lift simultaneously. Finish when head movement completes.""" 40 | $setup { 41 | launch: StateNode() =N=> {move_head, move_lift} 42 | 43 | move_head: SetHeadAngle(cozmo.robot.MAX_HEAD_ANGLE) 44 | move_lift: MoveLift(-3) 45 | 46 | {move_head, move_lift} =C(1)=> ParentCompletes() 47 | } 48 | 49 | class Boo(StateNode): 50 | $setup { 51 | launch: Say("Let's play") 52 | =C=> SetHeadAngle(30) 53 | =C=> player_appears 54 | 55 | player_appears: WaitForPlayer() 56 | =C=> AnimationNode('anim_freeplay_reacttoface_identified_01_head_angle_40') 57 | =C=> SetHeadAngle(cozmo.robot.MIN_HEAD_ANGLE) 58 | =C=> SetHeadAngle(cozmo.robot.MAX_HEAD_ANGLE) 59 | =C=> player_hides 60 | 61 | player_hides: WaitForHide() 62 | =C=> AnimationNode('anim_hiking_observe_01') 63 | =C=> HeadAndLiftGesture() 64 | =C=> player_reappears 65 | 66 | player_reappears: WaitForPlayer() 67 | =C=> AnimationNode('anim_freeplay_reacttoface_like_01') 68 | =C=> HeadAndLiftGesture() 69 | =C=> Say("play again") 70 | =C=> SetHeadAngle(30) 71 | =C=> player_hides 72 | } 73 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/CV_OpticalFlow.py: -------------------------------------------------------------------------------- 1 | """ 2 | CV_OpticalFlow demonstrates the Lucas and Kanade optical flow 3 | algorithm built in to OpenCV. 4 | """ 5 | 6 | import cv2 7 | import numpy as np 8 | from cozmo_fsm import * 9 | 10 | class CV_OpticalFlow(StateMachineProgram): 11 | def __init__(self): 12 | super().__init__(aruco=False, particle_filter=False, cam_viewer=False, 13 | annotate_sdk = False) 14 | 15 | def start(self): 16 | self.feature_params = dict( maxCorners = 100, 17 | qualityLevel = 0.3, 18 | minDistance = 7, 19 | blockSize = 7 ) 20 | 21 | self.lk_params = dict( winSize = (15,15), 22 | maxLevel = 2, 23 | criteria = (cv2.TERM_CRITERIA_EPS | 24 | cv2.TERM_CRITERIA_COUNT, 25 | 10, 0.03) ) 26 | 27 | self.colors = np.random.randint(0, 255, (100,3), dtype=np.uint8) 28 | 29 | self.prev_gray = None 30 | self.good_new = None 31 | self.mask = None 32 | 33 | super().start() 34 | cv2.namedWindow('OpticalFlow') 35 | 36 | def user_image(self,image,gray): 37 | cv2.waitKey(1) 38 | if self.prev_gray is None: 39 | self.prev_gray = gray 40 | self.prev_feat = cv2.goodFeaturesToTrack(gray, mask=None, 41 | **self.feature_params) 42 | return 43 | new_feat, status, err = \ 44 | cv2.calcOpticalFlowPyrLK(self.prev_gray, gray, 45 | self.prev_feat, None, **self.lk_params) 46 | if new_feat is None: 47 | self.good_new = None 48 | return 49 | self.good_new = new_feat[status == 1] 50 | self.good_old = self.prev_feat[status == 1] 51 | self.prev_gray = gray 52 | self.prev_feat = self.good_new.reshape(-1,1,2) 53 | 54 | (x,y,_) = image.shape 55 | image = cv2.resize(image,(y*2,x*2)) 56 | if self.mask is None: 57 | self.mask = np.zeros_like(image) 58 | 59 | for i,(new,old) in enumerate(zip(self.good_new, self.good_old)): 60 | a,b = new.ravel() 61 | c,d = old.ravel() 62 | a = int(a) 63 | b = int(b) 64 | c = int(c) 65 | d = int(d) 66 | self.mask = cv2.line(self.mask, (a+a,b+b), (c+c,d+d), 67 | self.colors[i].tolist(), 2) 68 | cv2.circle(image,(a+a,b+b),5,self.colors[i].tolist(),-1) 69 | image = cv2.add(image,self.mask) 70 | cv2.imshow('OpticalFlow', image) 71 | -------------------------------------------------------------------------------- /INSTALL.txt: -------------------------------------------------------------------------------- 1 | Ubuntu Linux: 2 | 1. For OpenGL you will need to do: 3 | apt-get install freeglut3 4 | 2. To get speech recognition to work: 5 | apt-get install python3-pyaudio 6 | 3. You can then install all required Python packages by doing: 7 | pip3 install -r requirements.txt 8 | 4. You will also need to install adb: 9 | apt-get install android-tools-adb 10 | 5. To get simple_cli to work, add the cozmo-tools directory to your PATH. 11 | 12 | 13 | Windows: 14 | 1. The requirements.txt file lists all the packages you can install with pip, except 15 | for PyOpenGL. As of 2020 the PyOpenGL and PyOpenGL_accelerate packages in PyPI 16 | were not good. Instead, download their whl files for your Python version from 17 | https://www.lfd.uci.edu/~gohlke/pythonlibs/#pyopengl 18 | and use pip to install them. They include the freeglut DLL. 19 | You should also grab numpy from this site, not pip, so that the version is 20 | compatible with the PyOpenGL code. 21 | 2. If you are using an Android device or a Kindle Fire to run the Cozmo app, you 22 | will also need to install adb. Follow the instructions on this page: 23 | http://cozmosdk.anki.com/docs/adb.html 24 | 3. If you are using an iOS device to run the Cozmo app you must install iTunes on 25 | your Windows computer so that you get the proper device drivers. Make sure 26 | that iTunes can talk to your iOS device before proceeding. 27 | 4. To get simple_cli and genfsm to work, add the following to your $Profile: 28 | function simple_cli() { py -i C:\Users\\...\cozmo-tools\simple_cli } 29 | function genfsm($file) { py C:\Users\\...\cozmo-tools\genfsm $file } 30 | 5. To allow Python to find the cozmo_fsm package, edit your user enviornment 31 | variables and create a new variable called PYTHONPATH that points to the 32 | cozmo-tools folder. 33 | 34 | 35 | MacOS (should be 10.13 or higher): 36 | 1. The requirements.txt file will work for for everything but PyAudio. 37 | The version of PyAudio in PyPI does not compile unless you have 38 | certain tools installed. Visit this page to learn how to install PyAudio: 39 | http://people.csail.mit.edu/hubert/pyaudio/ 40 | 2. To use OpenGL you need to install freeglut, which can be done with the 41 | command 'brew install freeglut'. If your system doesn't have the "brew" 42 | command, visit this page: https://brew.sh/ 43 | 3. If you are using an Android device to run the Cozmo app, you will need to 44 | install adb. Follow the instructions on this page: 45 | http://cozmosdk.anki.com/docs/adb.html 46 | 4. If you are using an iOS device to run the Cozmo app you don't need to 47 | install anything extra on your MacOS system. 48 | 5. To get simple_cli to work, add cozmo-tools to your PATH, and change the first 49 | line of simple_cli to include the correct path to your Python. 50 | -------------------------------------------------------------------------------- /cozmo_fsm/pilot0.py: -------------------------------------------------------------------------------- 1 | """ 2 | To avoid circular dependencies between pilot.fsm, doorpass.fsm, and 3 | path_planner.py, we put some pilot classes here so everyone can import 4 | them. 5 | 6 | """ 7 | 8 | from .base import * 9 | from .rrt import * 10 | from .events import PilotEvent 11 | 12 | class PilotCheckStart(StateNode): 13 | "Fails if rrt planner indicates start_collides" 14 | 15 | def start(self, event=None): 16 | super().start(event) 17 | (pose_x, pose_y, pose_theta) = self.robot.world.particle_filter.pose 18 | start_node = RRTNode(x=pose_x, y=pose_y, q=pose_theta) 19 | try: 20 | self.robot.world.rrt.plan_path(start_node,start_node) 21 | except StartCollides as e: 22 | print('PilotCheckStart: Start collides!',e) 23 | self.post_event(PilotEvent(StartCollides, args=e.args)) 24 | self.post_failure() 25 | return 26 | except Exception as e: 27 | print('PilotCheckStart: Unexpected planner exception',e) 28 | self.post_failure() 29 | return 30 | self.post_success() 31 | 32 | 33 | class PilotCheckStartDetail(StateNode): 34 | "Posts collision object if rrt planner indicates start_collides" 35 | 36 | def start(self, event=None): 37 | super().start(event) 38 | (pose_x, pose_y, pose_theta) = self.robot.world.particle_filter.pose 39 | start_node = RRTNode(x=pose_x, y=pose_y, q=pose_theta) 40 | try: 41 | self.robot.world.rrt.plan_path(start_node,start_node) 42 | except StartCollides as e: 43 | print('PilotCheckStartDetail: Start collides!',e) 44 | self.post_event(PilotEvent(StartCollides, args=e.args)) 45 | self.post_data(e.args) 46 | return 47 | except Exception as e: 48 | print('PilotCheckStartDetail: Unexpected planner exception',e) 49 | self.post_failure() 50 | return 51 | self.post_success() 52 | 53 | #---------------- Navigation Plan ---------------- 54 | 55 | class NavStep(): 56 | DRIVE = "drive" 57 | DOORPASS = "doorpass" 58 | BACKUP = "backup" 59 | 60 | def __init__(self, type, param): 61 | """For DRIVE and BACKUP types, param is a list of RRTNode instances. The 62 | reason we group these into a list instead of having one node per step is that 63 | the DriveContinuous function is going to be interpolating over the entire sequence. 64 | For a DOORPASS step the param is the door object.""" 65 | self.type = type 66 | self.param = param 67 | 68 | def __repr__(self): 69 | if self.type == NavStep.DOORPASS: 70 | pstring = self.param.id 71 | elif self.type == NavStep.DRIVE: 72 | psteps = [(round(node.x,1),round(node.y,1)) for node in self.param] 73 | pstring = repr(psteps) 74 | else: # NavStep.BACKUP and anything else 75 | pstring = repr(self.param) 76 | if len(pstring) > 40: 77 | pstring = pstring[0:20] + ' ...' + pstring[-20:] 78 | return '' % (self.type, pstring) 79 | 80 | 81 | class NavPlan(): 82 | def __init__(self, steps=[]): 83 | self.steps = steps 84 | 85 | def __repr__(self): 86 | steps = [(('doorpass(%s)' % s.param.id) if s.type == NavStep.DOORPASS else s.type) for s in self.steps] 87 | return '' % repr(steps) 88 | 89 | def extract_path(self): 90 | nodes = [] 91 | for step in self.steps: 92 | if step.type in (NavStep.DRIVE, NavStep.BACKUP): 93 | nodes += step.param 94 | return nodes 95 | -------------------------------------------------------------------------------- /cozmo_fsm/custom_objs.py: -------------------------------------------------------------------------------- 1 | import cozmo 2 | from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes 3 | 4 | custom_marker_types = [] 5 | custom_container_types = [] 6 | custom_cube_types = [] 7 | 8 | async def declare_objects(robot): 9 | 10 | """ 11 | await robot.world.define_custom_box( 12 | CustomObjectTypes.CustomType00, 13 | CustomObjectMarkers.Hexagons4, # front 14 | CustomObjectMarkers.Triangles5, # back 15 | CustomObjectMarkers.Circles2, # top 16 | CustomObjectMarkers.Diamonds3, # bottom 17 | CustomObjectMarkers.Circles4, # left 18 | CustomObjectMarkers.Diamonds5, # right 19 | 50, 20, 1, # depth, width, height 20 | 40, 40, # marker width and height 21 | True) # is_unique 22 | return 23 | """ 24 | 25 | global custom_marker_types, custom_cube_types 26 | 27 | decl_marker = robot.world.define_custom_wall 28 | custom_marker_types = [ 29 | CustomObjectTypes.CustomType00, 30 | CustomObjectTypes.CustomType01, 31 | CustomObjectTypes.CustomType02, 32 | CustomObjectTypes.CustomType03 33 | ] 34 | 35 | await decl_marker(CustomObjectTypes.CustomType00, 36 | CustomObjectMarkers.Circles2, 37 | 40, 40, 40, 40, True) 38 | 39 | await decl_marker(CustomObjectTypes.CustomType01, 40 | CustomObjectMarkers.Triangles2, 41 | 40, 40, 40, 40, True) 42 | 43 | await decl_marker(CustomObjectTypes.CustomType02, 44 | CustomObjectMarkers.Diamonds2, 45 | 40, 40, 40, 40, True) 46 | 47 | await decl_marker(CustomObjectTypes.CustomType03, 48 | CustomObjectMarkers.Hexagons2, 49 | 40, 40, 40, 40, True) 50 | 51 | 52 | # Markers for containers 53 | custom_container_types = [ 54 | CustomObjectTypes.CustomType04, 55 | CustomObjectTypes.CustomType05 56 | ] 57 | 58 | await decl_marker(CustomObjectTypes.CustomType04, 59 | CustomObjectMarkers.Circles3, 60 | 40, 40, 40, 40, False) 61 | 62 | await decl_marker(CustomObjectTypes.CustomType05, 63 | CustomObjectMarkers.Triangles3, 64 | 40, 40, 40, 40, False) 65 | 66 | 67 | 68 | # Markers for cubes 69 | 70 | decl_cube = robot.world.define_custom_cube 71 | 72 | custom_cube_types = [ 73 | CustomObjectTypes.CustomType10, 74 | CustomObjectTypes.CustomType11, 75 | CustomObjectTypes.CustomType12, 76 | CustomObjectTypes.CustomType13, 77 | CustomObjectTypes.CustomType14, 78 | CustomObjectTypes.CustomType15 79 | ] 80 | 81 | await decl_cube(CustomObjectTypes.CustomType10, 82 | CustomObjectMarkers.Circles5, 83 | 50, 40, 40, True) 84 | await decl_cube(CustomObjectTypes.CustomType11, 85 | CustomObjectMarkers.Diamonds5, 86 | 50, 40, 40, True) 87 | await decl_cube(CustomObjectTypes.CustomType12, 88 | CustomObjectMarkers.Hexagons5, 89 | 50, 40, 40, True) 90 | await decl_cube(CustomObjectTypes.CustomType13, 91 | CustomObjectMarkers.Triangles4, 92 | 50, 40, 40, True) 93 | await decl_cube(CustomObjectTypes.CustomType14, 94 | CustomObjectMarkers.Circles4, 95 | 50, 40, 40, True) 96 | await decl_cube(CustomObjectTypes.CustomType15, 97 | CustomObjectMarkers.Diamonds4, 98 | 50, 40, 40, True) 99 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Randomness.py: -------------------------------------------------------------------------------- 1 | """ 2 | Randomness.fsm demonstrates three ways to introduce randomness in 3 | state machine behavior. 4 | 5 | 1) Use the =RND=> transition to select a destination node at random. 6 | 7 | 2) Pass a list of utterances to Say(), and it will choose one at 8 | random. 9 | 10 | 3) Specialize a node class such as Forward or Turn and use Python's 11 | random() function to generate a random value for the node's 12 | parameter. 13 | 14 | """ 15 | 16 | import random 17 | 18 | from cozmo_fsm import * 19 | 20 | class RandomForward(Forward): 21 | """Move forward a random distance.""" 22 | def __init__(self,mindist=10,maxdist=50,**kwargs): 23 | super().__init__(**kwargs) 24 | self.mindist = mindist if isinstance(mindist,Distance) else distance_mm(mindist) 25 | self.maxdist = maxdist if isinstance(maxdist,Distance) else distance_mm(maxdist) 26 | 27 | def start(self,event=None): 28 | self.distance = distance_mm(self.mindist.distance_mm + 29 | random.random() * (self.maxdist.distance_mm - self.mindist.distance_mm)) 30 | super().start(event) 31 | 32 | class RandomTurn(Turn): 33 | """Turn by a random amount.""" 34 | def __init__(self,minangle=20,maxangle=170,**kwargs): 35 | super().__init__(**kwargs) 36 | self.minangle = minangle if isinstance(minangle,Angle) else degrees(minangle) 37 | self.maxangle = maxangle if isinstance(maxangle,Angle) else degrees(maxangle) 38 | 39 | def start(self,event=None): 40 | angle = self.minangle.degrees + random.random()*(self.maxangle.degrees - self.minangle.degrees) 41 | self.angle = degrees(angle) if random.random()>=0.5 else degrees(-angle) 42 | super().start(event) 43 | 44 | class Randomness(StateMachineProgram): 45 | def setup(self): 46 | """ 47 | startnode: StateNode() =RND=> {fwd, fwd, turn, turn, joke} 48 | 49 | fwd: Say(["Forward", "Straight", "Full steam ahead"]) 50 | =T(2)=> RandomForward() =T(2)=> startnode 51 | 52 | turn: Say(["Turn", "Rotate", "Yaw"]) 53 | =T(2)=> RandomTurn() =C=> startnode 54 | 55 | joke: Say(["Watch this", "Hold my beer", "I'm not lost", 56 | "Be cool", "Wanna race?"]) 57 | =C=> StateNode() =T(2)=> startnode 58 | """ 59 | 60 | # Code generated by genfsm on Mon Feb 17 03:16:24 2020: 61 | 62 | startnode = StateNode() .set_name("startnode") .set_parent(self) 63 | fwd = Say(["Forward", "Straight", "Full steam ahead"]) .set_name("fwd") .set_parent(self) 64 | randomforward1 = RandomForward() .set_name("randomforward1") .set_parent(self) 65 | turn = Say(["Turn", "Rotate", "Yaw"]) .set_name("turn") .set_parent(self) 66 | randomturn1 = RandomTurn() .set_name("randomturn1") .set_parent(self) 67 | joke = Say(["Watch this", "Hold my beer", "I'm not lost", 68 | "Be cool", "Wanna race?"]) .set_name("joke") .set_parent(self) 69 | statenode1 = StateNode() .set_name("statenode1") .set_parent(self) 70 | 71 | randomtrans1 = RandomTrans() .set_name("randomtrans1") 72 | randomtrans1 .add_sources(startnode) .add_destinations(fwd,fwd,turn,turn,joke) 73 | 74 | timertrans1 = TimerTrans(2) .set_name("timertrans1") 75 | timertrans1 .add_sources(fwd) .add_destinations(randomforward1) 76 | 77 | timertrans2 = TimerTrans(2) .set_name("timertrans2") 78 | timertrans2 .add_sources(randomforward1) .add_destinations(startnode) 79 | 80 | timertrans3 = TimerTrans(2) .set_name("timertrans3") 81 | timertrans3 .add_sources(turn) .add_destinations(randomturn1) 82 | 83 | completiontrans1 = CompletionTrans() .set_name("completiontrans1") 84 | completiontrans1 .add_sources(randomturn1) .add_destinations(startnode) 85 | 86 | completiontrans2 = CompletionTrans() .set_name("completiontrans2") 87 | completiontrans2 .add_sources(joke) .add_destinations(statenode1) 88 | 89 | timertrans4 = TimerTrans(2) .set_name("timertrans4") 90 | timertrans4 .add_sources(statenode1) .add_destinations(startnode) 91 | 92 | return self 93 | 94 | -------------------------------------------------------------------------------- /cozmo_fsm/kine.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | from . import geometry 5 | from . import rrt_shapes 6 | 7 | class Joint(): 8 | def __init__(self, name, parent=None, type='fixed', getter=(lambda:0), 9 | description='A kinematic joint', 10 | qmin=-math.inf, qmax=math.inf, 11 | d=0, theta=0, r=0, alpha=0, 12 | collision_model=None, ctransform=geometry.identity()): 13 | self.name = name 14 | self.parent = parent 15 | self.type = type 16 | if type == 'fixed': 17 | self.apply_q = self.fixed 18 | elif type == 'revolute': 19 | self.apply_q = self.revolute 20 | elif type == 'prismatic': 21 | self.apply_q = self.prismatic 22 | elif type == 'world': 23 | self.apply_q = self.world_joint 24 | else: 25 | raise ValueError("Type must be 'fixed', 'revolute', 'prismatic', or 'world'.") 26 | self.getter = getter 27 | self.description = description 28 | self.children = [] 29 | self.d = d 30 | self.theta = theta 31 | self.r = r 32 | self.alpha = alpha 33 | self.children = [] 34 | self.collision_model = collision_model 35 | self.q = 0 36 | self.qmin = qmin 37 | self.qmax = qmax 38 | self.parent_link_to_this_joint = geometry.dh_matrix(-d,-theta,-r,-alpha) 39 | self.this_joint_to_parent_link = np.linalg.inv(self.parent_link_to_this_joint) 40 | 41 | self.solver = None 42 | 43 | def __repr__(self): 44 | if self.type == 'fixed': 45 | qval = 'fixed' 46 | elif isinstance(self.q, (int,float)): 47 | qval = "q=%.2f deg." % (self.q*180/math.pi) 48 | else: 49 | qval = ("q=%s" % repr(self.q)) 50 | return "" % (self.name, qval) 51 | 52 | def this_joint_to_this_link(self): 53 | "The link moves by q in the joint's reference frame." 54 | return self.apply_q() 55 | 56 | def this_link_to_this_joint(self): 57 | return np.linalg.inv(self.this_joint_to_this_link()) 58 | 59 | def revolute(self): 60 | return geometry.aboutZ(-self.q) 61 | 62 | def prismatic(self): 63 | return geometry.translate(0.,0.,-self.q) 64 | 65 | def fixed(self): 66 | return geometry.identity() 67 | 68 | def world_joint(self): 69 | return geometry.translate(self.q[0],self.q[1]).dot(geometry.aboutZ(self.q[2])) 70 | 71 | class Kinematics(): 72 | def __init__(self,joint_list,robot): 73 | self.joints = dict() 74 | for j in joint_list: 75 | self.joints[j.name] = j 76 | if j.parent: 77 | j.parent.children.append(j) 78 | self.base = self.joints[joint_list[0].name] 79 | self.robot = robot 80 | robot.kine = self 81 | self.get_pose() 82 | 83 | def joint_to_base(self,joint): 84 | if isinstance(joint,str): 85 | joint = self.joints[joint] 86 | Tinv = geometry.identity() 87 | j = joint 88 | while j is not self.base and j.parent is not None: 89 | Tinv = j.parent.this_link_to_this_joint().dot( 90 | j.this_joint_to_parent_link.dot(Tinv) 91 | ) 92 | j = j.parent 93 | if j: 94 | return Tinv 95 | else: 96 | raise Exception('Joint %s has no path to base frame' % joint) 97 | 98 | def base_to_joint(self,joint): 99 | return np.linalg.inv(self.joint_to_base(joint)) 100 | 101 | def joint_to_joint(self,joint1,joint2): 102 | return self.base_to_joint(joint2).dot(self.joint_to_base(joint1)) 103 | 104 | def link_to_base(self,joint): 105 | if isinstance(joint,str): 106 | joint = self.joints[joint] 107 | return self.joint_to_base(joint).dot(joint.this_link_to_this_joint()) 108 | 109 | def base_to_link(self,joint): 110 | return np.linalg.inv(self.link_to_base(joint)) 111 | 112 | def link_to_link(self,joint1,joint2): 113 | return self.base_to_link(joint2).dot(self.link_to_base(joint1)) 114 | 115 | def get_pose(self): 116 | for j in self.joints.values(): 117 | j.q = j.getter() 118 | -------------------------------------------------------------------------------- /cozmo_fsm/events.py: -------------------------------------------------------------------------------- 1 | """ 2 | The base Event class is imported from evbase.py. 3 | All other events are defined here. 4 | """ 5 | 6 | import cozmo 7 | 8 | from .evbase import Event 9 | 10 | class CompletionEvent(Event): 11 | """Signals completion of a state node's action.""" 12 | pass 13 | 14 | 15 | class SuccessEvent(Event): 16 | """Signals success of a state node's action.""" 17 | def __init__(self,details=None): 18 | super().__init__() 19 | self.details = details 20 | 21 | 22 | class FailureEvent(Event): 23 | """Signals failure of a state node's action.""" 24 | def __init__(self,details=None): 25 | super().__init__() 26 | self.details = details 27 | 28 | def __repr__(self): 29 | if isinstance(self.details, cozmo.action.Action): 30 | reason = self.details.failure_reason[0] 31 | else: 32 | reason = self.details 33 | return '<%s for %s: %s>' % (self.__class__.__name__, self.source.name, reason) 34 | 35 | 36 | class DataEvent(Event): 37 | """Signals a data item broadcasted by the node.""" 38 | def __init__(self,data): 39 | super().__init__() 40 | self.data = data 41 | 42 | 43 | class TextMsgEvent(Event): 44 | """Signals a text message broadcasted to the state machine.""" 45 | def __init__(self,string,words=None,result=None): 46 | super().__init__() 47 | self.string = string 48 | self.words = words or string.split(None) 49 | self.result = result 50 | 51 | class SpeechEvent(Event): 52 | """Results of speech recognition process.""" 53 | def __init__(self,string,words=None,result=None): 54 | super().__init__() 55 | self.string = string 56 | self.words = words 57 | self.result = result 58 | 59 | class PilotEvent(Event): 60 | """Results of a pilot request.""" 61 | def __init__(self,status,**args): 62 | super().__init__() 63 | self.status = status 64 | self.args = args 65 | 66 | def __repr__(self): 67 | try: 68 | src_string = self.source.name 69 | except: 70 | src_string = repr(self.source) 71 | return '<%s %s from %s>' % (self.__class__.__name__, self.status.__name__, src_string) 72 | 73 | 74 | #________________ Cozmo-generated events ________________ 75 | 76 | class CozmoGeneratedEvent(Event): 77 | def __init__(self,source,params): 78 | super().__init__() 79 | self.source = source 80 | self.params = params 81 | # Note regarding generator(): we're going to curry this function 82 | # to supply EROUTER and EVENT_CLASS as the first two arguments. 83 | def generator(EROUTER, EVENT_CLASS, cozmo_event, obj=None, **kwargs): 84 | our_event = EVENT_CLASS(obj,kwargs) 85 | EROUTER.post(our_event) 86 | 87 | class TapEvent(CozmoGeneratedEvent): 88 | cozmo_evt_type = cozmo.objects.EvtObjectTapped 89 | 90 | class FaceEvent(CozmoGeneratedEvent): 91 | cozmo_evt_type = cozmo.faces.EvtFaceAppeared 92 | 93 | class ObservedMotionEvent(CozmoGeneratedEvent): 94 | cozmo_evt_type = cozmo.camera.EvtRobotObservedMotion 95 | 96 | def __repr__(self): 97 | top = self.params['has_top_movement'] 98 | left = self.params['has_left_movement'] 99 | right = self.params['has_right_movement'] 100 | movement = '' 101 | if top: 102 | pos = self.params['top_img_pos'] 103 | movement = movement + ('' if (movement=='') else ' ') + \ 104 | ('top:(%d,%d)' % (pos.x,pos.y)) 105 | if left: 106 | pos = self.params['left_img_pos'] 107 | movement = movement + ('' if (movement=='') else ' ') + \ 108 | ('left:(%d,%d)' % (pos.x,pos.y)) 109 | if right: 110 | pos = self.params['right_img_pos'] 111 | movement = movement + ('' if (movement=='') else ' ') + \ 112 | ('right:(%d,%d)' % (pos.x,pos.y)) 113 | if movement == '': 114 | pos = self.params['img_pos'] 115 | movement = movement + ('' if (movement=='') else ' ') + \ 116 | ('broad:(%d,%d)' % (pos.x,pos.y)) 117 | return '<%s %s>' % (self.__class__.__name__, movement) 118 | 119 | 120 | class UnexpectedMovementEvent(CozmoGeneratedEvent): 121 | cozmo_evt_type = cozmo.robot.EvtUnexpectedMovement 122 | 123 | def __repr__(self): 124 | side = self.params['movement_side'] 125 | # side.id == 0 means the movement_side is "unknown" 126 | # Occurs when reaction triggers are disabled (as is normally the case). 127 | side_string = ' '+side.name if side.id > 0 else '' 128 | return '<%s %s%s>' % (self.__class__.__name__, 129 | self.params['movement_type'].name, 130 | side_string) 131 | 132 | -------------------------------------------------------------------------------- /cozmo_fsm/aruco.py: -------------------------------------------------------------------------------- 1 | try: import cv2 2 | except: pass 3 | 4 | import math 5 | from numpy import sqrt, arctan2, array, multiply 6 | 7 | ARUCO_MARKER_SIZE = 44 8 | 9 | class ArucoMarker(object): 10 | def __init__(self, aruco_parent, marker_id, bbox, translation, rotation): 11 | self.id = marker_id 12 | self.id_string = 'Aruco-' + str(marker_id) 13 | self.bbox = bbox 14 | self.aruco_parent = aruco_parent 15 | 16 | # OpenCV Pose information 17 | self.opencv_translation = translation 18 | self.opencv_rotation = (180/math.pi)*rotation 19 | 20 | # Marker coordinates in robot's camera reference frame 21 | self.camera_coords = (-translation[0], -translation[1], translation[2]) 22 | 23 | # Distance in the x-y plane; particle filter ignores height so don't include it 24 | self.camera_distance = math.sqrt(translation[0]*translation[0] + 25 | # translation[1]*translation[1] + 26 | translation[2]*translation[2]) 27 | # Conversion to euler angles 28 | self.euler_rotation = self.rotationMatrixToEulerAngles( 29 | cv2.Rodrigues(rotation)[0])*(180/math.pi) 30 | 31 | def __str__(self): 32 | return "" % \ 33 | (self.id, *self.opencv_translation, *self.opencv_rotation, *self.euler_rotation) 34 | 35 | def __repr__(self): 36 | return self.__str__() 37 | 38 | @staticmethod 39 | def rotationMatrixToEulerAngles(R) : 40 | sy = sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) 41 | singular = sy < 1e-6 42 | if not singular: 43 | x = arctan2(R[2,1] , R[2,2]) 44 | y = arctan2(-R[2,0], sy) 45 | z = arctan2(R[1,0], R[0,0]) 46 | else: 47 | x = arctan2(-R[1,2], R[1,1]) 48 | y = arctan2(-R[2,0], sy) 49 | z = 0 50 | 51 | return array([x, y, z]) 52 | 53 | class Aruco(object): 54 | def __init__(self, robot, arucolibname, marker_size=ARUCO_MARKER_SIZE, disabled_ids=[]): 55 | self.arucolibname = arucolibname 56 | if arucolibname is not None: 57 | self.aruco_lib = cv2.aruco.getPredefinedDictionary(arucolibname) 58 | self.seen_marker_ids = [] 59 | self.seen_marker_objects = dict() 60 | self.disabled_ids = disabled_ids # disable markers with high false detection rates 61 | self.ids = [] 62 | self.corners = [] 63 | 64 | if robot.camera is None: return # robot is a SimRobot 65 | 66 | # Added for pose estimation 67 | self.marker_size = marker_size #these units will be pose est units!! 68 | self.image_size = (320,240) 69 | focal_len = robot.camera._config._focal_length 70 | self.camera_matrix = \ 71 | array([[focal_len.x , 0, self.image_size[0]/2], 72 | [0, -focal_len.y, self.image_size[1]/2], 73 | [0, 0, 1]]).astype(float) 74 | self.distortion_array = array([[0,0,0,0,0]]).astype(float) 75 | 76 | def process_image(self,gray): 77 | self.seen_marker_ids = [] 78 | self.seen_marker_objects = dict() 79 | (self.corners,self.ids,_) = \ 80 | cv2.aruco.detectMarkers(gray, self.aruco_lib) 81 | if self.ids is None: return 82 | 83 | # Estimate poses 84 | # Warning: OpenCV 3.2 estimate returns a pair; 3.3 returns a triplet 85 | estimate = \ 86 | cv2.aruco.estimatePoseSingleMarkers(self.corners, 87 | self.marker_size, 88 | self.camera_matrix, 89 | self.distortion_array) 90 | 91 | self.rvecs = estimate[0] 92 | self.tvecs = estimate[1] 93 | for i in range(len(self.ids)): 94 | id = int(self.ids[i][0]) 95 | if id in self.disabled_ids: continue 96 | tvec = self.tvecs[i][0] 97 | rvec = self.rvecs[i][0] 98 | if rvec[2] > math.pi/2 or rvec[2] < -math.pi/2: 99 | # can't see a marker facing away from us, so bogus 100 | print('Marker rejected! id=', id, 'tvec=', tvec, 'rvec=', rvec) 101 | continue 102 | marker = ArucoMarker(self, id, 103 | self.corners[i], self.tvecs[i][0], self.rvecs[i][0]) 104 | self.seen_marker_ids.append(marker.id) 105 | self.seen_marker_objects[marker.id] = marker 106 | 107 | def annotate(self, image, scale_factor): 108 | scaled_corners = [ multiply(corner, scale_factor) for corner in self.corners ] 109 | displayim = cv2.aruco.drawDetectedMarkers(image, scaled_corners, self.ids) 110 | 111 | #add poses currently fails since image is already scaled. How to scale camMat? 112 | #if(self.ids is not None): 113 | # for i in range(len(self.ids)): 114 | # displayim = cv2.aruco.drawAxis(displayim,self.cameraMatrix, 115 | # self.distortionArray,self.rvecs[i],self.tvecs[i]*scale_factor,self.axisLength*scale_factor) 116 | return displayim 117 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/CV_Hough.py: -------------------------------------------------------------------------------- 1 | """ 2 | CV_Hough demonstrates OpenCV's HoughLines and probabilistic HoughLinesP 3 | primitives. The 'edges' window displays the results of a Canny edge operator 4 | that is the input to the Hough transform. The 'Hough' window shows the 5 | output of HoughLines with the given settings of the r and theta tolerances 6 | and minimum bin count (threshold). The 'HoughP' window shows the output of 7 | HoughLinesP using the r and theta values from the Hough window, plus the 8 | minLineLength and maxLineGap parameters and its own bin count threshold. 9 | """ 10 | 11 | import cv2 12 | import numpy as np 13 | from cozmo_fsm import * 14 | 15 | class CV_Hough(StateMachineProgram): 16 | def __init__(self): 17 | super().__init__(aruco=False, particle_filter=False, cam_viewer=False, 18 | force_annotation=True, annotate_sdk=False) 19 | 20 | def start(self): 21 | cv2.namedWindow('gray') 22 | cv2.namedWindow('edges') 23 | cv2.namedWindow('Hough') 24 | cv2.namedWindow('HoughP') 25 | dummy = numpy.array([[0]], dtype='uint8') 26 | cv2.imshow('gray',dummy) 27 | cv2.imshow('edges',dummy) 28 | cv2.imshow('Hough',dummy) 29 | cv2.imshow('HoughP',dummy) 30 | 31 | self.h_lines = None 32 | self.p_lines = None 33 | 34 | cv2.createTrackbar('thresh1','edges',0,255,lambda self: None) 35 | cv2.createTrackbar('thresh2','edges',0,255,lambda self: None) 36 | cv2.setTrackbarPos('thresh1','edges',50) 37 | cv2.setTrackbarPos('thresh2','edges',150) 38 | 39 | cv2.createTrackbar('r_tol','Hough',1,10,lambda self: None) 40 | cv2.createTrackbar('deg_tol','Hough',1,18,lambda self: None) 41 | cv2.createTrackbar('h_thresh','Hough',1,250,lambda self: None) 42 | cv2.createTrackbar('h_main','Hough',0,1,lambda self: None) 43 | cv2.setTrackbarPos('r_tol','Hough',2) 44 | cv2.setTrackbarPos('deg_tol','Hough',2) 45 | cv2.setTrackbarPos('h_thresh','Hough',120) 46 | cv2.setTrackbarPos('h_main','Hough',0) 47 | 48 | cv2.createTrackbar('minLineLength','HoughP',1,80,lambda self: None) 49 | cv2.createTrackbar('maxLineGap','HoughP',1,50,lambda self: None) 50 | cv2.createTrackbar('p_thresh','HoughP',1,250,lambda self: None) 51 | cv2.setTrackbarPos('minLineLength','HoughP',40) 52 | cv2.setTrackbarPos('maxLineGap','HoughP',20) 53 | cv2.setTrackbarPos('p_thresh','HoughP',20) 54 | cv2.createTrackbar('p_main','HoughP',0,1,lambda self: None) 55 | cv2.setTrackbarPos('p_main','HoughP',0) 56 | super().start() 57 | 58 | def user_image(self,image,gray): 59 | self.gray = gray 60 | 61 | # Canny edge detector 62 | self.thresh1 = cv2.getTrackbarPos('thresh1','edges') 63 | self.thresh2 = cv2.getTrackbarPos('thresh2','edges') 64 | self.edges = cv2.Canny(gray, self.thresh1, self.thresh2, apertureSize=3) 65 | 66 | # regular Hough 67 | self.r_tol = max(0.1, cv2.getTrackbarPos('r_tol','Hough')) 68 | self.deg_tol = max(0.1, cv2.getTrackbarPos('deg_tol','Hough')) 69 | self.h_thresh = cv2.getTrackbarPos('h_thresh','Hough') 70 | self.h_lines = cv2.HoughLines(self.edges, self.r_tol, 71 | self.deg_tol/180.*np.pi, 72 | self.h_thresh) 73 | # probabilistic Hough 74 | self.p_thresh = cv2.getTrackbarPos('p_thresh','HoughP') 75 | self.minLineLength = cv2.getTrackbarPos('minLineLength','HoughP') 76 | self.maxLineGap = cv2.getTrackbarPos('maxLineGap','HoughP') 77 | self.p_lines = cv2.HoughLinesP(self.edges, self.r_tol, self.deg_tol/180.*np.pi, 78 | self.p_thresh, None, 79 | self.minLineLength, self.maxLineGap) 80 | 81 | def user_annotate(self,image): 82 | cv2.imshow('gray',self.gray) 83 | cv2.imshow('edges',self.edges) 84 | if self.h_lines is not None: 85 | hough_image = cv2.cvtColor(self.edges,cv2.COLOR_GRAY2BGR) 86 | h_main = cv2.getTrackbarPos('h_main','Hough') 87 | for line in self.h_lines: 88 | rho, theta = line[0] 89 | a = np.cos(theta) 90 | b = np.sin(theta) 91 | x0 = a * rho 92 | y0 = b * rho 93 | x1 = int(x0 + 1000*(-b)) 94 | y1 = int(y0 + 1000*a) 95 | x2 = int(x0 - 1000*(-b)) 96 | y2 = int(y0 - 1000*a) 97 | cv2.line(hough_image,(x1,y1),(x2,y2),(0,255,0),1) 98 | if h_main: 99 | cv2.line(image,(2*x1,2*y1),(2*x2,2*y2),(0,255,0),2) 100 | cv2.imshow('Hough',hough_image) 101 | if self.p_lines is not None: 102 | houghp_image = cv2.cvtColor(self.edges,cv2.COLOR_GRAY2BGR) 103 | p_main = cv2.getTrackbarPos('p_main','HoughP') 104 | for line in self.p_lines: 105 | x1,y1,x2,y2 = line[0] 106 | cv2.line(houghp_image,(x1,y1),(x2,y2),(255,0,0),1) 107 | if p_main: 108 | cv2.line(image,(2*x1,2*y1),(2*x2,2*y2),(255,0,0),2) 109 | cv2.imshow('HoughP',houghp_image) 110 | cv2.waitKey(1) 111 | return image 112 | -------------------------------------------------------------------------------- /cozmo_fsm/speech.py: -------------------------------------------------------------------------------- 1 | try: 2 | import speech_recognition as sr 3 | except: pass 4 | 5 | import time 6 | from threading import Thread 7 | 8 | from .evbase import Event 9 | from .events import SpeechEvent 10 | 11 | #================ Thesaurus ================ 12 | 13 | class Thesaurus(): 14 | def __init__(self): 15 | self.words = dict() 16 | self.add_homophones('cozmo', \ 17 | ["cozimo","cosimo","cosmo", \ 18 | "kozmo","cosmos","cozmos"]) 19 | self.add_homophones('right', ['write','wright']) 20 | self.add_homophones('1',['one','won']) 21 | self.add_homophones('cube1',['q1','coupon','cuban']) 22 | self.phrase_tree = dict() 23 | self.add_phrases('cube1',['cube 1']) 24 | self.add_phrases('cube2',['cube 2']) 25 | self.add_phrases('cube2',['cube to']) 26 | self.add_phrases('cube3',['cube 3']) 27 | self.add_phrases('paperclip',['paper clip']) 28 | self.add_phrases('deli-slicer',['deli slicer']) 29 | 30 | 31 | def add_homophones(self,word,homophones): 32 | if not isinstance(homophones,list): 33 | homophones = [homophones] 34 | for h in homophones: 35 | self.words[h] = word 36 | 37 | def lookup_word(self,word): 38 | return self.words.get(word,word) 39 | 40 | def add_phrases(self,word,phrases): 41 | if not isinstance(phrases,list): 42 | phrases = [phrases] 43 | for phrase in phrases: 44 | wdict = self.phrase_tree 45 | for pword in phrase.split(' '): 46 | wdict[pword] = wdict.get(pword,dict()) 47 | wdict = wdict[pword] 48 | wdict[''] = word 49 | 50 | def substitute_phrases(self,words): 51 | result = [] 52 | while words != []: 53 | word = words[0] 54 | del words[0] 55 | wdict = self.phrase_tree.get(word,None) 56 | if wdict is None: 57 | result.append(word) 58 | continue 59 | prefix = [word] 60 | while words != []: 61 | wdict2 = wdict.get(words[0],None) 62 | if wdict2 is None: break 63 | prefix.append(words[0]) 64 | del words[0] 65 | wdict = wdict2 66 | subst = wdict.get('',None) 67 | if subst is not None: 68 | result.append(subst) 69 | else: 70 | result = result + prefix 71 | return result 72 | 73 | #================ SpeechListener ================ 74 | 75 | class SpeechListener(): 76 | def __init__(self,robot, thesaurus=Thesaurus(), debug=False): 77 | self.robot = robot 78 | self.thesaurus = thesaurus 79 | self.debug = debug 80 | 81 | def speech_listener(self): 82 | warned_no_mic = False 83 | print('Launched speech listener.') 84 | self.rec = sr.Recognizer() 85 | while True: 86 | try: 87 | with sr.Microphone() as source: 88 | if warned_no_mic: 89 | print('Got a microphone!') 90 | warned_no_mic = False 91 | while True: 92 | if self.debug: print('--> Listening...') 93 | try: 94 | audio = self.rec.listen(source, timeout=8, phrase_time_limit=8) 95 | audio_len = len(audio.frame_data) 96 | except: 97 | continue 98 | if self.debug: 99 | print('--> Got audio data: length = {:,d} bytes.'. \ 100 | format(audio_len)) 101 | if audio_len > 1000000: #500000: 102 | print('**** Audio segment too long. Try again.') 103 | continue 104 | try: 105 | utterance = self.rec.recognize_google(audio).lower() 106 | print("Raw utterance: '%s'" % utterance) 107 | words = [self.thesaurus.lookup_word(w) for w in utterance.split(" ")] 108 | words = self.thesaurus.substitute_phrases(words) 109 | string = " ".join(words) 110 | print("Heard: '%s'" % string) 111 | evt = SpeechEvent(string,words) 112 | self.robot.erouter.post(evt) 113 | except sr.RequestError as e: 114 | print("Could not request results from google speech recognition service; {0}".format(e)) 115 | except sr.UnknownValueError: 116 | if self.debug: 117 | print('--> Recognizer found no words.') 118 | except Exception as e: 119 | print('Speech recognition got exception:', repr(e)) 120 | except OSError as e: 121 | if not warned_no_mic: 122 | print("Couldn't get a microphone:",e) 123 | warned_no_mic = True 124 | time.sleep(10) 125 | 126 | def start(self): 127 | self.thread = Thread(target=self.speech_listener) 128 | self.thread.daemon = True #ending fg program will kill bg program 129 | self.thread.start() 130 | -------------------------------------------------------------------------------- /cozmo_fsm/wall_defs.py: -------------------------------------------------------------------------------- 1 | from .worldmap import * 2 | 3 | # Disabled ArUco ids: 17 and 37 4 | 5 | def make_walls(): 6 | 7 | # ~12 inch walls 8 | 9 | w1 = WallSpec(length=600, height=190, door_width=77, door_height=110, 10 | marker_specs={ 11 | 'Aruco-1' : (+1, ( 62., 30.)), 12 | 'Aruco-2' : (+1, (150.,150.)), 13 | 'Aruco-3' : (+1, (238., 30.)), 14 | 'Aruco-4' : (+1, (362., 30.)), 15 | 'Aruco-5' : (+1, (450.,150.)), 16 | 'Aruco-6' : (+1, (538., 30.)), 17 | 'Aruco-12' : (-1, ( 62., 30.)), 18 | 'Aruco-11' : (-1, (150.,150.)), 19 | 'Aruco-10' : (-1, (238., 30.)), 20 | 'Aruco-9' : (-1, (362., 30.)), 21 | 'Aruco-8' : (-1, (450.,150.)), 22 | 'Aruco-7' : (-1, (538., 30.)) 23 | }, 24 | doorways = [ (150., 77.), (450., 77.) ], 25 | door_ids = [ (2, 8), (5, 11) ]) 26 | 27 | w13 = WallSpec(length=600, height=190, door_width=77, door_height=110, 28 | marker_specs={ 29 | 'Aruco-13' : (+1, ( 62., 30.)), 30 | 'Aruco-14' : (+1, (150.,150.)), 31 | 'Aruco-15' : (+1, (238., 30.)), 32 | 'Aruco-16' : (+1, (362., 30.)), 33 | 'Aruco-18' : (+1, (450.,150.)), 34 | 'Aruco-19' : (+1, (538., 30.)), 35 | 'Aruco-25' : (-1, ( 62., 30.)), 36 | 'Aruco-24' : (-1, (150.,150.)), 37 | 'Aruco-23' : (-1, (238., 30.)), 38 | 'Aruco-22' : (-1, (362., 30.)), 39 | 'Aruco-21' : (-1, (450.,150.)), 40 | 'Aruco-20' : (-1, (538., 30.)) 41 | }, 42 | doorways = [ (150., 77.), (450., 77.) ], 43 | door_ids = [ (14, 21), (18, 24) ]) 44 | 45 | w26 = WallSpec(length=600, height=190, door_width=77, door_height=110, 46 | marker_specs={ 47 | 'Aruco-26' : (+1, ( 62., 30.)), 48 | 'Aruco-27' : (+1, (150.,150.)), 49 | 'Aruco-28' : (+1, (238., 30.)), 50 | 'Aruco-29' : (+1, (362., 30.)), 51 | 'Aruco-30' : (+1, (450.,150.)), 52 | 'Aruco-31' : (+1, (538., 30.)), 53 | 'Aruco-38' : (-1, ( 62., 30.)), 54 | 'Aruco-36' : (-1, (150.,150.)), 55 | 'Aruco-35' : (-1, (238., 30.)), 56 | 'Aruco-34' : (-1, (362., 30.)), 57 | 'Aruco-33' : (-1, (450.,150.)), 58 | 'Aruco-32' : (-1, (538., 30.)) 59 | }, 60 | doorways = [ (150., 77.), (450., 77.) ], 61 | door_ids = [ (27, 33), (30, 36) ]) 62 | 63 | 64 | # ~6 inch walls 65 | 66 | w39 = WallSpec(length=300, height=190, door_width=77, door_height=110, 67 | marker_specs={ 68 | 'Aruco-39' : (+1, ( 62., 30.)), 69 | 'Aruco-40' : (+1, (150.,150.)), 70 | 'Aruco-41' : (+1, (238., 30.)), 71 | 'Aruco-44' : (-1, ( 62., 30.)), 72 | 'Aruco-43' : (-1, (150.,150.)), 73 | 'Aruco-42' : (-1, (238., 30.)) 74 | }, 75 | doorways = [ (150., 77.) ], 76 | door_ids = [ (40, 43) ]) 77 | 78 | w45 = WallSpec(length=300, height=190, door_width=77, door_height=110, 79 | marker_specs={ 80 | 'Aruco-45' : (+1, ( 62., 30.)), 81 | 'Aruco-46' : (+1, (150.,150.)), 82 | 'Aruco-47' : (+1, (238., 30.)), 83 | 'Aruco-50' : (-1, ( 62., 30.)), 84 | 'Aruco-49' : (-1, (150.,150.)), 85 | 'Aruco-48' : (-1, (238., 30.))} 86 | , 87 | doorways = [ (150., 77.) ], 88 | door_ids = [ (46, 49) ]) 89 | 90 | 91 | # ~9 inch walls 92 | w51 = WallSpec(length=400, height=190, door_width=77, door_height=110, 93 | marker_specs={ 94 | 'Aruco-51' : (+1, (112., 30.)), 95 | 'Aruco-52' : (+1, (200.,150.)), 96 | 'Aruco-53' : (+1, (288., 30.)), 97 | 'Aruco-56' : (-1, (112., 30.)), 98 | 'Aruco-55' : (-1, (200.,150.)), 99 | 'Aruco-54' : (-1, (288., 30.)) 100 | }, 101 | doorways = [ (200., 77.) ], 102 | door_ids = [ (52, 55) ]) 103 | 104 | w57 = WallSpec(length=400, height=190, door_width=77, door_height=110, 105 | marker_specs={ 106 | 'Aruco-57' : (+1, (112., 30.)), 107 | 'Aruco-58' : (+1, (200.,150.)), 108 | 'Aruco-59' : (+1, (288., 30.)), 109 | 'Aruco-62' : (-1, (112., 30.)), 110 | 'Aruco-61' : (-1, (200.,150.)), 111 | 'Aruco-60' : (-1, (288., 30.)) 112 | }, 113 | doorways = [ (200., 77.) ], 114 | door_ids = [ (58, 61) ]) 115 | 116 | 117 | # Walls without markers 118 | 119 | wA = WallSpec(label='A', length=300, height=190) 120 | 121 | wB = WallSpec(label='B', length=400, height=190) 122 | 123 | wC = WallSpec(label='C', length=100, height=190) 124 | 125 | wD = WallSpec(label='D', length=50, height=190) 126 | 127 | make_walls() 128 | -------------------------------------------------------------------------------- /event_monitor.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Event Monitor Tool for Cozmo 4 | ============================ 5 | 6 | Usage: 7 | monitor(robot) to monitor all event types in the dispatch table 8 | monitor(robot, Event) to monitor a specific type of event 9 | 10 | unmonitor(robot[, Event]) to turn off monitoring 11 | 12 | Author: David S. Touretzky, Carnegie Mellon University 13 | ===== 14 | 15 | ChangeLog 16 | ========= 17 | 18 | * Add event handlers to world instead of to robot. 19 | Dave Touretzky 20 | - Many events (e.g. face stuff) aren't reliably sent to robot. 21 | 22 | * Renaming and more face support 23 | Dave Touretzky 24 | - Renamed module to event_monitor 25 | - Renamed monitor_on/off to monitor/unmonitor 26 | - Added monitor_face to handle face events 27 | 28 | * Created 29 | Dave Touretzky 30 | 31 | """ 32 | 33 | import re 34 | 35 | import cozmo 36 | 37 | 38 | def print_prefix(evt): 39 | robot.world.last_event = evt 40 | print('-> ', evt.event_name, ' ', sep='', end='') 41 | 42 | 43 | def print_object(obj): 44 | if isinstance(obj,cozmo.objects.LightCube): 45 | cube_id = next(k for k,v in robot.world.light_cubes.items() if v==obj) 46 | print('LightCube-',cube_id,sep='',end='') 47 | else: 48 | r = re.search('<(\w*)', obj.__repr__()) 49 | print(r.group(1), end='') 50 | 51 | 52 | def monitor_generic(evt, **kwargs): 53 | print_prefix(evt) 54 | if 'behavior_type_name' in kwargs: 55 | print(kwargs['behavior_type_name'], '', end='') 56 | print(' ', end='') 57 | if 'obj' in kwargs: 58 | print_object(kwargs['obj']) 59 | print(' ', end='') 60 | if 'action' in kwargs: 61 | action = kwargs['action'] 62 | if isinstance(action, cozmo.anim.Animation): 63 | print(action.anim_name, '', end='') 64 | elif isinstance(action, cozmo.anim.AnimationTrigger): 65 | print(action.trigger.name, '', end='') 66 | print(set(kwargs.keys())) 67 | 68 | 69 | def monitor_EvtActionCompleted(evt, action, state, failure_code, failure_reason, **kwargs): 70 | print_prefix(evt) 71 | print_object(action) 72 | if isinstance(action, cozmo.anim.Animation): 73 | print('', action.anim_name, end='') 74 | elif isinstance(action, cozmo.anim.AnimationTrigger): 75 | print('', action.trigger.name, end='') 76 | print('',state,end='') 77 | if failure_code is not None: 78 | print('',failure_code,failure_reason,end='') 79 | print() 80 | 81 | 82 | def monitor_EvtObjectTapped(evt, *, obj, tap_count, tap_duration, tap_intensity, **kwargs): 83 | print_prefix(evt) 84 | print_object(obj) 85 | print(' count=', tap_count, 86 | ' duration=', tap_duration, ' intensity=', tap_intensity, sep='') 87 | 88 | 89 | def monitor_EvtObjectMovingStarted(evt, *, obj, acceleration, **kwargs): 90 | print_prefix(evt) 91 | print_object(obj) 92 | print(' accleration=', acceleration, sep='') 93 | 94 | 95 | def monitor_EvtObjectMovingStopped(evt, *, obj, move_duration, **kwargs): 96 | print_prefix(evt) 97 | print_object(obj) 98 | print(' move_duration=%3.1f secs' %move_duration) 99 | 100 | 101 | def monitor_face(evt, face, **kwargs): 102 | print_prefix(evt) 103 | name = face.name if face.name != '' else '[unknown face]' 104 | expr = face.expression if face.expression is not None else 'expressionless' 105 | kw = set(kwargs.keys()) if len(kwargs) > 0 else '{}' 106 | print(name, ' (%s) ' % expr, ' face_id=', face.face_id, ' ', kw, sep='') 107 | 108 | dispatch_table = { 109 | cozmo.action.EvtActionStarted : monitor_generic, 110 | cozmo.action.EvtActionCompleted : monitor_EvtActionCompleted, 111 | cozmo.behavior.EvtBehaviorStarted : monitor_generic, 112 | cozmo.behavior.EvtBehaviorStopped : monitor_generic, 113 | cozmo.anim.EvtAnimationsLoaded : monitor_generic, 114 | cozmo.anim.EvtAnimationCompleted : monitor_EvtActionCompleted, 115 | cozmo.objects.EvtObjectAppeared : monitor_generic, 116 | cozmo.objects.EvtObjectDisappeared : monitor_generic, 117 | cozmo.objects.EvtObjectMovingStarted : monitor_EvtObjectMovingStarted, 118 | cozmo.objects.EvtObjectMovingStopped : monitor_EvtObjectMovingStopped, 119 | cozmo.objects.EvtObjectObserved : monitor_generic, 120 | cozmo.objects.EvtObjectTapped : monitor_EvtObjectTapped, 121 | cozmo.faces.EvtFaceAppeared : monitor_face, 122 | cozmo.faces.EvtFaceObserved : monitor_face, 123 | cozmo.faces.EvtFaceDisappeared : monitor_face, 124 | } 125 | 126 | excluded_events = { # Occur too frequently to monitor by default 127 | cozmo.objects.EvtObjectObserved, 128 | cozmo.faces.EvtFaceObserved, 129 | } 130 | 131 | 132 | def monitor(_robot, evt_class=None): 133 | if not isinstance(_robot, cozmo.robot.Robot): 134 | raise TypeError('First argument must be a Robot instance') 135 | if evt_class is not None and not issubclass(evt_class, cozmo.event.Event): 136 | raise TypeError('Second argument must be an Event subclass') 137 | global robot 138 | robot = _robot 139 | if evt_class in dispatch_table: 140 | robot.world.add_event_handler(evt_class,dispatch_table[evt_class]) 141 | elif evt_class is not None: 142 | robot.world.add_event_handler(evt_class,monitor_generic) 143 | else: 144 | for k,v in dispatch_table.items(): 145 | if k not in excluded_events: 146 | robot.world.add_event_handler(k,v) 147 | 148 | 149 | def unmonitor(_robot, evt_class=None): 150 | if not isinstance(_robot, cozmo.robot.Robot): 151 | raise TypeError('First argument must be a Robot instance') 152 | if evt_class is not None and not issubclass(evt_class, cozmo.event.Event): 153 | raise TypeError('Second argument must be an Event subclass') 154 | global robot 155 | robot = _robot 156 | try: 157 | if evt_class in dispatch_table: 158 | robot.world.remove_event_handler(evt_class,dispatch_table[evt_class]) 159 | elif evt_class is not None: 160 | robot.world.remove_event_handler(evt_class,monitor_generic) 161 | else: 162 | for k,v in dispatch_table.items(): 163 | robot.world.remove_event_handler(k,v) 164 | except Exception: 165 | pass 166 | 167 | -------------------------------------------------------------------------------- /cozmo_fsm/cozmo_kin.py: -------------------------------------------------------------------------------- 1 | from math import pi, tan 2 | 3 | import cozmo 4 | 5 | from .kine import * 6 | from cozmo_fsm import geometry 7 | from .geometry import tprint, point, translation_part, rotation_part 8 | from .rrt_shapes import * 9 | 10 | # ================ Constants ================ 11 | 12 | wheelbase = 45 # millimeters 13 | front_wheel_diameter = 52 # millimeters 14 | hook_spacing = 35 # millimeters 15 | center_of_rotation_offset = -19.7 # millimeters 16 | 17 | # ================================================================ 18 | 19 | class CozmoKinematics(Kinematics): 20 | def __init__(self,robot): 21 | base_frame = Joint('base', 22 | description='Base frame: the root of the kinematic tree') 23 | 24 | # cor is center of rotation 25 | cor_frame = Joint('cor', parent=base_frame, 26 | description='Center of rotation', 27 | r=-19., 28 | collision_model=Rectangle(geometry.point(), 29 | dimensions=(95,60))) 30 | 31 | # Use link instead of joint for world_frame 32 | world_frame = Joint('world', parent=base_frame, type='world', getter=self.get_world, 33 | description='World origin in base frame coordinates', 34 | qmin=None, qmax=None) 35 | 36 | front_axle_frame = Joint('front_axle', parent=base_frame, 37 | description='Center of the front axle', 38 | d=front_wheel_diameter/2, alpha=pi/2) 39 | back_axle_frame = Joint('back_axle', parent=base_frame, r=-46., alpha=pi/2) 40 | 41 | # This frame is on the midline. Could add separate left and right shoulders. 42 | # Positive angle is up, so z must point to the right. 43 | # x is forward, y points up. 44 | shoulder_frame = Joint('shoulder', parent=base_frame, 45 | type='revolute', getter=self.get_shoulder, 46 | description='Rotation axis of the lift; z points to the right', 47 | qmin=cozmo.robot.MIN_LIFT_ANGLE.radians, 48 | qmax=cozmo.robot.MAX_LIFT_ANGLE.radians, 49 | d=21., r=-39., alpha=pi/2) 50 | 51 | lift_attach_frame = \ 52 | Joint('lift_attach', parent=shoulder_frame, type='revolute', 53 | description='Tip of the lift, where cubes attach; distal end of four-bar linkage', 54 | getter=self.get_lift_attach, r=66., 55 | qmax = - cozmo.robot.MIN_LIFT_ANGLE.radians, 56 | qmin = - cozmo.robot.MAX_LIFT_ANGLE.radians, 57 | #collision_model=Circle(geometry.point(), radius=10)) 58 | ) 59 | 60 | # Positive head angle is up, so z must point to the right. 61 | # With x pointing forward, y must point up. 62 | head_frame = Joint('head', parent=base_frame, type='revolute', 63 | getter=self.get_head, 64 | description='Axis of head rotation; z points to the right', 65 | qmin=cozmo.robot.MIN_HEAD_ANGLE.radians, 66 | qmax=cozmo.robot.MAX_HEAD_ANGLE.radians, 67 | d=35., r=-10., alpha=pi/2) 68 | 69 | # Dummy joint located below head joint at level of the camera frame, 70 | # and x axis points down, z points forward, y points left 71 | camera_dummy = Joint('camera_dummy', parent=head_frame, 72 | description='Dummy joint below the head, at the level of the camera frame', 73 | theta=-pi/2, r=7.5, alpha=-pi/2) 74 | # x axis points right, y points down, z points forward 75 | camera_frame = Joint('camera', parent=camera_dummy, 76 | description='Camera reference frame; y is down and z is outward', 77 | d=15., theta=-pi/2) 78 | 79 | joints = [base_frame, world_frame, cor_frame, 80 | front_axle_frame, back_axle_frame, 81 | shoulder_frame, lift_attach_frame, 82 | head_frame, camera_dummy, camera_frame] 83 | 84 | super().__init__(joints,robot) 85 | 86 | def get_head(self): 87 | return self.robot.head_angle.radians 88 | 89 | def get_shoulder(self): 90 | # Formula supplied by Mark Wesley at Anki 91 | # Check SDK documentation for new lift-related calls that might replace this 92 | return math.asin( (self.robot.lift_height.distance_mm-45.0) / 66.0) 93 | 94 | def get_lift_attach(self): 95 | return -self.get_shoulder() 96 | 97 | def get_world(self): 98 | return self.robot.world.particle_filter.pose_estimate() 99 | 100 | def project_to_ground(self,cx,cy): 101 | "Converts camera coordinates to a ground point in the base frame." 102 | # Formula taken from Tekkotsu's projectToGround method 103 | camera_res = (320, 240) 104 | half_camera_max = max(*camera_res) / 2 105 | config = self.robot.camera.config 106 | # Convert to generalized coordinates in range [-1, 1] 107 | gx = (cx-config.center.x) / half_camera_max 108 | gy = (cy-config.center.y) / half_camera_max 109 | #tekkotsu_focal_length_x = camera_res[0]/camera_max / tan(config.fov_x.radians/2) 110 | #tekkotsu_focal_length_y = camera_res[1]/camera_max / tan(config.fov_y.radians/2) 111 | # Generate a ray in the camera frame 112 | rx = gx / (config.focal_length.x / half_camera_max) 113 | ry = gy / (config.focal_length.y / half_camera_max) 114 | ray = point(rx,ry,1) 115 | 116 | cam_to_base = self.robot.kine.joint_to_base('camera') 117 | offset = translation_part(cam_to_base) 118 | rot_ray = rotation_part(cam_to_base).dot(ray) 119 | dist = - offset[2,0] 120 | align = rot_ray[2,0] 121 | 122 | if abs(align) > 1e-5: 123 | s = dist / align 124 | hit = point(rot_ray[0,0]*s, rot_ray[1,0]*s, rot_ray[2,0]*s) + offset 125 | elif align * dist < 0: 126 | hit = point(-rot_ray[0,0], -rot_ray[1,0], -rot_ray[2,0], abs(align)) 127 | else: 128 | hit = point(rot_ray[0,0], rot_ray[1,0], rot_ray[2,0], abs(align)) 129 | return hit 130 | -------------------------------------------------------------------------------- /cozmo_fsm/perched.py: -------------------------------------------------------------------------------- 1 | from numpy import matrix, array, ndarray, sqrt, arctan2, pi 2 | import threading 3 | from time import sleep 4 | from .geometry import wrap_angle 5 | 6 | try: 7 | import cv2 8 | import cv2.aruco as aruco 9 | except: pass 10 | 11 | # Known camera parameters 12 | 13 | # Microsoft HD ( Calibrated to death ) 14 | microsoft_HD_webcam_cameraMatrix = matrix([[1148.00, -3, 641.0], 15 | [0.000000, 1145.0, 371.0], 16 | [0.000000, 0.000000, 1.000000]]) 17 | microsoft_HD_webcam_distCoeffs = array([0.211679, -0.179776, 0.041896, 0.040334, 0.000000]) 18 | 19 | class Cam(): 20 | def __init__(self,cap,x,y,z,phi, theta): 21 | self.cap = cap 22 | self.x = x 23 | self.y = y 24 | self.z = z 25 | self.phi = phi 26 | self.theta = theta 27 | 28 | def __repr__(self): 29 | return ' @ %.2f' % \ 30 | (self.x, self.y, self.z,self.phi*180/pi) 31 | 32 | class PerchedCameraThread(threading.Thread): 33 | def __init__(self, robot): 34 | threading.Thread.__init__(self) 35 | self.robot = robot 36 | self.use_perched_cameras = False 37 | self.perched_cameras = [] 38 | # Set camera parameters. (Current code assumes same parameters for all cameras connected to a computer.) 39 | self.cameraMatrix = microsoft_HD_webcam_cameraMatrix 40 | self.distCoeffs = microsoft_HD_webcam_distCoeffs 41 | self.aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250) 42 | self.parameters = aruco.DetectorParameters_create() 43 | # camera landmarks from local cameras 44 | self.cameras = {} 45 | # camera landamrks from network (sent from server) 46 | self.camera_pool = {} 47 | 48 | def run(self): 49 | while(True): 50 | if self.use_perched_cameras: 51 | self.process_image() 52 | # Computer overloaded if not given break 53 | sleep(0.01) 54 | else: 55 | break 56 | 57 | def start_perched_camera_thread(self,cameras=[]): 58 | if not isinstance(cameras,list): 59 | cameras = [cameras] 60 | 61 | if self.robot.aruco_id == -1: 62 | self.robot.aruco_id = int(input("Please enter the aruco id of the robot:")) 63 | self.robot.world.server.camera_landmark_pool[self.robot.aruco_id]={} 64 | self.use_perched_cameras=True 65 | self.perched_cameras = [] 66 | for x in cameras: 67 | cap = cv2.VideoCapture(x) 68 | if cap.isOpened(): 69 | self.perched_cameras.append(cap) 70 | else: 71 | raise RuntimeError("Could not open camera %s." % repr(x)) 72 | for cap in self.perched_cameras: 73 | # hack to set highest resolution 74 | cap.set(3,4000) 75 | cap.set(4,4000) 76 | self.robot.world.particle_filter.sensor_model.use_perched_cameras = True 77 | print("Particle filter now using perched cameras") 78 | self.start() 79 | 80 | def stop_perched_camera_thread(self): 81 | self.use_perched_cameras=False 82 | sleep(1) 83 | for cap in self.perched_cameras: 84 | cap.release() 85 | self.robot.world.particle_filter.sensor_model.use_perched_cameras = False 86 | print("Particle filter stopped using perched cameras") 87 | 88 | def check_camera(self,camera): 89 | cap = cv2.VideoCapture(camera) 90 | for j in range(10): 91 | # hack to clear buffer 92 | for i in range(5): 93 | cap.grab() 94 | ret, frame = cap.read() 95 | if not ret: 96 | print('Failed to get camera frame from camera %s.' % camera ) 97 | return 98 | gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY) 99 | corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, self.aruco_dict, parameters=self.parameters) 100 | gray = cv2.aruco.drawDetectedMarkers(gray, corners, ids) 101 | cv2.imshow("Camera:"+str(camera),gray) 102 | cv2.waitKey(1) 103 | cap.release() 104 | cv2.destroyAllWindows() 105 | 106 | def rotationMatrixToEulerAngles(self, R) : 107 | sy = sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) 108 | singular = sy < 1e-6 109 | if not singular : 110 | x = arctan2(R[2,1] , R[2,2]) 111 | y = arctan2(-R[2,0], sy) 112 | z = arctan2(R[1,0], R[0,0]) 113 | else : 114 | x = arctan2(-R[1,2], R[1,1]) 115 | y = arctan2(-R[2,0], sy) 116 | z = 0 117 | 118 | return array([x, y, z]) 119 | 120 | def process_image(self): 121 | # Dict with key: aruco id with values as cameras that can see the marker 122 | self.temp_cams = {} # Necessary, else self.cameras is empty most of the time 123 | for cap in self.perched_cameras: 124 | # Clearing Buffer by grabbing five frames 125 | for i in range(5): 126 | cap.grab() 127 | ret, frame = cap.read() 128 | gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY) 129 | corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, self.aruco_dict, parameters=self.parameters) 130 | 131 | if type(ids) is ndarray: 132 | vecs = aruco.estimatePoseSingleMarkers(corners, 50, self.cameraMatrix, self.distCoeffs) 133 | rvecs, tvecs = vecs[0], vecs[1] 134 | for i in range(len(ids)): 135 | rotationm, jcob = cv2.Rodrigues(rvecs[i]) 136 | # transform to robot coordinate frame 137 | transformed = matrix(rotationm).T*(-matrix(tvecs[i]).T) 138 | phi = self.rotationMatrixToEulerAngles(rotationm.T) 139 | if ids[i][0] in self.temp_cams: 140 | self.temp_cams[ids[i][0]][str(cap)]=Cam(str(cap),transformed[0][0,0], 141 | transformed[1][0,0],transformed[2][0,0],wrap_angle(phi[2]-pi/2), wrap_angle(phi[0]+pi/2)) 142 | else: 143 | self.temp_cams[ids[i][0]]={str(cap):Cam(str(cap),transformed[0][0,0], 144 | transformed[1][0,0],transformed[2][0,0],wrap_angle(phi[2]-pi/2), wrap_angle(phi[0]+pi/2))} 145 | self.cameras = self.temp_cams 146 | 147 | # Only server clears the pool 148 | if self.robot.world.is_server: 149 | self.camera_pool = self.temp_cams 150 | -------------------------------------------------------------------------------- /cozmo_fsm/rrt_shapes.py: -------------------------------------------------------------------------------- 1 | from cozmo_fsm import geometry 2 | from math import sqrt, pi, atan2 3 | import numpy as np 4 | 5 | class Shape(): 6 | def __init__(self, center=geometry.point()): 7 | if center is None: raise ValueError() 8 | self.center = center 9 | self.rotmat = geometry.identity() 10 | self.obstacle_id = None # only store the string, so shape is pickle-able 11 | 12 | def __repr__(self): 13 | return "<%s >" % (self.__class__.__name__) 14 | 15 | def collides(self, shape): 16 | if isinstance(shape, Rectangle): 17 | return self.collides_rect(shape) 18 | elif isinstance(shape, Polygon): 19 | return self.collides_poly(shape) 20 | elif isinstance(shape, Circle): 21 | return self.collides_circle(shape) 22 | elif isinstance(shape, Compound): 23 | return shape.collides(self) 24 | else: 25 | raise Exception("%s has no collides() method defined for %s." % (self, shape)) 26 | 27 | def get_bounding_box(self): 28 | """Should return ((xmin,ymin), (xmax,ymax))""" 29 | raise NotImplementedError("get_bounding_box") 30 | 31 | #================ Basic Shapes ================ 32 | 33 | class Circle(Shape): 34 | def __init__(self, center=geometry.point(), radius=25/2): 35 | super().__init__(center) 36 | self.radius = radius 37 | self.orient = 0. 38 | 39 | def __repr__(self): 40 | id = self.obstacle_id if self.obstacle_id else '[no obstacle]' 41 | return '' % \ 42 | (self.center[0,0], self.center[1,0], self.radius, id) 43 | 44 | def instantiate(self, tmat): 45 | return Circle(center=tmat.dot(self.center), radius=self.radius) 46 | 47 | def collides_rect(self,rect): 48 | return rect.collides_circle(self) 49 | 50 | def collides_poly(self,poly): 51 | return poly.collides(self) 52 | 53 | def collides_circle(self,circle): 54 | dx = self.center[0,0] - circle.center[0,0] 55 | dy = self.center[1,0] - circle.center[1,0] 56 | dist = sqrt(dx*dx + dy*dy) 57 | return dist < (self.radius + circle.radius) 58 | 59 | def get_bounding_box(self): 60 | xmin = self.center[0,0] - self.radius 61 | xmax = self.center[0,0] + self.radius 62 | ymin = self.center[1,0] - self.radius 63 | ymax = self.center[1,0] + self.radius 64 | return ((xmin,ymin), (xmax,ymax)) 65 | 66 | class Polygon(Shape): 67 | def __init__(self, vertices=None, orient=0): 68 | center = vertices.mean(1) 69 | center.resize(4,1) 70 | super().__init__(center) 71 | self.vertices = vertices 72 | self.orient = orient # should move vertex rotation code from Rectangle to here 73 | N = vertices.shape[1] 74 | self.edges = tuple( (vertices[:,i:i+1], vertices[:,(i+1)%N:((i+1)%N)+1]) 75 | for i in range(N) ) 76 | 77 | def get_bounding_box(self): 78 | mins = self.vertices.min(1) 79 | maxs = self.vertices.max(1) 80 | xmin = mins[0] 81 | ymin = mins[1] 82 | xmax = maxs[0] 83 | ymax = maxs[1] 84 | return ((xmin,ymin), (xmax,ymax)) 85 | 86 | def collides_poly(self,poly): 87 | raise NotImplementedError() 88 | 89 | def collides_circle(self,circle): 90 | raise NotImplementedError() 91 | 92 | 93 | class Rectangle(Polygon): 94 | def __init__(self, center=None, dimensions=None, orient=0): 95 | self.dimensions = dimensions 96 | self.orient = orient 97 | if not isinstance(dimensions[0],(float,int)): 98 | raise ValueError(dimensions) 99 | dx2 = dimensions[0]/2 100 | dy2 = dimensions[1]/2 101 | relative_vertices = np.array([[-dx2, dx2, dx2, -dx2 ], 102 | [-dy2, -dy2, dy2, dy2 ], 103 | [ 0, 0, 0, 0 ], 104 | [ 1, 1, 1, 1 ]]) 105 | self.unrot = geometry.aboutZ(-orient) 106 | center_ex = self.unrot.dot(center) 107 | extents = geometry.translate(center_ex[0,0],center_ex[1,0]).dot(relative_vertices) 108 | # Extents measured along the rectangle's axes, not world axes 109 | self.min_Ex = min(extents[0,:]) 110 | self.max_Ex = max(extents[0,:]) 111 | self.min_Ey = min(extents[1,:]) 112 | self.max_Ey = max(extents[1,:]) 113 | vertices = geometry.translate(center[0,0],center[1,0]).dot( 114 | geometry.aboutZ(orient).dot(relative_vertices)) 115 | super().__init__(vertices=vertices, orient=orient) 116 | 117 | def __repr__(self): 118 | id = self.obstacle_id if self.obstacle_id else '[no obstacle]' 119 | return '' % \ 120 | (self.center[0,0],self.center[1,0],*self.dimensions, 121 | self.orient*(180/pi), id) 122 | 123 | def instantiate(self, tmat): 124 | dimensions = (self.max_Ex-self.min_Ex, self.max_Ey-self.min_Ey) 125 | rot = atan2(tmat[1,0], tmat[0,0]) 126 | return Rectangle(center = tmat.dot(self.center), 127 | orient = rot + self.orient, 128 | dimensions = dimensions) 129 | 130 | def collides_rect(self,other): 131 | # Test others edges in our reference frame 132 | o_verts = self.unrot.dot(other.vertices) 133 | o_min_x = min(o_verts[0,:]) 134 | o_max_x = max(o_verts[0,:]) 135 | o_min_y = min(o_verts[1,:]) 136 | o_max_y = max(o_verts[1,:]) 137 | if o_max_x <= self.min_Ex or self.max_Ex <= o_min_x or \ 138 | o_max_y <= self.min_Ey or self.max_Ey <= o_min_y: 139 | return False 140 | 141 | if self.orient == other.orient: return True 142 | 143 | # Test our edges in other's reference frame 144 | s_verts = other.unrot.dot(self.vertices) 145 | s_min_x = min(s_verts[0,:]) 146 | s_max_x = max(s_verts[0,:]) 147 | s_min_y = min(s_verts[1,:]) 148 | s_max_y = max(s_verts[1,:]) 149 | if s_max_x <= other.min_Ex or other.max_Ex <= s_min_x or \ 150 | s_max_y <= other.min_Ey or other.max_Ey <= s_min_y: 151 | return False 152 | return True 153 | 154 | def collides_circle(self,circle): 155 | p = self.unrot.dot(circle.center)[0:2,0] 156 | pmin = p - circle.radius 157 | pmax = p + circle.radius 158 | if pmax[0] <= self.min_Ex or self.max_Ex <= pmin[0] or \ 159 | pmax[1] <= self.min_Ey or self.max_Ey <= pmin[1]: 160 | return False 161 | # Need corner tests here 162 | return True 163 | 164 | #================ Compound Shapes ================ 165 | 166 | class Compound(Shape): 167 | def __init__(self, shapes=[]): 168 | self.shapes = shapes 169 | 170 | def collides(self,shape): 171 | for s in self.shapes: 172 | if s.collides(shape): 173 | return True 174 | return False 175 | 176 | -------------------------------------------------------------------------------- /cozmo_fsm/cam_viewer.py: -------------------------------------------------------------------------------- 1 | """ 2 | OpenGL based CamViewer 3 | """ 4 | 5 | import numpy as np 6 | import math 7 | import random 8 | import time 9 | import cozmo 10 | from cozmo.util import degrees, distance_mm, speed_mmps 11 | 12 | try: 13 | import cv2 14 | from PIL import Image 15 | from OpenGL.GLUT import * 16 | from OpenGL.GL import * 17 | from OpenGL.GLU import * 18 | except: 19 | pass 20 | 21 | from . import opengl 22 | from . import program 23 | 24 | #For capturing images 25 | global snapno, path 26 | snapno = 0 27 | path = 'snap/' 28 | 29 | WINDOW = None 30 | 31 | class CamViewer(): 32 | def __init__(self, robot, width=640, height=480, 33 | windowName="Cozmo's World", 34 | bgcolor=(0, 0, 0)): 35 | self.robot = robot 36 | self.width = width 37 | self.height = height 38 | self.aspect = self.width/self.height 39 | self.windowName = windowName 40 | self.bgcolor = bgcolor 41 | self.scale = 1 42 | self.show_axes = True 43 | self.show_memory_map = False 44 | 45 | def process_image(self): 46 | raw = self.robot.world.latest_image.raw_image 47 | curim = np.array(raw) 48 | gray = cv2.cvtColor(curim,cv2.COLOR_BGR2GRAY) 49 | 50 | running_fsm = program.running_fsm 51 | 52 | # Aruco image processing 53 | if running_fsm.aruco: 54 | running_fsm.robot.world.aruco.process_image(gray) 55 | # Other image processors can run here if the user supplies them. 56 | running_fsm.user_image(curim,gray) 57 | # Done with image processing 58 | 59 | # Annotate and display image if requested 60 | if running_fsm.force_annotation or running_fsm.cam_viewer is not None: 61 | scale = running_fsm.annotated_scale_factor 62 | # Apply Cozmo SDK annotations and rescale. 63 | if running_fsm.annotate_sdk: 64 | coz_ann = self.robot.world.latest_image.annotate_image(scale=scale) 65 | annotated_im = np.array(coz_ann) 66 | elif scale != 1: 67 | shape = curim.shape 68 | dsize = (scale*shape[1], scale*shape[0]) 69 | annotated_im = cv2.resize(curim, dsize) 70 | else: 71 | annotated_im = curim 72 | # Aruco annotation 73 | if running_fsm.aruco and \ 74 | len(running_fsm.robot.world.aruco.seen_marker_ids) > 0: 75 | annotated_im = running_fsm.robot.world.aruco.annotate(annotated_im,scale) 76 | # Other annotators can run here if the user supplies them. 77 | annotated_im = running_fsm.user_annotate(annotated_im) 78 | # Done with annotation 79 | # Yellow viewer crosshairs 80 | if running_fsm.viewer_crosshairs: 81 | shape = annotated_im.shape 82 | cv2.line(annotated_im, (int(shape[1]/2),0), (int(shape[1]/2),shape[0]), (0,255,255), 1) 83 | cv2.line(annotated_im, (0,int(shape[0]/2)), (shape[1],int(shape[0]/2)), (0,255,255), 1) 84 | image = annotated_im 85 | glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, self.width, self.height,0,GL_RGB, GL_UNSIGNED_BYTE, image) 86 | glutPostRedisplay() 87 | 88 | # ================ Window Setup ================ 89 | def window_creator(self): 90 | global WINDOW 91 | #glutInit(sys.argv) 92 | WINDOW = opengl.create_window( 93 | bytes(self.windowName, 'utf-8'), (self.width, self.height)) 94 | glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH) 95 | glutInitWindowSize(self.width, self.height) 96 | glutInitWindowPosition(100, 100) 97 | glClearColor(0.0, 0.0, 0.0, 1.0) 98 | glutDisplayFunc(self.display) 99 | glutReshapeFunc(self.reshape) 100 | glutKeyboardFunc(self.keyPressed) 101 | glutSpecialFunc(self.specialKeyPressed) 102 | glutSpecialUpFunc(self.specialKeyUp) 103 | 104 | def start(self): # Displays in background 105 | if not WINDOW: 106 | opengl.init() 107 | opengl.CREATION_QUEUE.append(self.window_creator) 108 | 109 | def display(self): 110 | self.process_image() 111 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) 112 | glEnable(GL_TEXTURE_2D) 113 | glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST) 114 | 115 | # Set Projection Matrix 116 | glMatrixMode(GL_PROJECTION) 117 | glLoadIdentity() 118 | gluOrtho2D(0, self.width, 0, self.height) 119 | 120 | glMatrixMode(GL_TEXTURE) 121 | glLoadIdentity() 122 | glScalef(1.0, -1.0, 1.0) 123 | glMatrixMode(GL_MODELVIEW) 124 | glBegin(GL_QUADS) 125 | glTexCoord2f(0.0, 0.0) 126 | glVertex2f(0.0, 0.0) 127 | glTexCoord2f(1.0, 0.0) 128 | glVertex2f(self.width, 0.0) 129 | glTexCoord2f(1.0, 1.0) 130 | glVertex2f(self.width, self.height) 131 | glTexCoord2f(0.0, 1.0) 132 | glVertex2f(0.0, self.height) 133 | glEnd() 134 | 135 | glFlush() 136 | glutSwapBuffers() 137 | 138 | def reshape(self, w, h): 139 | if h == 0: 140 | h = 1 141 | 142 | glViewport(0, 0, w, h) 143 | glMatrixMode(GL_PROJECTION) 144 | 145 | glLoadIdentity() 146 | nRange = 1.0 147 | if w <= h: 148 | glOrtho(-nRange, nRange, -nRange*h/w, nRange*h/w, -nRange, nRange) 149 | else: 150 | glOrtho(-nRange*w/h, nRange*w/h, -nRange, nRange, -nRange, nRange) 151 | 152 | glMatrixMode(GL_MODELVIEW) 153 | glLoadIdentity() 154 | 155 | def keyPressed(self, key, x, y): 156 | if ord(key) == 27: 157 | print("Use 'exit' to quit.") 158 | #return 159 | if key == b'c': 160 | print("Taking a snap") 161 | self.capture() 162 | self.display() 163 | 164 | def specialKeyPressed(self, key, x, y): 165 | global leftorrightindicate, globthres 166 | if key == GLUT_KEY_LEFT: 167 | self.robot.drive_wheels(-100, 100) 168 | leftorrightindicate = True 169 | globthres=100 170 | elif key == GLUT_KEY_RIGHT: 171 | self.robot.drive_wheels(100, -100) 172 | leftorrightindicate = True 173 | globthres = 100 174 | elif key == GLUT_KEY_UP: 175 | self.robot.drive_wheels(200, 200) 176 | leftorrightindicate = False 177 | globthres = 100 178 | elif key == GLUT_KEY_DOWN: 179 | self.robot.drive_wheels(-200, -200) 180 | leftorrightindicate = True 181 | globthres = 100 182 | glutPostRedisplay() 183 | 184 | def specialKeyUp(self, key, x, y): 185 | global leftorrightindicate, go_forward 186 | self.robot.drive_wheels(0, 0) 187 | leftorrightindicate = True 188 | go_forward = GLUT_KEY_UP 189 | glutPostRedisplay() 190 | 191 | def capture(self, name='cozmo_snap'): 192 | global snapno, path 193 | if not os.path.exists(path): 194 | os.makedirs(path) 195 | 196 | image = np.array(self.robot.world.latest_image.raw_image) 197 | Image.fromarray(image).save(path + '/' + name + str(snapno) + '.jpg') 198 | snapno +=1 199 | -------------------------------------------------------------------------------- /cozmo_fsm/examples/Boo.py: -------------------------------------------------------------------------------- 1 | """ 2 | Peek-A-Boo game inspired by the Boo game of Pablo Barros. 3 | 4 | This version is coded using the cozmo_fsm package and illustrates 5 | features such as repetitive polling, nested state machines, and a 6 | completion transition that uses one completing source node to 7 | terminate a second source (MoveHead) that doesn't complete. 8 | """ 9 | 10 | from cozmo_fsm import * 11 | 12 | class WaitForPlayer(StateMachineProgram): 13 | """Wait for player's face to appear and remain visible for a little while.""" 14 | def start(self,event=None): 15 | self.set_polling_interval(0.2) 16 | self.faces_found = 0 # initialize before polling starts 17 | super().start(event) 18 | 19 | def poll(self): 20 | if self.robot.world.visible_face_count() == 0: return 21 | self.faces_found += 1 22 | if self.faces_found > 3: 23 | self.post_completion() 24 | 25 | class WaitForHide(StateNode): 26 | """Wait for player's face to disappear and remain not visible for a little while.""" 27 | def start(self,event=None): 28 | self.set_polling_interval(0.2) 29 | self.faces_not_found = 0 # initialize before polling starts 30 | super().start(event) 31 | 32 | def poll(self): 33 | if self.robot.world.visible_face_count() > 0: return 34 | self.faces_not_found += 1 35 | if self.faces_not_found > 2: 36 | self.post_completion() 37 | 38 | class HeadAndLiftGesture(StateNode): 39 | """Move head and lift simultaneously. Finish when head movement completes.""" 40 | def setup(self): 41 | """ 42 | launch: StateNode() =N=> {move_head, move_lift} 43 | 44 | move_head: SetHeadAngle(cozmo.robot.MAX_HEAD_ANGLE) 45 | move_lift: MoveLift(-3) 46 | 47 | {move_head, move_lift} =C(1)=> ParentCompletes() 48 | """ 49 | 50 | # Code generated by genfsm on Mon Feb 17 03:10:20 2020: 51 | 52 | launch = StateNode() .set_name("launch") .set_parent(self) 53 | move_head = SetHeadAngle(cozmo.robot.MAX_HEAD_ANGLE) .set_name("move_head") .set_parent(self) 54 | move_lift = MoveLift(-3) .set_name("move_lift") .set_parent(self) 55 | parentcompletes1 = ParentCompletes() .set_name("parentcompletes1") .set_parent(self) 56 | 57 | nulltrans1 = NullTrans() .set_name("nulltrans1") 58 | nulltrans1 .add_sources(launch) .add_destinations(move_head,move_lift) 59 | 60 | completiontrans1 = CompletionTrans(1) .set_name("completiontrans1") 61 | completiontrans1 .add_sources(move_head,move_lift) .add_destinations(parentcompletes1) 62 | 63 | return self 64 | 65 | class Boo(StateNode): 66 | def setup(self): 67 | """ 68 | launch: Say("Let's play") 69 | =C=> SetHeadAngle(30) 70 | =C=> player_appears 71 | 72 | player_appears: WaitForPlayer() 73 | =C=> AnimationNode('anim_freeplay_reacttoface_identified_01_head_angle_40') 74 | =C=> SetHeadAngle(cozmo.robot.MIN_HEAD_ANGLE) 75 | =C=> SetHeadAngle(cozmo.robot.MAX_HEAD_ANGLE) 76 | =C=> player_hides 77 | 78 | player_hides: WaitForHide() 79 | =C=> AnimationNode('anim_hiking_observe_01') 80 | =C=> HeadAndLiftGesture() 81 | =C=> player_reappears 82 | 83 | player_reappears: WaitForPlayer() 84 | =C=> AnimationNode('anim_freeplay_reacttoface_like_01') 85 | =C=> HeadAndLiftGesture() 86 | =C=> Say("play again") 87 | =C=> SetHeadAngle(30) 88 | =C=> player_hides 89 | """ 90 | 91 | # Code generated by genfsm on Mon Feb 17 03:10:20 2020: 92 | 93 | launch = Say("Let's play") .set_name("launch") .set_parent(self) 94 | setheadangle1 = SetHeadAngle(30) .set_name("setheadangle1") .set_parent(self) 95 | player_appears = WaitForPlayer() .set_name("player_appears") .set_parent(self) 96 | animationnode1 = AnimationNode('anim_freeplay_reacttoface_identified_01_head_angle_40') .set_name("animationnode1") .set_parent(self) 97 | setheadangle2 = SetHeadAngle(cozmo.robot.MIN_HEAD_ANGLE) .set_name("setheadangle2") .set_parent(self) 98 | setheadangle3 = SetHeadAngle(cozmo.robot.MAX_HEAD_ANGLE) .set_name("setheadangle3") .set_parent(self) 99 | player_hides = WaitForHide() .set_name("player_hides") .set_parent(self) 100 | animationnode2 = AnimationNode('anim_hiking_observe_01') .set_name("animationnode2") .set_parent(self) 101 | headandliftgesture1 = HeadAndLiftGesture() .set_name("headandliftgesture1") .set_parent(self) 102 | player_reappears = WaitForPlayer() .set_name("player_reappears") .set_parent(self) 103 | animationnode3 = AnimationNode('anim_freeplay_reacttoface_like_01') .set_name("animationnode3") .set_parent(self) 104 | headandliftgesture2 = HeadAndLiftGesture() .set_name("headandliftgesture2") .set_parent(self) 105 | say1 = Say("play again") .set_name("say1") .set_parent(self) 106 | setheadangle4 = SetHeadAngle(30) .set_name("setheadangle4") .set_parent(self) 107 | 108 | completiontrans2 = CompletionTrans() .set_name("completiontrans2") 109 | completiontrans2 .add_sources(launch) .add_destinations(setheadangle1) 110 | 111 | completiontrans3 = CompletionTrans() .set_name("completiontrans3") 112 | completiontrans3 .add_sources(setheadangle1) .add_destinations(player_appears) 113 | 114 | completiontrans4 = CompletionTrans() .set_name("completiontrans4") 115 | completiontrans4 .add_sources(player_appears) .add_destinations(animationnode1) 116 | 117 | completiontrans5 = CompletionTrans() .set_name("completiontrans5") 118 | completiontrans5 .add_sources(animationnode1) .add_destinations(setheadangle2) 119 | 120 | completiontrans6 = CompletionTrans() .set_name("completiontrans6") 121 | completiontrans6 .add_sources(setheadangle2) .add_destinations(setheadangle3) 122 | 123 | completiontrans7 = CompletionTrans() .set_name("completiontrans7") 124 | completiontrans7 .add_sources(setheadangle3) .add_destinations(player_hides) 125 | 126 | completiontrans8 = CompletionTrans() .set_name("completiontrans8") 127 | completiontrans8 .add_sources(player_hides) .add_destinations(animationnode2) 128 | 129 | completiontrans9 = CompletionTrans() .set_name("completiontrans9") 130 | completiontrans9 .add_sources(animationnode2) .add_destinations(headandliftgesture1) 131 | 132 | completiontrans10 = CompletionTrans() .set_name("completiontrans10") 133 | completiontrans10 .add_sources(headandliftgesture1) .add_destinations(player_reappears) 134 | 135 | completiontrans11 = CompletionTrans() .set_name("completiontrans11") 136 | completiontrans11 .add_sources(player_reappears) .add_destinations(animationnode3) 137 | 138 | completiontrans12 = CompletionTrans() .set_name("completiontrans12") 139 | completiontrans12 .add_sources(animationnode3) .add_destinations(headandliftgesture2) 140 | 141 | completiontrans13 = CompletionTrans() .set_name("completiontrans13") 142 | completiontrans13 .add_sources(headandliftgesture2) .add_destinations(say1) 143 | 144 | completiontrans14 = CompletionTrans() .set_name("completiontrans14") 145 | completiontrans14 .add_sources(say1) .add_destinations(setheadangle4) 146 | 147 | completiontrans15 = CompletionTrans() .set_name("completiontrans15") 148 | completiontrans15 .add_sources(setheadangle4) .add_destinations(player_hides) 149 | 150 | return self 151 | -------------------------------------------------------------------------------- /cozmo_fsm/evbase.py: -------------------------------------------------------------------------------- 1 | """ 2 | The Event Router and Event Listener 3 | 4 | This file implements an event router scheme modeled after the 5 | one in Tekkotsu. 6 | 7 | """ 8 | 9 | import functools 10 | from multiprocessing import Queue 11 | 12 | import cozmo 13 | 14 | from .trace import TRACE 15 | 16 | #________________ Event base class ________________ 17 | 18 | class Event: 19 | """Base class for all events.""" 20 | def __init__(self, source=None): 21 | self.source = source 22 | 23 | cozmo_evt_type = None 24 | 25 | def generator(self,erouter,cozmo_evt): pass 26 | 27 | def __repr__(self): 28 | try: 29 | src_string = self.source.name 30 | except: 31 | src_string = repr(self.source) 32 | return '<%s from %s>' % (self.__class__.__name__, src_string) 33 | 34 | #________________ Event Router ________________ 35 | 36 | class EventRouter: 37 | """An event router drives the state machine.""" 38 | def __init__(self): 39 | # dispatch_table: event_class -> (source,listener)... 40 | self.dispatch_table = dict() 41 | # listener_registry: listener -> (event_class, source)... 42 | self.listener_registry = dict() 43 | # wildcard registry: true if listener is a wildcard (should run last) 44 | self.wildcard_registry = dict() 45 | # event generator objects 46 | self.event_generators = dict() 47 | # running processes 48 | self.processes = dict() # id -> node 49 | self.interprocess_queue = Queue() 50 | 51 | def start(self): 52 | self.clear() 53 | self.poll_processes() 54 | 55 | def clear(self): 56 | self.dispatch_table.clear() 57 | self.listener_registry.clear() 58 | self.wildcard_registry.clear() 59 | self.event_generators.clear() 60 | self.processes.clear() 61 | self.interprocess_queue.close() 62 | self.interprocess_queue = Queue() 63 | 64 | def add_listener(self, listener, event_class, source): 65 | if not issubclass(event_class, Event): 66 | raise TypeError('% is not an Event' % event_type) 67 | source_dict = self.dispatch_table.get(event_class) 68 | if source_dict is None: 69 | source_dict = dict() 70 | # start a cozmo event handler if this event type requires one 71 | if event_class.cozmo_evt_type: 72 | coztype = event_class.cozmo_evt_type 73 | if not issubclass(coztype, cozmo.event.Event): 74 | raise ValueError('%s cozmo_evt_type %s not a subclass of cozmo.event.Event' % (event_type, coztype)) 75 | world = self.robot.world 76 | # supply the erouter and event type 77 | gen = functools.partial(event_class.generator, self, event_class) 78 | self.event_generators[event_class] = gen 79 | world.add_event_handler(coztype,gen) 80 | handlers = source_dict.get(source, []) 81 | handlers.append(listener.handle_event) 82 | source_dict[source] = handlers 83 | self.dispatch_table[event_class] = source_dict 84 | reg_entry = self.listener_registry.get(listener,[]) 85 | reg_entry.append((event_class,source)) 86 | self.listener_registry[listener] = reg_entry 87 | 88 | # Transitions like =Hear('foo')=> must use None as the source 89 | # value because they do the matching themselves instead of relying 90 | # on the event router. So to distinguish a wildcard =Hear=> 91 | # transition, which must be invoked last, from all the other Hear 92 | # transitions, we must register it specially. 93 | def add_wildcard_listener(self, listener, event_class, source): 94 | self.add_listener(listener, event_class, source) 95 | self.wildcard_registry[listener.handle_event] = True 96 | 97 | def remove_listener(self, listener, event_class, source): 98 | try: 99 | del self.wildcard_registry[listener.handle_event] 100 | except: pass 101 | if not issubclass(event_class, Event): 102 | raise TypeError('% is not an Event' % event_class) 103 | source_dict = self.dispatch_table.get(event_class) 104 | if source_dict is None: return 105 | handlers = source_dict.get(source) 106 | if handlers is None: return 107 | try: 108 | handlers.remove(listener.handle_event) 109 | except: pass 110 | if handlers == []: 111 | del source_dict[source] 112 | if len(source_dict) == 0: # no one listening for this event 113 | del self.dispatch_table[event_class] 114 | # remove the cozmo SDK event handler if there was one 115 | if event_class.cozmo_evt_type: 116 | coztype = event_class.cozmo_evt_type 117 | world = self.robot.world 118 | gen = self.event_generators[event_class] 119 | world.remove_event_handler(coztype, gen) 120 | del self.event_generators[event_class] 121 | 122 | def remove_all_listener_entries(self, listener): 123 | for event_class, source in self.listener_registry.get(listener,[]): 124 | self.remove_listener(listener, event_class, source) 125 | try: 126 | del self.listener_registry[listener] 127 | except: pass 128 | 129 | def _get_listeners(self,event): 130 | source_dict = self.dispatch_table.get(type(event), None) 131 | if source_dict is None: # no listeners for this event type 132 | return [] 133 | source_matches = source_dict.get(event.source, []) 134 | match_handlers = [] 135 | # TapEvent can be wildcarded even though the source is never None 136 | wildcard_matches = source_dict.get(None, []) if event.source is not None else [] 137 | wildcard_handlers = [] 138 | for handler in source_matches + wildcard_matches: 139 | if self.wildcard_registry.get(handler,False) is True: 140 | wildcard_handlers.append(handler) 141 | else: 142 | match_handlers.append(handler) 143 | # wildcard handlers must come last in the list 144 | return match_handlers + wildcard_handlers 145 | 146 | def post(self,event): 147 | if not isinstance(event,Event): 148 | raise TypeError('%s is not an Event' % event) 149 | listeners = self._get_listeners(event) 150 | cnt = 0 151 | for listener in listeners: 152 | cnt += 1 153 | if TRACE.trace_level >= TRACE.listener_invocation: 154 | print('TRACE%d:' % TRACE.listener_invocation, listener.__self__, 'receiving', event) 155 | self.robot.loop.call_soon(listener,event) 156 | 157 | def add_process_node(self, node): 158 | self.processes[id(node)] = node 159 | 160 | def delete_process_node(self, node): 161 | node_id = id(node) 162 | if node_id in self.processes: 163 | del self.processes[node_id] 164 | # print('Deleted id',node_id,'for',node) 165 | else: 166 | print('*** ERROR in delete_process_node: node',node_id,'not in process dict!') 167 | 168 | POLLING_INTERVAL = 0.1 169 | 170 | def poll_processes(self): 171 | while not self.interprocess_queue.empty(): 172 | (id,event) = self.interprocess_queue.get() 173 | if id in self.processes: 174 | node = self.processes[id] 175 | event.source = node 176 | print('Node %s returned %s' % (node,event)) 177 | self.post(event) 178 | else: 179 | print('*** ERROR in poll_processes: node',id,'not in process dict!', self.processes) 180 | self.robot.loop.call_later(self.POLLING_INTERVAL, self.poll_processes) 181 | 182 | #________________ Event Listener ________________ 183 | 184 | class EventListener: 185 | """Parent class for both StateNode and Transition.""" 186 | def __init__(self): 187 | rep = object.__repr__(self) 188 | self.name = rep[1+rep.rfind(' '):-1] # name defaults to hex address 189 | self.running = False 190 | if not hasattr(self,'polling_interval'): 191 | self.polling_interval = None 192 | self.poll_handle = None 193 | self._robot = robot_for_loading 194 | 195 | def __repr__(self): 196 | return '<%s %s>' % (self.__class__.__name__, self.name) 197 | 198 | def set_name(self,name): 199 | if not isinstance(name,str): 200 | raise ValueError('name must be a string, not %s' % name) 201 | self.name = name 202 | return self 203 | 204 | def start(self): 205 | self.running = True 206 | if self.polling_interval: 207 | self.poll_handle = \ 208 | self.robot.loop.call_later(self.polling_interval, self._next_poll) 209 | else: 210 | self.poll_handle = None 211 | 212 | def stop(self): 213 | if not self.running: return 214 | self.running = False 215 | if self.poll_handle: self.poll_handle.cancel() 216 | self.robot.erouter.remove_all_listener_entries(self) 217 | 218 | def handle_event(self, event): 219 | pass 220 | 221 | def set_polling_interval(self,interval): 222 | if isinstance(interval, (int,float)): 223 | self.polling_interval = interval 224 | else: 225 | raise TypeError('interval must be a number') 226 | 227 | def _next_poll(self): 228 | """Called to schedule the next polling interval and then poll the node.""" 229 | # Schedule the next poll first because self.poll may cancel it. 230 | if self.running and self.polling_interval: 231 | self.poll_handle = \ 232 | self.robot.loop.call_later(self.polling_interval, self._next_poll) 233 | self.poll() 234 | 235 | def poll(self): 236 | """Dummy polling function in case sublass neglects to supply one.""" 237 | if TRACE.trace_level >= TRACE.polling: 238 | print('TRACE%d: polling' % TRACE.polling, self) 239 | print('%s has no poll() method' % self) 240 | -------------------------------------------------------------------------------- /cozmo_fsm/doorpass.fsm: -------------------------------------------------------------------------------- 1 | from cozmo.util import Pose 2 | from numpy import matrix, tan, arctan2 3 | from math import sin, cos, atan2, pi, sqrt 4 | 5 | try: from cv2 import Rodrigues 6 | except: pass 7 | 8 | from .nodes import * 9 | from .transitions import * 10 | from .geometry import wrap_angle 11 | from .pilot0 import * 12 | from .worldmap import WallObj, DoorwayObj 13 | from time import sleep 14 | 15 | class DoorPass(StateNode): 16 | """Pass through a doorway. Assumes the doorway is nearby and unobstructed.""" 17 | 18 | OUTER_GATE_DISTANCE = 150 # mm 19 | INNER_GATE_DISTANCE = 70 # mm 20 | 21 | def __init__(self, door=None): 22 | self.door = door 23 | super().__init__() 24 | 25 | def start(self, event=None): 26 | door = self.door 27 | if isinstance(event,DataEvent): 28 | door = event.data 29 | if isinstance(door, int): 30 | door ='Doorway-%d' % door 31 | if isinstance(door, str): 32 | doorway = self.robot.world.world_map.objects.get(door) 33 | elif isinstance(door, DoorwayObj): 34 | doorway = door 35 | else: 36 | doorway = None 37 | if isinstance(doorway, DoorwayObj): 38 | self.object = doorway 39 | else: 40 | print("Error in DoorPass: no doorway named %s" % repr(door)) 41 | raise ValueError(door,doorway) 42 | super().start(event) 43 | 44 | 45 | @staticmethod 46 | def calculate_gate(start_point, door, offset): 47 | """Returns closest gate point (gx, gy)""" 48 | (rx,ry) = start_point 49 | dx = door.x 50 | dy = door.y 51 | dtheta = door.theta 52 | pt1x = dx + offset * cos(dtheta) 53 | pt1y = dy + offset * sin(dtheta) 54 | pt2x = dx + offset * cos(dtheta+pi) 55 | pt2y = dy + offset * sin(dtheta+pi) 56 | dist1sq = (pt1x-rx)**2 + (pt1y-ry)**2 57 | dist2sq = (pt2x-rx)**2 + (pt2y-ry)**2 58 | if dist1sq < dist2sq: 59 | return (pt1x, pt1y, wrap_angle(dtheta+pi)) 60 | else: 61 | return (pt2x, pt2y, dtheta) 62 | 63 | 64 | class AdjustLiftHeight(SetLiftHeight): 65 | # TODO: If lift is high, push it higher (we're carrying something). 66 | # If lift isn't high, drop it to zero 67 | def start(self, event=None): 68 | self.height = 0 69 | super().start(event) 70 | 71 | 72 | class AwayFromCollide(Forward): 73 | def start(self, event=None): 74 | # super().start(event) 75 | if isinstance(event,DataEvent): 76 | startNode = event.data[0] 77 | collideObj = event.data[1] 78 | (rx, ry, rtheta) = self.robot.world.particle_filter.pose_estimate() 79 | cx, cy = collideObj.center[0,0],collideObj.center[1,0] 80 | ctheta = atan2(cy-ry, cx-rx) 81 | delta_angle = wrap_angle(ctheta - rtheta) 82 | delta_angle = delta_angle/pi*180 83 | if -90 < delta_angle and delta_angle < 90: 84 | self.distance = distance_mm(-40) 85 | else: 86 | self.distance = distance_mm(40) 87 | self.speed = speed_mmps(50) 88 | super().start(event) 89 | else: 90 | raise ValueError('DataEvent to AwayFromCollide must be a StartCollides.args', event.data) 91 | self.post_failure() 92 | 93 | 94 | class TurnToGate(Turn): 95 | """Turn to the approach gate, or post success if we're already there.""" 96 | def __init__(self,offset): 97 | self.offset = offset 98 | super().__init__(speed=Angle(radians=2.0)) 99 | 100 | def start(self,event=None): 101 | (rx, ry, rtheta) = self.robot.world.particle_filter.pose_estimate() 102 | (gate_x, gate_y, _) = DoorPass.calculate_gate((rx,ry), self.parent.object, self.offset) 103 | bearing = atan2(gate_y-ry, gate_x-rx) 104 | turn = wrap_angle(bearing - rtheta) 105 | print('^^ TurnToGate: gate=(%.1f, %.1f) offset=%.1f rtheta=%.1f bearing=%.1f turn=%.1f' % 106 | (gate_x, gate_y, self.offset, rtheta*180/pi, bearing*180/pi, turn*180/pi)) 107 | if False and abs(turn) < 0.1: 108 | self.angle = Angle(0) 109 | super().start(event) 110 | self.post_success() 111 | else: 112 | self.angle = Angle(radians=turn) 113 | super().start(event) 114 | 115 | 116 | class ForwardToGate(Forward): 117 | """Travel forward to reach the approach gate.""" 118 | def __init__(self,offset): 119 | self.offset = offset 120 | super().__init__() 121 | 122 | def start(self,event=None): 123 | (rx, ry, rtheta) = self.robot.world.particle_filter.pose_estimate() 124 | (gate_x, gate_y, _) = DoorPass.calculate_gate((rx,ry), self.parent.object, self.offset) 125 | dist = sqrt((gate_x-rx)**2 + (gate_y-ry)**2) 126 | self.distance = distance_mm(dist) 127 | self.speed = speed_mmps(50) 128 | super().start(event) 129 | 130 | class TurnToMarker(Turn): 131 | """Use camera image and native pose to center the door marker.""" 132 | def start(self,event=None): 133 | marker_ids = self.parent.object.marker_ids 134 | marker = self.robot.world.aruco.seen_marker_objects.get(marker_ids[0], None) or \ 135 | self.robot.world.aruco.seen_marker_objects.get(marker_ids[1], None) 136 | if not marker: 137 | self.angle = Angle(0) 138 | super().start(event) 139 | print("TurnToMarker failed to find marker %s or %s!" % marker_ids) 140 | self.post_failure() 141 | return 142 | else: 143 | print('TurnToMarker saw marker', marker) 144 | sensor_dist = marker.camera_distance 145 | sensor_bearing = atan2(marker.camera_coords[0], 146 | marker.camera_coords[2]) 147 | x = self.robot.pose.position.x 148 | y = self.robot.pose.position.y 149 | theta = self.robot.pose.rotation.angle_z.radians 150 | direction = theta + sensor_bearing 151 | dx = sensor_dist * cos(direction) 152 | dy = sensor_dist * sin(direction) 153 | turn = wrap_angle(atan2(dy,dx) - self.robot.pose.rotation.angle_z.radians) 154 | if abs(turn) < 0.5*pi/180: 155 | self.angle = Angle(0) 156 | else: 157 | self.angle = Angle(radians=turn) 158 | print("TurnToMarker %s turning by %.1f degrees" % (self.name, self.angle.degrees)) 159 | super().start(event) 160 | 161 | class CheckCarrying(SetLiftHeight): 162 | def __init__(self): 163 | super().__init__() 164 | 165 | def start(self,event=None): 166 | if self.robot.carrying: 167 | self.height = 0.4 if self.robot.lift_ratio > 0.5 else 1 168 | super().start(event) 169 | 170 | class DriveThrough(Forward): 171 | """Travel forward to drive through the gate.""" 172 | def __init__(self): 173 | super().__init__() 174 | 175 | def start(self,event=None): 176 | (rx, ry, rtheta) = self.robot.world.particle_filter.pose_estimate() 177 | (gate_x, gate_y, gate_theta) = DoorPass.calculate_gate((rx,ry), self.parent.object, 5) 178 | dist = sqrt((gate_x-rx)**2 + (gate_y-ry)**2) 179 | offset = 120 180 | delta_theta = wrap_angle(rtheta-(gate_theta+pi/2)) 181 | delta_dist = abs(offset/sin(delta_theta)) 182 | dist += delta_dist 183 | self.distance = distance_mm(dist) 184 | self.speed = speed_mmps(50) 185 | super().start(event) 186 | 187 | $setup{ 188 | droplift: self.AdjustLiftHeight() =N=> 189 | SetHeadAngle(0) =T(0.2)=> check_start # Time for vision to process 190 | 191 | check_start: PilotCheckStartDetail() 192 | check_start =S=> turn_to_gate1 193 | check_start =D=> away_from_collide 194 | check_start =F=> Forward(-80) =C=> check_start2 195 | 196 | check_start2: PilotCheckStartDetail() 197 | check_start2 =S=> turn_to_gate1 198 | check_start2 =D=> away_from_collide2 199 | check_start2 =F=> ParentFails() 200 | 201 | check_start3: PilotCheckStart() 202 | check_start3 =S=> turn_to_gate1 203 | check_start3 =F=> ParentFails() 204 | 205 | turn_to_gate1: self.TurnToGate(DoorPass.OUTER_GATE_DISTANCE) =C=> 206 | StateNode() =T(0.2)=> forward_to_gate1 207 | 208 | away_from_collide: self.AwayFromCollide() =C=> StateNode() =T(0.2)=> check_start2 209 | away_from_collide =F=> check_start2 210 | 211 | away_from_collide2: self.AwayFromCollide() =C=> StateNode() =T(0.2)=> check_start3 212 | away_from_collide2 =F=> check_start3 213 | 214 | forward_to_gate1: self.ForwardToGate(DoorPass.OUTER_GATE_DISTANCE) =C=> 215 | StateNode() =T(0.2)=> {look_up, turn_to_gate2} 216 | 217 | # If we're carrying a cube, we lower the lift so we can see 218 | look_up: SetHeadAngle(35) 219 | 220 | turn_to_gate2: self.TurnToGate(DoorPass.INNER_GATE_DISTANCE) =C=> 221 | StateNode() =T(0.2)=> self.CheckCarrying() =C=> turn_to_marker1 222 | 223 | turn_to_marker1: self.TurnToMarker() 224 | turn_to_marker1 =C=> marker_forward1 225 | turn_to_marker1 =F=> marker_forward1 226 | 227 | marker_forward1: self.ForwardToGate(DoorPass.INNER_GATE_DISTANCE) =C=> 228 | SetHeadAngle(40) =C=> StateNode() =T(0.2)=> turn_to_marker2 229 | 230 | turn_to_marker2: self.TurnToMarker() 231 | turn_to_marker2 =C=> marker_forward2 232 | turn_to_marker2 =F=> marker_forward2 233 | 234 | marker_forward2: StateNode() =T(0.2)=> {lower_head, through_door} 235 | 236 | lower_head: SetHeadAngle(0) 237 | 238 | through_door: self.DriveThrough() 239 | 240 | {lower_head, through_door} =C=> self.CheckCarrying() =C=> ParentCompletes() 241 | 242 | } 243 | -------------------------------------------------------------------------------- /cozmo_fsm/base.py: -------------------------------------------------------------------------------- 1 | """ 2 | Base classes StateNode for nodes.py and Transition for 3 | transitions.py are placed here due to circular dependencies. 4 | Their parent class EventListener is imported from evbase.py. 5 | 6 | """ 7 | 8 | import cozmo 9 | 10 | from .trace import TRACE 11 | from .evbase import Event, EventListener 12 | from .events import CompletionEvent, SuccessEvent, FailureEvent, DataEvent 13 | 14 | class StateNode(EventListener): 15 | """Base class for state nodes; does nothing.""" 16 | def __init__(self): 17 | super().__init__() 18 | self.parent = None 19 | self.children = {} 20 | self.transitions = [] 21 | self.start_node = None 22 | self.setup() 23 | self.setup2() 24 | 25 | # Cache 'robot' in the instance because we could have two state 26 | # machine instances controlling different robots. 27 | @property 28 | def robot(self): 29 | return self._robot 30 | 31 | def setup(self): 32 | """Redefine this to set up a child state machine.""" 33 | pass 34 | 35 | def setup2(self): 36 | """Redefine this if post-setup processing is required.""" 37 | pass 38 | 39 | def start(self,event=None): 40 | if self.running: return 41 | if TRACE.trace_level >= TRACE.statenode_start: 42 | print('TRACE%d:' % TRACE.statenode_start, self, 'starting') 43 | super().start() 44 | # Start transitions before children, because children 45 | # may post an event that we're listening for (such as completion). 46 | for t in self.transitions: 47 | t.start() 48 | if self.start_node: 49 | if TRACE.trace_level >= TRACE.statenode_start: 50 | print('TRACE%d:' % TRACE.statenode_start, self, 'starting child', self.start_node) 51 | self.start_node.start() 52 | 53 | def stop(self): 54 | # If this node was stopped by an outgoing transition firing, 55 | # and then its parent tries to stop it, we need to cancel the 56 | # pending fire2 call. 57 | if self.running: 58 | if TRACE.trace_level >= TRACE.statenode_startstop: 59 | print('TRACE%d:' % TRACE.statenode_startstop, self, 'stopping') 60 | super().stop() 61 | self.stop_children() 62 | # Stop transitions even if we're not running, because a firing 63 | # transition could have stopped us and left a fire2 pending. 64 | for t in self.transitions: 65 | t.stop() 66 | 67 | def stop_children(self): 68 | if self.children == {}: 69 | return 70 | if TRACE.trace_level >= TRACE.statenode_startstop: 71 | print('TRACE%d:' % TRACE.statenode_startstop, self, 'is stopping its children') 72 | for child in self.children.values(): 73 | if child.running: 74 | child.stop() 75 | 76 | def add_transition(self, trans): 77 | if not isinstance(trans, Transition): 78 | raise TypeError('%s is not a Transition' % trans) 79 | self.transitions.append(trans) 80 | 81 | def set_parent(self, parent): 82 | if not isinstance(parent, StateNode): 83 | raise TypeError('%s is not a StateNode' % parent) 84 | try: 85 | if isinstance(self.parent, StateNode): 86 | raise Exception('parent already set') 87 | except AttributeError: 88 | raise Exception("It appears %s's __init__ method did not call super().__init__" 89 | % self.__class__.__name__) 90 | self.parent = parent 91 | parent.children[self.name] = self 92 | # First-declared child is the default start node. 93 | if not parent.start_node: 94 | parent.start_node = self 95 | return self 96 | 97 | def post_event(self, event, suppress_trace=False): 98 | if not isinstance(event,Event): 99 | raise ValuError('post_event given a non-Event argument:',event) 100 | if event.source is None: 101 | event.source = self 102 | if (not suppress_trace) and (TRACE.trace_level >= TRACE.event_posted): 103 | print('TRACE%d:' % TRACE.event_posted, self, 'posting event',event) 104 | if not self.running: 105 | print("*** ERROR: Node", self, "posted event", event,"before calling super().start(). ***") 106 | self.robot.erouter.post(event) 107 | 108 | def post_completion(self): 109 | if TRACE.trace_level > TRACE.statenode_startstop: 110 | print('TRACE%d:' % TRACE.statenode_startstop, self, 'posting completion') 111 | event = CompletionEvent() 112 | event.source = self 113 | self.post_event(event, suppress_trace=True) 114 | 115 | def post_success(self,details=None): 116 | if TRACE.trace_level > TRACE.statenode_startstop: 117 | print('TRACE%d:' % TRACE.statenode_startstop, 118 | self, 'posting success, details=%s' % details) 119 | event = SuccessEvent(details) 120 | event.source = self 121 | self.post_event(event, suppress_trace=True) 122 | 123 | def post_failure(self,details=None): 124 | if TRACE.trace_level > TRACE.statenode_startstop: 125 | print('TRACE%d:' % TRACE.statenode_startstop, 126 | self, 'posting failure, details=%s' % details) 127 | event = FailureEvent(details) 128 | event.source = self 129 | self.post_event(event, suppress_trace=True) 130 | 131 | def post_data(self,value): 132 | if TRACE.trace_level > TRACE.statenode_startstop: 133 | print('TRACE%d:' % TRACE.statenode_startstop, 134 | self, 'posting data', value) 135 | event = DataEvent(value) 136 | event.source = self 137 | self.post_event(event, suppress_trace=True) 138 | 139 | def now(self): 140 | """Use now() to execute this node from the command line instead of as part of a state machine.""" 141 | if not self.robot: 142 | raise ValueError('Node %s has no robot designated.' % self) 143 | # 'program' is inserted into this module by __init__ to avoid circular importing 144 | program.running_fsm.children = dict() 145 | program.running_fsm.children[self.name] = self 146 | self.robot.loop.call_soon(self.start) 147 | return self 148 | 149 | def print_trace_message(self, msg1='', msg2=''): 150 | print('<><><> %s' % msg1, self, end='') 151 | p = self.parent 152 | while p is not None: 153 | print(' of', p.name, end='') 154 | p = p.parent 155 | print(' ', msg2) 156 | 157 | class Transition(EventListener): 158 | """Base class for transitions: does nothing.""" 159 | def __init__(self): 160 | super().__init__() 161 | self.sources = [] 162 | self.destinations = [] 163 | self.handle = None 164 | 165 | def __repr__(self): 166 | srcs = ','.join(node.name for node in self.sources) 167 | dests = ','.join(node.name for node in self.destinations) 168 | return '<%s %s: %s=>%s >' % \ 169 | (self.__class__.__name__, self.name, srcs, dests) 170 | 171 | @property 172 | def robot(self): 173 | return self._robot 174 | 175 | def _sibling_check(self,node): 176 | for sibling in self.sources + self.destinations: 177 | if sibling.parent is not node.parent: 178 | raise ValueError("All source/destination nodes must have the same parent.") 179 | 180 | def add_sources(self, *nodes): 181 | for node in nodes: 182 | if not isinstance(node, StateNode): 183 | raise TypeError('%s is not a StateNode' % node) 184 | self._sibling_check(node) 185 | node.add_transition(self) 186 | self.sources.append(node) 187 | return self 188 | 189 | def add_destinations(self, *nodes): 190 | for node in nodes: 191 | if not isinstance(node, StateNode): 192 | raise TypeError('%s is not a StateNode' % node) 193 | self._sibling_check(node) 194 | self.destinations.append(node) 195 | return self 196 | 197 | def start(self): 198 | if self.running: return 199 | self.handle = None 200 | if TRACE.trace_level >= TRACE.transition_startstop: 201 | print('TRACE%d:' % TRACE.transition_startstop, self, 'starting') 202 | super().start() 203 | 204 | def stop(self): 205 | if self.running: 206 | # don't stop if we still have a live source 207 | for src in self.sources: 208 | if src.running: 209 | if TRACE.trace_level >= TRACE.transition_startstop: 210 | print('TRACE%d:' % TRACE.transition_startstop,self,'saved from stopping by',src) 211 | return 212 | if TRACE.trace_level >= TRACE.transition_startstop: 213 | print('TRACE%d:' % TRACE.transition_startstop, self, 'stopping') 214 | super().stop() 215 | # stop pending fire2 if fire already stopped this transition 216 | if self.handle: 217 | self.handle.cancel() 218 | if TRACE.trace_level >= TRACE.task_cancel: 219 | print('TRACE%d:' % TRACE.task_cancel, self.handle, 'cancelled') 220 | self.handle = None 221 | 222 | def fire(self,event=None): 223 | """Shut down source nodes and schedule start of destination nodes. 224 | Lets the stack unwind by returning before destinations are started. 225 | Delay also gives time for Cozmo action cancellation to take effect.""" 226 | if not self.running: return 227 | if TRACE.trace_level >= TRACE.transition_fire: 228 | if event == None: 229 | evt_desc = '' 230 | else: 231 | evt_desc = ' on %s' % event 232 | print('TRACE%d:' % TRACE.transition_fire, self, 'firing'+evt_desc) 233 | for src in self.sources: 234 | src.stop() 235 | self.stop() 236 | action_cancel_delay = 0.01 # wait for source node action cancellations to take effect 237 | self.handle = self.robot.loop.call_later(action_cancel_delay, self.fire2, event) 238 | 239 | def fire2(self,event): 240 | if self.handle is None: 241 | print('@ @ @ @ @ HANDLE GONE:', self, 'SHOULD BE DEAD', self, event) 242 | return 243 | else: 244 | self.handle = None 245 | parent = self.sources[0].parent 246 | if not parent.running: 247 | # print('@ @ @ @ @ PARENT OF', self, 'IS', parent, 'IS DEAD! event=', event, '%x' % event.__hash__()) 248 | return 249 | for dest in self.destinations: 250 | if TRACE.trace_level >= TRACE.transition_fire: 251 | print('TRACE%d: ' % TRACE.transition_fire, self, 'starting', dest) 252 | dest.start(event) 253 | 254 | default_value_delay = 0.1 # delay before wildcard match will fire 255 | -------------------------------------------------------------------------------- /cozmo_fsm/transitions.py: -------------------------------------------------------------------------------- 1 | import random 2 | import re 3 | 4 | from .base import * 5 | from .events import * 6 | from .nodes import Say, Iterate 7 | 8 | class NullTrans(Transition): 9 | """Transition fires immediately; does not require an event to trigger it.""" 10 | def start(self): 11 | if self.running: return 12 | super().start() 13 | # Don't fire immediately on start because the source node(s) may 14 | # have other startup calls to make. Give them time to finish. 15 | self.handle = self.robot.loop.call_soon(self.fire) 16 | 17 | def stop(self): 18 | if self.handle: 19 | print(self, 'cancelling', self.handle) 20 | self.handle.cancel() 21 | self.handle = None 22 | super().stop() 23 | 24 | def fire(self, event=None): 25 | self.handle = None 26 | super().fire(event) 27 | 28 | 29 | class CSFEventBase(Transition): 30 | """Base class for Completion, Success, and Failure Events""" 31 | def __init__(self,event_type,count=None): 32 | super().__init__() 33 | self.event_type = event_type 34 | self.count = count 35 | 36 | def start(self): 37 | if self.running: return 38 | super().start() 39 | self.observed_sources = set() 40 | for source in self.sources: 41 | self.robot.erouter.add_listener(self, self.event_type, source) 42 | 43 | def handle_event(self,event): 44 | if not self.running: 45 | print('***',self,'got an event ', event, ' while not running!') 46 | return 47 | if TRACE.trace_level >= TRACE.listener_invocation: 48 | print('TRACE%d: %s is handling %s' % 49 | (TRACE.listener_invocation, self,event)) 50 | super().handle_event(event) 51 | if isinstance(event, self.event_type): 52 | self.observed_sources.add(event.source) 53 | if len(self.observed_sources) >= (self.count or len(self.sources)): 54 | self.fire(event) 55 | else: 56 | raise ValueError("%s can't handle %s" % (self.event_type, event)) 57 | 58 | class CompletionTrans(CSFEventBase): 59 | """Transition fires when source nodes complete.""" 60 | def __init__(self,count=None): 61 | super().__init__(CompletionEvent,count) 62 | 63 | class SuccessTrans(CSFEventBase): 64 | """Transition fires when source nodes succeed.""" 65 | def __init__(self,count=None): 66 | super().__init__(SuccessEvent,count) 67 | 68 | class FailureTrans(CSFEventBase): 69 | """Transition fires when source nodes fail.""" 70 | def __init__(self,count=None): 71 | super().__init__(FailureEvent,count) 72 | 73 | class CNextTrans(CSFEventBase): 74 | """Transition fires when source nodes complete.""" 75 | def __init__(self,count=None): 76 | super().__init__(CompletionEvent,count) 77 | 78 | def fire(self, event=None): 79 | super().fire(Iterate.NextEvent()) 80 | 81 | 82 | class NextTrans(Transition): 83 | """Transition sends a NextEvent to its target nodes to advance an iterator.""" 84 | def start(self, event=None): 85 | super().start() 86 | self.fire(Iterate.NextEvent()) 87 | 88 | 89 | class SayDataTrans(Transition): 90 | """Converts a DataEvent to Say.SayDataEvent so we can speak the data.""" 91 | def start(self): 92 | if self.running: return 93 | super().start() 94 | for source in self.sources: 95 | self.robot.erouter.add_listener(self, DataEvent, source) 96 | self.robot.erouter.add_listener(self, Say.SayDataEvent, source) 97 | 98 | def handle_event(self,event): 99 | if not self.running: return 100 | super().handle_event(event) 101 | if isinstance(event, Say.SayDataEvent): 102 | say_data_event = event 103 | elif isinstance(event, DataEvent): 104 | say_data_event = Say.SayDataEvent(event.data) 105 | else: 106 | return 107 | self.fire(say_data_event) 108 | 109 | 110 | class TimerTrans(Transition): 111 | """Transition fires when the timer has expired.""" 112 | def __init__(self,duration=None): 113 | if not isinstance(duration, (int, float)) or duration < 0: 114 | raise ValueError("TimerTrans requires a positive number for duration, not %s" % duration) 115 | super().__init__() 116 | self.set_polling_interval(duration) 117 | 118 | def poll(self): 119 | if not self.running: return 120 | self.fire() 121 | 122 | 123 | class TapTrans(Transition): 124 | """Transition fires when a cube is tapped.""" 125 | def __init__(self,cube=None): 126 | super().__init__() 127 | self.cube = cube 128 | 129 | def start(self): 130 | if self.running: return 131 | super().start() 132 | if self.cube: 133 | self.robot.erouter.add_listener(self,TapEvent,self.cube) 134 | else: 135 | self.robot.erouter.add_wildcard_listener(self,TapEvent,None) 136 | 137 | def handle_event(self,event): 138 | if not self.running: return 139 | if self.cube: 140 | self.fire(event) 141 | else: 142 | self.handle = \ 143 | self.robot.loop.call_later(Transition.default_value_delay, self.fire, event) 144 | 145 | 146 | class ObservedMotionTrans(Transition): 147 | """Transition fires when motion is observed in the camera image.""" 148 | def start(self): 149 | if self.running: return 150 | super().start() 151 | self.robot.erouter.add_listener(self,ObservedMotionEvent,None) 152 | 153 | def handle_event(self,event): 154 | if not self.running: return 155 | super().handle_event(event) 156 | self.fire(event) 157 | 158 | 159 | class UnexpectedMovementTrans(Transition): 160 | """Transition fires when unexpected movement is detected.""" 161 | def start(self): 162 | if self.running: return 163 | super().start() 164 | self.robot.erouter.add_listener(self,UnexpectedMovementEvent,None) 165 | 166 | def handle_event(self,event): 167 | if not self.running: return 168 | super().handle_event(event) 169 | self.fire(event) 170 | 171 | 172 | class DataTrans(Transition): 173 | """Transition fires when data matches.""" 174 | def __init__(self, data=None): 175 | super().__init__() 176 | self.data = data 177 | 178 | def start(self): 179 | if self.running: return 180 | super().start() 181 | for source in self.sources: 182 | if self.data is None or isinstance(self.data, type): 183 | self.robot.erouter.add_wildcard_listener(self, DataEvent, source) 184 | else: 185 | self.robot.erouter.add_listener(self, DataEvent, source) 186 | 187 | def handle_event(self,event): 188 | if not self.running: return 189 | super().handle_event(event) 190 | if isinstance(event,DataEvent): 191 | if self.data is None or \ 192 | (isinstance(self.data, type) and isinstance(event.data, self.data)): 193 | self.fire(event) 194 | else: 195 | try: 196 | if self.data == event.data: # error if == barfs 197 | self.fire(event) 198 | except TypeError: pass 199 | else: 200 | raise TypeError('%s is not a DataEvent' % event) 201 | 202 | class ArucoTrans(Transition): 203 | """Fires if one of the specified markers is visible""" 204 | def __init__(self,marker_ids=None): 205 | super().__init__() 206 | self.polling_interval = 0.1 207 | if isinstance(marker_ids,(list,tuple)): 208 | marker_ids = set(marker_ids) 209 | self.marker_ids = marker_ids 210 | 211 | def poll(self,event=None): 212 | if not self.running: return 213 | if self.marker_ids is None: 214 | if self.robot.world.aruco.seen_marker_ids != []: 215 | self.fire() 216 | elif isinstance(self.marker_ids,set): 217 | if self.marker_ids.intersection(self.robot.world.aruco.seen_marker_ids) != set(): 218 | self.fire() 219 | elif self.marker_ids in self.robot.world.aruco.seen_marker_ids: 220 | self.fire() 221 | 222 | 223 | class PatternMatchTrans(Transition): 224 | wildcard = re.compile('.*') 225 | 226 | def __init__(self, pattern=None, event_type=None): 227 | super().__init__() 228 | if pattern: 229 | pattern = re.compile(pattern) 230 | self.pattern = pattern 231 | self.event_type = event_type 232 | 233 | def start(self): 234 | if self.running: return 235 | super().start() 236 | if self.pattern is None: 237 | self.robot.erouter.add_wildcard_listener(self, self.event_type, None) 238 | else: 239 | self.robot.erouter.add_listener(self, self.event_type, None) 240 | 241 | def handle_event(self,event): 242 | if not self.running: return 243 | super().handle_event(event) 244 | if self.pattern is None: 245 | result = self.wildcard.match(event.string) 246 | else: 247 | result = self.pattern.match(event.string) 248 | if result: 249 | match_event = self.event_type(event.string,event.words,result) 250 | self.fire(match_event) 251 | 252 | class TextMsgTrans(PatternMatchTrans): 253 | """Transition fires when text message event matches pattern.""" 254 | def __init__(self,pattern=None): 255 | super().__init__(pattern,TextMsgEvent) 256 | 257 | 258 | class HearTrans(PatternMatchTrans): 259 | """Transition fires if speech event matches pattern.""" 260 | def __init__(self,pattern=None): 261 | super().__init__(pattern,SpeechEvent) 262 | 263 | 264 | class PilotTrans(Transition): 265 | """Fires if a matching PilotEvent is observed.""" 266 | def __init__(self,status=None): 267 | super().__init__() 268 | self.status = status 269 | 270 | def start(self): 271 | if self.running: return 272 | super().start() 273 | for source in self.sources: 274 | if self.status == None: 275 | self.robot.erouter.add_wildcard_listener(self, PilotEvent, source) 276 | else: 277 | self.robot.erouter.add_listener(self, PilotEvent, source) 278 | 279 | def handle_event(self,event): 280 | if not self.running: return 281 | super().handle_event(event) 282 | if self.status == None or self.status == event.status: 283 | self.fire(event) 284 | 285 | 286 | class RandomTrans(Transition): 287 | """Picks a destination node at random.""" 288 | def start(self): 289 | if self.running: return 290 | super().start() 291 | # Don't fire immediately on start because the source node(s) may 292 | # have other startup calls to make. Give them time to finish. 293 | self.handle = self.robot.loop.call_soon(self.fire) # okay to use Transition.fire 294 | 295 | def stop(self): 296 | if self.handle: 297 | self.handle.cancel() 298 | self.handle = None 299 | super().stop() 300 | 301 | def fire2(self,event): 302 | """Overrides Transition.fire2 to only start one randomly-chosen destination node.""" 303 | dest = random.choice(self.destinations) 304 | dest.start(event) 305 | --------------------------------------------------------------------------------