feat: improve 3d gripping point detection

[Juliaj]

Purpose

• Address Improve 3D detection pipeline #693.

This is joint work with @maciejmajek, who implemented the core pipeline for 3D gripping point detection.

Proposed Changes

This PR adds a 3D gripping point detection mechanism to estimate grasping positions with various strategies for objects in their environment.

Pipeline Architecture:

RGB + Depth Images → GroundingDINO/SAM → Point Cloud Generation → 
Filtering → Gripping Point Estimation → 3D Coordinates for gripper

The implementation includes:

• GetGrippingPointTool (src/rai_core/rai/tools/ros2/detection/tools.py): main ROS2 tool that provides gripping point detection for specified objects. This will eventually become a replacement for GetObjectPositionsTool .

• PointCloudFromSegmentation and GrippingPointEstimator (src/rai_core/rai/tools/ros2/detection/pcl.py) to
  generate masked point clouds from segmented images and support multiple gripping point estimation strategies such as "centroid", `"top_plane" and RANSAC-based plane. A set of filters are introduced to handle noisy point clouds using sklearn algorithms.

• Timeout handling (src/rai_core/rai/tools/timeout.py): a ThreadPoolExecutor-based timeout mechanism for long-running operations

• The detection pipeline uses a two-tier configuration design: ROS2 parameters for runtime deployment settings (topics, frames, timeouts) that can be set via launch files, and Pydantic models for algorithm-specific parameters (thresholds, strategies, clustering) that are configured at tool initialization time.

• Debugging helpers to publish to point clouds + markers for Rviz2 with an annotated image generation with projected gripping points

• Unit tests tests/tools/ros2/test_detection_tools.py and integration/manual tests tests/tools/ros2/test_gripping_points.py

• Updated examples/manipulation-demo-v2.py with the new configurable detection tool. We can remove after review.

Issues

• Links to Improve 3D detection pipeline #693 (3D gripping point detection feature request)

Testing

To run unit tests

pytest tests/tools/ros2/test_detection_tools.py -s -v

To run manual tests

• Launch manipulation demo app, then run

pytest tests/tools/ros2/test_gripping_points.py::test_gripping_points_manipulation_demo -m "" -s -v

# or pass in strategy
pytest tests/tools/ros2/test_gripping_points.py::test_gripping_points_manipulation_demo -m "" -s -v --strategy <strategy>

Manipulation-demo-v2 has been run to validate the change.

TODOs

• Refactor code to align new GetGrippingPointTool with GetObjectPositionsTool.
• Fix timeout implementation - replaced signal-based approach (incompatible with worker threads) with ThreadPoolExecutor
• Add unit tests and manual integration tests
• Implement configuration system with namespacing (noted in TODO comment)
• Merge 3D detection pipeline code as part of rai_open_set_vision tools
• Code cleanup and address remaining TODO comments
• Rename/remove examples/manipulation-demo-v2.py after review.
• Conduct additional testing with real-world images and fine-tune default parameters

Future TODOs

• Add documentation and usage examples
• Rename rai_open_set_vision once consensus is reached

[maciejmajek]

Quality work. Thank you!

Ran the test suite.
Results below.

pytest tests/tools/ros2/test_gripping_points.py::test_gripping_points_manipulation_demo -m "" -s -v

Details

git diff (publishing filtered pcl)

diff --git a/src/rai_core/rai/tools/ros2/detection/pcl.py b/src/rai_core/rai/tools/ros2/detection/pcl.py
index 2adee2ff..c0a1202a 100644
--- a/src/rai_core/rai/tools/ros2/detection/pcl.py
+++ b/src/rai_core/rai/tools/ros2/detection/pcl.py
@@ -86,6 +86,7 @@ def _publish_gripping_point_debug_data(
     gripping_points_xyz: list[NDArray[np.float32]],
     base_frame_id: str = "egoarm_base_link",
     publish_duration: float = 10.0,
+    topic: str = "/debug_gripping_points_pointcloud",
 ) -> None:
     """Publish the gripping point debug data for visualization in RVIZ via point cloud and marker array.
 
@@ -112,7 +113,7 @@ def _publish_gripping_point_debug_data(
     msg.header.frame_id = base_frame_id  # type: ignore[reportUnknownMemberType]
     msg.points = [Point32(x=float(p[0]), y=float(p[1]), z=float(p[2])) for p in points]  # type: ignore[reportUnknownArgumentType]
     pub = connector.node.create_publisher(  # type: ignore[reportUnknownMemberType]
-        PointCloud, "/debug_gripping_points_pointcloud", 10
+        PointCloud, topic, 10
     )
 
     marker_pub = connector.node.create_publisher(  # type: ignore[reportUnknownMemberType]
diff --git a/tests/tools/ros2/test_gripping_points.py b/tests/tools/ros2/test_gripping_points.py
index 7c1106af..ccab06b9 100644
--- a/tests/tools/ros2/test_gripping_points.py
+++ b/tests/tools/ros2/test_gripping_points.py
@@ -203,9 +203,9 @@ def main(
 
     if filter_config is None:
         filter_config = {
-            "strategy": "dbscan",
-            "dbscan_eps": 0.02,
-            "dbscan_min_samples": 5,
+            "strategy": "isolation_forest",
+            #"if_max_samples": "auto",
+            #"if_contamination": 0.05,
         }
 
     services = ["/grounded_sam_segment", "/grounding_dino_classify"]
@@ -268,12 +268,16 @@ def main(
             segmented_clouds = gripping_tool.point_cloud_from_segmentation.run(
                 test_object
             )
+            filtered_clouds = gripping_tool.point_cloud_filter.run(segmented_clouds)
             print(
                 "\nPublishing debug data to /debug_gripping_points_pointcloud and /debug_gripping_points_markerarray"
             )
             _publish_gripping_point_debug_data(
                 connector, segmented_clouds, gripping_points, frames["target"]
             )
+            _publish_gripping_point_debug_data(
+                connector, filtered_clouds, gripping_points, frames["target"], topic='/debug_filtered_pointcloud'
+            )
             print("✅ Debug data published")
 
             annotated_image_path = f"{test_object}_{strategy}_gripping_points.jpg"
@@ -328,7 +332,7 @@ def test_gripping_points_maciej_demo(strategy):
         },
         filter_config={
             "strategy": "dbscan",
-            "dbscan_eps": 0.02,
+            "dbscan_eps": 0.2,
             "dbscan_min_samples": 10,
         },
     )

I've noticed, that the filtering does not work well for other methods than isolation_forest, but I believe this is just a default param issue.

I need to switch now, what's left for me to do is run the examples/manipulation-demo-v2.py. Will keep you updated!
Thanks @Juliaj!

[maciejmajek]

❤️ 😆

[maciejmajek]

When designing RAI 2.0, we decided to move a bit further away from ROS 2 because of its limitations with Python usage. However, ROS 2 had a very convenient launch system. I would love to have something similar in RAI, so scripts like this would no longer be necessary:
e.g., rai-run rai_openset openset_agents

[Juliaj]

Indeed, fascinating! I created an issue for this #696.

[maciejmajek]

Great work, I really like the new tools.
I am wondering if we should keep these tools and pipelines in rai-core or move them to rai_extensions/rai_openset_detection, since they come with extra dependencies.

The plan is to publish rai_openset_detection on PyPI, so it would be valuable to keep this code there. By "keep" I mean we should remove the old implementation and replace it with the new one.

[Juliaj]

These are good points! I agree that rai_extensions would be a better location for these tools since they depend on GDINO and GSAM services. I also just had a good look of segmentation_tools.py and noticed there's some overlap between that file in rai_open_set_vision and the new detection pipeline.

A few questions to help clarify the approach:

• Has rai_open_set_vision been released to PyPI? I couldn't find it there yet.
• When you mention "remove the old implementation and replace it with the new one," are you suggesting we consolidate the code in segmentation_tools.py with the new detection pipeline, or would you prefer to release it as a separate rai_openset_detection package as you mentioned?

Do you have a preference for whether this consolidation should be done in this PR or handled separately?

[maciejmajek]

No, it hasn't. The main blocker is a git+ python dependency (pypi does not allow packages with git dependencies)

When you mention "remove the old implementation a...

iirc there are four tools in the rai_open_set_vision. One of them is GetGrabbingPointTool, which uses the old api. This one should be replaced with your implementation. (we can of course leave it and add your implementation + mark the previous one as deprecated)

So, the contents of detection directory should be moved to the open set package.
When we have that the next steps would be (for the rai_open_set_vision package):

• Remove ROS 2 configuration files
• Remove git depenendecies
• Rename the package to something a little bit more graceful
• Publish to pypi

Do you have a preference for whether this consolidation should be done in this PR or handled separately?

Preferably in this PR, we are using semver for rai-core versioning and removing features will introduce breaking changes (major bump).

[Juliaj]

@maciejmajek, the work of merging 3D detection pipeline code to rai_open_set_vision package has been completed. Instead of modifying segementation_tools.py directly, I added the 3D detection pipeline as a new tool so that we can gradually migrate to it. Please review when you have a chance. Please also let me know whether we should update manipulation demo to -v2.

To reduce the burden of the PR review and keep the code changes more manageable, I propose we address the rai_open_set_vision package renaming and documentation updates in future PRs if you're okay with that approach.

[Juliaj]

I've noticed, that the filtering does not work well for other methods than isolation_forest, but I believe this is just a default param issue.

Yeah, isolation_forest works better for the manipulation demo. I changed the default algo for filtering to isolation_forest, wdyt?

[Juliaj]

@maciejmajek, this was to preserve some of your previous test code so that you can easily test the new API with your setup. Let me know whether you'd like to keep it or any updates needed.