plant demo

predicators/spot_utils/perception/object_detection.py

@@ -19,6 +19,8 @@
 from functools import partial
 from pathlib import Path
 from typing import Any, Collection, Dict, List, Optional, Set, Tuple
+from datetime import datetime
+
 
 try:
     import apriltag
@@ -718,13 +720,19 @@ def _run_manual_test() -> None:
         assert path.exists()
         localizer = SpotLocalizer(robot, path, lease_client, lease_keepalive)
         rgbds = capture_images(robot, localizer, TEST_CAMERAS)
-
         language_ids: List[ObjectDetectionID] = [
             LanguageObjectDetectionID(d) for d in TEST_LANGUAGE_DESCRIPTIONS
         ]
         detections, artifacts = detect_objects(language_ids, rgbds)
+
+        with open(f"state_{datetime.now()}.txt", 'w') as file:
+            for obj_id, detection in detections.items():
+                file.write(f"Detected {obj_id} at {detection}\n")
+            file.write(f"Robot pose: {localizer.get_last_robot_pose()}\n")
+
         for obj_id, detection in detections.items():
             print(f"Detected {obj_id} at {detection}")
+        print(f"Robot pose: {localizer.get_last_robot_pose()}")
 
         # Visualize the artifacts.
         detections_outfile = Path(".") / "object_detection_artifacts.png"

state_2024-05-16 18:46:52.332823.txt

@@ -0,0 +1,3 @@
+Detected LanguageID(potted plant) at position -- X: 0.645 Y: -0.424 Z: -0.129 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000
+Detected LanguageID(green apple/tennis ball) at position -- X: 2.717 Y: -1.078 Z: -0.321 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000
+Robot pose: position -- X: 3.823 Y: 0.344 Z: 0.166 rotation -- W: -0.1006 X: -0.0028 Y: 0.0002 Z: 0.9949

state_2024-05-16 18:47:53.380841.txt

@@ -0,0 +1,3 @@
+Detected LanguageID(potted plant) at position -- X: 0.716 Y: -0.367 Z: -0.406 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000
+Detected LanguageID(green apple/tennis ball) at position -- X: 2.700 Y: -1.025 Z: -0.578 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000
+Robot pose: position -- X: 3.083 Y: 0.112 Z: -0.028 rotation -- W: -0.5851 X: 0.0199 Y: 0.0145 Z: 0.8106

state_2024-05-16 18:49:19.905006.txt

@@ -0,0 +1,2 @@
+Detected LanguageID(potted plant) at position -- X: 0.802 Y: -0.395 Z: -0.181 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000
+Robot pose: position -- X: 2.918 Y: -0.344 Z: 0.131 rotation -- W: 0.6081 X: 0.0032 Y: 0.0020 Z: -0.7938

state_2024-05-16 18:49:48.309635.txt

@@ -0,0 +1,2 @@
+Detected LanguageID(potted plant) at position -- X: 0.770 Y: -0.188 Z: -0.143 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000
+Robot pose: position -- X: 1.455 Y: -0.109 Z: 0.123 rotation -- W: 0.1047 X: -0.0038 Y: -0.0002 Z: -0.9945

state_2024-05-16 18:51:11.845416.txt

@@ -0,0 +1,3 @@
+Detected LanguageID(potted plant) at position -- X: 0.858 Y: -0.403 Z: -0.142 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000
+Detected LanguageID(green apple/tennis ball) at position -- X: 0.476 Y: -1.390 Z: -0.338 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000
+Robot pose: position -- X: 1.534 Y: -0.237 Z: 0.123 rotation -- W: 0.1069 X: -0.0084 Y: -0.0009 Z: -0.9942