diff --git a/src/rai_extensions/rai_perception/pyproject.toml b/src/rai_extensions/rai_perception/pyproject.toml index 935a38716..dca82a77f 100644 --- a/src/rai_extensions/rai_perception/pyproject.toml +++ b/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" diff --git a/src/rai_extensions/rai_perception/rai_perception/__init__.py b/src/rai_extensions/rai_perception/rai_perception/__init__.py index 2efabc185..078fb193f 100644 --- a/src/rai_extensions/rai_perception/rai_perception/__init__.py +++ b/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", diff --git a/src/rai_extensions/rai_perception/rai_perception/components/gripping_points.py b/src/rai_extensions/rai_perception/rai_perception/components/gripping_points.py index cb08d0b3e..eeeea9c9f 100644 --- a/src/rai_extensions/rai_perception/rai_perception/components/gripping_points.py +++ b/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() diff --git a/src/rai_extensions/rai_perception/rai_perception/tools/gripping_points_tools.py b/src/rai_extensions/rai_perception/rai_perception/tools/gripping_points_tools.py index 15cff6029..1665121e9 100644 --- a/src/rai_extensions/rai_perception/rai_perception/tools/gripping_points_tools.py +++ b/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) diff --git a/src/src/rai_interfaces b/src/src/rai_interfaces deleted file mode 160000 index f03dc0f21..000000000 --- a/src/src/rai_interfaces +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f03dc0f217679980447dfead177979f485eb44f6 diff --git a/uv.lock b/uv.lock index bb92e4e33..9da3cc3ec 100644 --- a/uv.lock +++ b/uv.lock @@ -4865,7 +4865,7 @@ sdist = { url = "https://files.pythonhosted.org/packages/a3/ec/d16bb3d94ccf45524 [[package]] name = "rai-perception" -version = "0.3.1" +version = "0.4.0" source = { editable = "src/rai_extensions/rai_perception" } dependencies = [ { name = "rai-core" },