Add voice control, working but need more work

dora_ulite6/dora_ulite6/main.py

@@ -781,6 +781,12 @@ def _status_snapshot(helper: ULite6Helper) -> Dict[str, Any]:
     }
 
 
+def _log(msg: str) -> None:
+    """Print a timestamped log message."""
+    timestamp = time.strftime("%H:%M:%S")
+    print(f"[ulite6 {timestamp}] {msg}", flush=True)
+
+
 def main() -> None:
     node = Node()
 
@@ -791,7 +797,42 @@ def main() -> None:
     api_port = int(os.getenv("API_PORT", "8080"))
     vacuum_enabled = os.getenv("VACUUM_ENABLED", "false").lower() in ("true", "1", "yes")
 
+    # Initial position settings
+    init_mode = os.getenv("INIT_MODE", "none").lower()  # "home", "pose", or "none"
+    init_x = float(os.getenv("INIT_X", "300.0"))
+    init_y = float(os.getenv("INIT_Y", "0.0"))
+    init_z = float(os.getenv("INIT_Z", "250.0"))
+    init_roll = float(os.getenv("INIT_ROLL", "180.0"))
+    init_pitch = float(os.getenv("INIT_PITCH", "0.0"))
+    init_yaw = float(os.getenv("INIT_YAW", "0.0"))
+    init_speed = float(os.getenv("INIT_SPEED", "50.0"))
+
+    _log(f"Connecting to robot at {robot_ip}...")
     helper = ULite6Helper(robot_ip)
+    _log("Robot connected")
+
+    # Move to initial position on startup
+    if init_mode == "home":
+        _log("Moving to home position...")
+        code = helper.go_home()
+        if code == 0:
+            _log("Home position reached")
+        else:
+            _log(f"Home failed with code {code}")
+    elif init_mode == "pose":
+        _log(f"Moving to initial pose: [{init_x}, {init_y}, {init_z}] roll={init_roll} pitch={init_pitch} yaw={init_yaw}")
+        code = helper.move_to_pose(
+            init_x, init_y, init_z,
+            init_roll, init_pitch, init_yaw,
+            speed=init_speed,
+            units="mm",
+        )
+        if code == 0:
+            _log("Initial pose reached")
+        else:
+            _log(f"Move to initial pose failed with code {code}")
+    else:
+        _log("Skipping initial position (INIT_MODE=none)")
 
     # Create and start FastAPI server in background thread
     app = create_api(helper, default_speed, default_units, vacuum_enabled)
@@ -895,6 +936,28 @@ def main() -> None:
                         code=code,
                         status=_status_snapshot(helper),
                     )
+                elif action in ("vacuum_on", "vacuum_off"):
+                    if not vacuum_enabled:
+                        _send_command_status(
+                            node,
+                            command_id=command_id,
+                            action=action,
+                            ok=False,
+                            message="Vacuum gripper not enabled",
+                            status=_status_snapshot(helper),
+                        )
+                        continue
+                    vacuum_on = action == "vacuum_on"
+                    code = helper.set_vacuum_gripper(vacuum_on)
+                    _send_command_status(
+                        node,
+                        command_id=command_id,
+                        action=action,
+                        ok=code == 0,
+                        message="Vacuum command executed",
+                        code=code,
+                        status=_status_snapshot(helper),
+                    )
                 else:
                     _send_command_status(
                         node,