feat: GetObjectsGrippingPointsTool

src/rai_extensions/rai_perception/pyproject.toml

@@ -1,7 +1,6 @@
 [project]
 name = "rai-perception"
-# TODO, update the version once it is published to PyPi
-version = "0.3.1"
+version = "0.4.0"
 description = "Package for object detection, segmentation and gripping point detection."
 readme = "README.md"
 requires-python = ">=3.10,<3.13"

src/rai_extensions/rai_perception/rai_perception/__init__.py

@@ -36,6 +36,8 @@
 from .tools.gripping_points_tools import (  # noqa: E402
     GetObjectGrippingPointsTool,
     GetObjectGrippingPointsToolInput,
+    GetObjectsGrippingPointsTool,
+    GetObjectsGrippingPointsToolInput,
 )
 
 __all__ = [
@@ -47,6 +49,8 @@
     "GetDistanceToObjectsTool",
     "GetObjectGrippingPointsTool",
     "GetObjectGrippingPointsToolInput",
+    "GetObjectsGrippingPointsTool",
+    "GetObjectsGrippingPointsToolInput",
     "GrippingPointEstimator",
     "GrippingPointEstimatorConfig",
     "GroundedSamAgent",

src/rai_extensions/rai_perception/rai_perception/components/gripping_points.py

@@ -287,6 +287,71 @@ def _transform_points_source_to_target(
         return transformed
 
     # --------------------- Public API ---------------------
+    def run_multi_class(
+        self, classes: list[str]
+    ) -> dict[str, list[NDArray[np.float32]]]:
+        """Single GDINO + GSAM pass for all classes.
+
+        Returns {class_id: [per-instance point clouds in target frame]}.
+        Masks from GSAM are associated back to their class via the class_id
+        stored in each GDINO detection result, so the order of masks in the
+        GSAM response is assumed to match the order of detections.
+        """
+        logger = self.connector.node.get_logger()
+        result: dict[str, list[NDArray[np.float32]]] = {c: [] for c in classes}
+
+        camera_img_msg = self._get_image_message(self.camera_topic)
+        depth_msg = self.connector.receive_message(self.depth_topic).payload
+        camera_info = self._get_camera_info_message(self.camera_info_topic)
+        fx, fy, cx, cy = self._get_intrinsic_from_camera_info(camera_info)
+
+        gdino_future = self._call_gdino_node(camera_img_msg, ", ".join(classes))
+        gdino_resolved = get_future_result(
+            gdino_future, timeout_sec=self.config.service_timeout
+        )
+        if gdino_resolved is None:
+            logger.warning("Detection service returned None")
+            return result
+
+        gsam_future = self._call_gsam_node(camera_img_msg, gdino_resolved)
+        gsam_resolved = get_future_result(
+            gsam_future, timeout_sec=self.config.service_timeout
+        )
+        if gsam_resolved is None or not gsam_resolved.masks:
+            logger.warning("Segmentation service returned None or empty masks")
+            return result
+
+        depth_raw = convert_ros_img_to_ndarray(depth_msg)
+        depth = convert_depth_to_meters(depth_raw, depth_msg)
+        if self.conversion_ratio != 1.0:
+            depth = depth * float(self.conversion_ratio)
+
+        detections = gdino_resolved.detections.detections
+        for i, mask_msg in enumerate(gsam_resolved.masks):
+            if i >= len(detections):
+                break
+            det = detections[i]
+            class_id = det.results[0].hypothesis.class_id if det.results else None
+            if class_id not in result:
+                logger.warning(
+                    f"Detected class '{class_id}' not in requested classes, skipping"
+                )
+                continue
+            mask = cast(NDArray[np.uint8], convert_ros_img_to_ndarray(mask_msg))
+            masked_depth = np.zeros_like(depth, dtype=np.float32)
+            masked_depth[mask == 255] = depth[mask == 255]
+            points_camera = depth_to_point_cloud(masked_depth, fx, fy, cx, cy)
+            if points_camera.size == 0:
+                logger.warning(f"Point cloud for '{class_id}' instance {i} is empty")
+                continue
+            result[class_id].append(
+                self._transform_points_source_to_target(points_camera).astype(
+                    np.float32
+                )
+            )
+
+        return result
+
     def run(self, object_name: str) -> list[NDArray[np.float32]]:
         """Return Nx3 numpy array [X, Y, Z] of the object's masked point cloud in target frame."""
         logger = self.connector.node.get_logger()

src/rai_extensions/rai_perception/rai_perception/tools/gripping_points_tools.py

@@ -948,3 +948,111 @@ def _run(self, object_name: str) -> str:
             )
         # Delegate to the wrapped tool - it will return centroids with default_grasp preset
         return self.gripping_points_tool._run(object_name=object_name)
+
+
+# ---------------------------------------------------------------------------
+# Multi-class tool
+# ---------------------------------------------------------------------------
+
+
+class GetObjectsGrippingPointsToolInput(BaseModel):
+    object_names: list[str] = Field(
+        ...,
+        description=(
+            "List of object class names to detect and get gripping points for, "
+            "e.g. ['cube', 'cylinder', 'triangle']. All classes are detected in "
+            "a single GDINO + GSAM forward pass."
+        ),
+    )
+    debug: bool = Field(
+        default=False,
+        description="Publish intermediate pipeline results to ROS2 topics for RVIZ visualization.",
+    )
+
+
+class GetObjectsGrippingPointsTool(GetObjectGrippingPointsTool):
+    """Multi-class variant of GetObjectGrippingPointsTool.
+
+    Detects all requested classes in a single GDINO + GSAM forward pass, then
+    runs PointCloudFilter and GrippingPointEstimator per class. Prefer this over
+    calling GetObjectGrippingPointsTool once per class.
+    """
+
+    name: str = "get_objects_gripping_points"
+    description: str = (
+        "Get gripping points for all detected objects of multiple specified types "
+        "in a single perception pass. More efficient than get_object_gripping_points "
+        "when querying more than one class at a time."
+    )
+    args_schema: Type[GetObjectsGrippingPointsToolInput] = (  # type: ignore[assignment]
+        GetObjectsGrippingPointsToolInput
+    )
+
+    def _run(self, object_names: list[str], debug: bool = False) -> str:  # type: ignore[override]
+        @timeout(
+            self.timeout_sec,
+            f"Gripping point detection for {object_names} exceeded {self.timeout_sec} seconds",
+        )
+        def _run_with_timeout():
+            per_class_pcls = self.point_cloud_from_segmentation.run_multi_class(
+                object_names
+            )
+
+            if debug:
+                all_raw = [pc for pcs in per_class_pcls.values() for pc in pcs]
+                if all_raw:
+                    self._publish_point_cloud_debug(
+                        all_raw, "/debug/gripping_points/raw_point_clouds"
+                    )
+
+            per_class_gripping_points: dict[str, list] = {}
+            all_filtered: list = []
+            all_gripping_points: list = []
+            for class_id, point_clouds in per_class_pcls.items():
+                if not point_clouds:
+                    per_class_gripping_points[class_id] = []
+                    continue
+                filtered = self.point_cloud_filter.run(point_clouds)
+                gripping_points = (
+                    self.gripping_point_estimator.run(filtered) if filtered else []
+                )
+                per_class_gripping_points[class_id] = gripping_points
+                if debug:
+                    all_filtered.extend(filtered)
+                    all_gripping_points.extend(gripping_points)
+
+            if debug:
+                if all_filtered:
+                    self._publish_point_cloud_debug(
+                        all_filtered, "/debug/gripping_points/filtered_point_clouds"
+                    )
+                if all_gripping_points and all_filtered:
+                    self._publish_gripping_point_debug_data(
+                        all_filtered, all_gripping_points
+                    )
+
+            return self._format_result_message(object_names, per_class_gripping_points)
+
+        try:
+            return _run_with_timeout()
+        except RaiTimeoutError as e:
+            self.connector.node.get_logger().warning(f"Timeout: {e}")
+            return f"Timeout: Gripping point detection for {object_names} exceeded {self.timeout_sec} seconds"
+        except Exception:
+            raise
+
+    def _format_result_message(  # type: ignore[override]
+        self, object_names: list[str], per_class_gripping_points: dict[str, list]
+    ) -> str:
+        lines = []
+        for class_id, gripping_points in per_class_gripping_points.items():
+            if not gripping_points:
+                lines.append(f"No {class_id}s detected.")
+                continue
+            pts = ", ".join(
+                f"({float(gp[0]):.6f}, {float(gp[1]):.6f}, {float(gp[2]):.6f})"
+                for gp in gripping_points
+                if isinstance(gp, np.ndarray) and len(gp) >= 3
+            )
+            lines.append(f"{class_id}: [{pts}]")
+        return "\n".join(lines)