Yara/pick and place yolo

.gitignore

@@ -149,4 +149,4 @@ common/vision/lasr_vision_open_vocabulary/lasr_vision_open_vocabulary/models/l2_
 *.sif
 common/vision/lasr_vision_open_vocabulary/models/*
 common/vision/lasr_vision_open_vocabulary/yolo*.pt
-yolo*.pt
+yolo*.pt

common/foundation_models/lasr_vlm/lasr_vlm/nodes/vlm_service.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python3
+import os
+import tempfile
+
+import cv2
+import numpy as np
+import rclpy
+from rclpy.node import Node
+
+from lasr_vlm_interfaces.srv import VlmDescribePeople
+from lasr_vlm.vlm_inference import (
+    ModelConfig,
+    VLMInference,
+    visually_describe_people,
+)
+
+
+class VlmDescribePeopleService(Node):
+    """
+    ROS 2 service node that wraps VLM inference to visually describe people.
+    Receives a ROS image, runs it through the VLM, and returns the attributes.
+    """
+
+    def __init__(self):
+        super().__init__("vlm_describe_people_service")
+
+        self.create_service(
+            VlmDescribePeople,
+            "/vlm/describe_people",
+            self.describe_people_callback,
+        )
+
+        model_config = ModelConfig(model_name="moondream")
+        self.vlm = VLMInference(model_config, new_model=False)
+        self.get_logger().info("VLM Describe People service started")
+
+    def _image_msg_to_bgr8(self, image_msg):
+        """Convert a ROS Image message to an OpenCV BGR image without cv_bridge."""
+        if image_msg.encoding not in ("bgr8", "rgb8", "mono8"):
+            raise ValueError(f"Unsupported image encoding: {image_msg.encoding}")
+
+        image = np.frombuffer(image_msg.data, dtype=np.uint8)
+
+        if image_msg.encoding == "mono8":
+            image = image.reshape((image_msg.height, image_msg.width))
+            return cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
+
+        image = image.reshape((image_msg.height, image_msg.width, 3))
+        if image_msg.encoding == "rgb8":
+            return cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
+
+        return image
+
+    def describe_people_callback(self, request, response):
+        """
+        Handle incoming service requests.
+        Converts the ROS image to a file, runs VLM inference, and returns attributes.
+        """
+        self.get_logger().info("Received request to describe person")
+
+        try:
+            cv_image = self._image_msg_to_bgr8(request.image_raw)
+
+            with tempfile.NamedTemporaryFile(suffix=".jpg", delete=False) as f:
+                tmp_path = f.name
+            cv2.imwrite(tmp_path, cv_image)
+
+            result = visually_describe_people(
+                input_image=tmp_path,
+                inference=self.vlm,
+            )
+
+            os.unlink(tmp_path)
+
+            def _get(key, default):
+
+                val = result.get(key, [default])
+
+                return val[0] if isinstance(val, list) and val else default
+
+            response.hair_color = str(_get("hair_color", "unknown"))
+
+            response.hair_length = str(_get("hair_length", "unknown"))
+
+            response.glasses = bool(_get("glasses", False))
+
+            response.hat = bool(_get("hat", False))
+
+            response.shirt_color = str(_get("shirt color", "unknown"))
+
+            self.get_logger().info(f"VLM result: {result}")
+
+        except Exception as e:
+            self.get_logger().error(f"Failed to describe person: {e}")
+
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VlmDescribePeopleService()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

common/foundation_models/lasr_vlm/lasr_vlm/test_vlm_service.py

@@ -0,0 +1,192 @@
+#!/usr/bin/env python3
+"""
+Two-layer test for the VLM describe-people pipeline:
+  1. Unit-test parse_vlm_response directly (no ROS, no GPU)
+  2. End-to-end ROS service client (requires service + Ollama running)
+
+Usage:
+  # Unit tests only:
+  python3 test_vlm_describe_people.py --unit
+
+  # End-to-end service call:
+  python3 test_vlm_describe_people.py --e2e /path/to/person.jpg
+"""
+
+import sys
+import argparse
+
+import numpy as np
+
+# ─── Layer 1: unit-test the parser ────────────────────────────────────────────
+
+
+def test_parser():
+    """Test parse_vlm_response against realistic model outputs without needing ROS."""
+    # Import directly from the inference module (no ROS needed)
+    from lasr_vlm.vlm_inference import parse_vlm_response
+
+    attributes = ["hair_color", "hair_length", "glasses", "hat", "shirt color"]
+
+    cases = [
+        # (description, raw_response, expected_dict)
+        (
+            "clean key:value format",
+            "hair_color: brown, hair_length: short, glasses: True, hat: False, shirt color: red.",
+            {
+                "hair_color": "brown",
+                "hair_length": "short",
+                "glasses": True,
+                "hat": False,
+                "shirt color": "red",
+            },
+        ),
+        (
+            "verbose sentence answer",
+            "The person has long blonde hair. They are not wearing glasses or a hat. Their shirt color is blue.",
+            # parser will likely fail on hair_color/hair_length here — shows the fragility
+            {},  # we just print, don't assert
+        ),
+        (
+            "uppercase values",
+            "hair_color: Black, hair_length: Long, glasses: YES, hat: NO, shirt color: Green.",
+            {
+                "hair_color": "black",
+                "hair_length": "long",
+                "glasses": True,
+                "hat": False,
+                "shirt color": "green",
+            },
+        ),
+        (
+            "missing attributes",
+            "hair_color: red, glasses: False.",
+            {
+                "hair_color": "red",
+                "glasses": False,
+                "hair_length": "unknown",
+                "hat": "unknown",
+                "shirt color": "unknown",
+            },
+        ),
+    ]
+
+    print("=== Parser Unit Tests ===\n")
+    for desc, raw, expected in cases:
+        result = parse_vlm_response(raw, attributes)
+        print(f"[{desc}]")
+        print(f"  Input   : {raw!r}")
+        print(f"  Parsed  : {result}")
+        if expected:
+            passed = all(result.get(k) == v for k, v in expected.items())
+            print(f"  Status  : {'PASS' if passed else 'FAIL'}")
+            if not passed:
+                for k, v in expected.items():
+                    if result.get(k) != v:
+                        print(
+                            f"    MISMATCH {k!r}: got {result.get(k)!r}, expected {v!r}"
+                        )
+        print()
+
+
+# ─── Layer 2: end-to-end ROS service call ─────────────────────────────────────
+
+
+def test_service(image_path: str):
+    import cv2
+    import rclpy
+    from rclpy.node import Node
+    from sensor_msgs.msg import Image
+    from lasr_vlm_interfaces.srv import VlmDescribePeople
+
+    class VlmTestClient(Node):
+        def __init__(self):
+            super().__init__("vlm_test_client")
+            self.client = self.create_client(VlmDescribePeople, "/vlm/describe_people")
+
+        def _cv2_to_image_msg(self, cv_image):
+            image_msg = Image()
+            image_msg.height, image_msg.width = cv_image.shape[:2]
+            image_msg.encoding = "bgr8"
+            image_msg.is_bigendian = False
+            image_msg.step = cv_image.shape[1] * cv_image.shape[2]
+            image_msg.data = np.ascontiguousarray(cv_image).tobytes()
+            return image_msg
+
+        def run(self, image_path: str):
+            self.get_logger().info("Waiting for /vlm/describe_people...")
+            if not self.client.wait_for_service(timeout_sec=15.0):
+                self.get_logger().error("Service not available. Is the server running?")
+                return
+
+            cv_image = cv2.imread(image_path)
+            if cv_image is None:
+                self.get_logger().error(f"Could not read image: {image_path}")
+                return
+
+            self.get_logger().info(f"Image loaded: {cv_image.shape} from {image_path}")
+
+            request = VlmDescribePeople.Request()
+            request.image_raw = self._cv2_to_image_msg(cv_image)
+
+            self.get_logger().info(
+                "Sending request (Ollama inference may take ~10-30s)..."
+            )
+            future = self.client.call_async(request)
+            rclpy.spin_until_future_complete(self, future, timeout_sec=60.0)
+
+            if future.result() is None:
+                self.get_logger().error("Call timed out or returned None.")
+                return
+
+            r = future.result()
+            print("\n=== Service Response ===")
+            print(f"  hair_color  : {r.hair_color}")
+            print(f"  hair_length : {r.hair_length}")
+            print(f"  glasses     : {r.glasses}")
+            print(f"  hat         : {r.hat}")
+            print(f"  shirt_color : {r.shirt_color}")
+            print("========================\n")
+
+            # Flag unparsed fields
+            unknowns = [
+                f
+                for f, v in [
+                    ("hair_color", r.hair_color),
+                    ("hair_length", r.hair_length),
+                    ("shirt_color", r.shirt_color),
+                ]
+                if v == "unknown"
+            ]
+            if unknowns:
+                print(
+                    f"⚠ These came back 'unknown' — check the raw VLM output in the service logs: {unknowns}"
+                )
+
+    rclpy.init()
+    node = VlmTestClient()
+    try:
+        node.run(image_path)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+# ─── Entry point ──────────────────────────────────────────────────────────────
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--unit", action="store_true", help="Run parser unit tests")
+    parser.add_argument(
+        "--e2e", metavar="IMAGE", help="Run end-to-end service test with this image"
+    )
+    args = parser.parse_args()
+
+    if not args.unit and not args.e2e:
+        parser.print_help()
+        sys.exit(1)
+
+    if args.unit:
+        test_parser()
+
+    if args.e2e:
+        test_service(args.e2e)

common/foundation_models/lasr_vlm/lasr_vlm/vlm_inference.py

@@ -142,10 +142,16 @@ def visually_describe_people(input_image, inference: VLMInference) -> dict[str,
     )
     for attr in attributes:
         user_query += f"\n- {attr}"
+    # user_query_example = (
+    #     "\n\n The structure of the response should be a comma separated list of attribute: value pairs. For example, "
+    #     "'hair_color: _, hair_length: _, glasses: _, hat: _, shirt color: _', where the _ is replaced with the model's answer for that attribute. "
+    #     "For true or false attributes, the value should be simply true or false. For example, 'glasses: true' if the model thinks the person is wearing glasses, and 'hat: false' if the model thinks the person is not wearing a hat."
+    # )
     user_query_example = (
         "\n\n The structure of the response should be a comma separated list of attribute: value pairs. For example, "
         "'hair_color: _, hair_length: _, glasses: _, hat: _, shirt color: _', where the _ is replaced with the model's answer for that attribute. "
-        "For true or false attributes, the value should be simply true or false. For example, 'glasses: true' if the model thinks the person is wearing glasses, and 'hat: false' if the model thinks the person is not wearing a hat."
+        "For glasses and hat, only answer true if they are clearly and visibly present in the image. "
+        "If you are not sure, answer false. For example, 'glasses: false' means the person is definitely not wearing glasses."
     )
     user_query += user_query_example
 

common/foundation_models/lasr_vlm/requirements.txt

@@ -4,6 +4,7 @@
 #
 #    pip-compile requiremenets.in
 #
+numpy<2.0
 annotated-types==0.7.0
     # via pydantic
 anyio==4.13.0

common/foundation_models/lasr_vlm/setup.py

@@ -25,7 +25,6 @@ def run(self):
     name=package_name,
     version="0.0.0",
     packages=find_packages(exclude=["test"]),
-    cmdclass={"install": InstallCommand},
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
         ("share/" + package_name, ["package.xml", "requirements.txt"]),
@@ -42,6 +41,6 @@ def run(self):
         ],
     },
     entry_points={
-        "console_scripts": [],
+        "console_scripts": ["vlm_service = lasr_vlm.nodes.vlm_service:main"],
     },
 )