feat: improve 3d gripping point detection

[Juliaj]

Purpose

• Address #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 #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

• [x] Refactor code to align new GetGrippingPointTool with GetObjectPositionsTool.
• [x] Fix timeout implementation - replaced signal-based approach (incompatible with worker threads) with ThreadPoolExecutor
• [x] Add unit tests and manual integration tests
• [x] Implement configuration system with namespacing (noted in TODO comment)
• [x] Merge 3D detection pipeline code as part of rai_open_set_vision tools
• [x] 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

[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?

[boczekbartek]

@Juliaj Apologies for no activity on our side for this PR. Would you find some time to resolve conflicts, so we can continue the review?

[codecov[bot]]

Codecov Report

:white_check_mark: All modified and coverable lines are covered by tests. :white_check_mark: Project coverage is 65.70%. Comparing base (148bf74) to head (3404ca1). :white_check_mark: All tests successful. No failed tests found.

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #694      +/-   ##
==========================================
+ Coverage   65.32%   65.70%   +0.37%     
==========================================
  Files          78       80       +2     
  Lines        3386     3423      +37     
==========================================
+ Hits         2212     2249      +37     
  Misses       1174     1174              

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

[Juliaj]

@Juliaj Apologies for no activity on our side for this PR. Would you find some time to resolve conflicts, so we can continue the review?

@boczekbartek, thanks for helping with the review. I did the merge but ran into some issues with CI. I will look into this shortly and circle back once the CI is happy!

[maciejmajek]

https://github.com/RobotecAI/rai/pull/727#discussion_r2604923660

[maciejmajek]

https://github.com/RobotecAI/rai/pull/727#discussion_r2602732857