feat: improve 3d gripping point detection #694
Conversation
Juliaj
force-pushed
the
feat/debug_3dpipe
branch
from
359ec5d to
6385b23
Compare
There was a problem hiding this comment.
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 -vDetails
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!
Yeah, isolation_forest works better for the manipulation demo. I changed the default algo for filtering to |
Juliaj
force-pushed
the
feat/debug_3dpipe
branch
from
91a6ee2 to
8713bed
Compare
|
|
||
|
|
||
| @pytest.mark.manual | ||
| def test_gripping_points_maciej_demo(strategy): |
There was a problem hiding this comment.
@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.
|
@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 Report✅ All modified and coverable lines are covered by tests. Additional details and impacted files@@ Coverage Diff @@
## main #694 +/- ##
==========================================
+ Coverage 65.34% 65.72% +0.37%
==========================================
Files 78 80 +2
Lines 3388 3425 +37
==========================================
+ Hits 2214 2251 +37
Misses 1174 1174 ☔ View full report in Codecov by Sentry. |
@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! |
|
@coderabbitai review |
✅ Actions performedReview triggered.
|
WalkthroughThis PR introduces ROS2-based point-cloud manipulation tooling with gripping point detection, adds timeout decorators for functions and methods, deprecates an older positioning tool, establishes new public APIs for perception components, includes a new interactive manipulation demo, and updates CI test selection and test infrastructure. Changes
Sequence Diagram(s)sequenceDiagram
autonumber
participant User
participant Tool as GetObjectGrippingPointsTool
participant Segment as PointCloudFromSegmentation
participant Filter as PointCloudFilter
participant Estimate as GrippingPointEstimator
participant GDINO as GDINO Service
participant GSam as Grounded SAM Service
participant TF as Transform Service
User->>Tool: _run(object_name)
activate Tool
rect rgba(100, 180, 220, 0.3)
Note over Segment,GSam: Segmentation Phase
Tool->>Segment: run(object_name)
activate Segment
Segment->>GDINO: detect_bounding_box(image, object_name)
GDINO-->>Segment: bounding_box
Segment->>GSam: get_mask(image, bounding_box)
GSam-->>Segment: segmentation_mask
Segment->>TF: transform_points(camera → target_frame)
TF-->>Segment: transformed_points
Segment-->>Tool: [segmented_point_clouds]
deactivate Segment
end
rect rgba(180, 220, 100, 0.3)
Note over Filter: Filtering Phase
Tool->>Filter: run(segmented_point_clouds)
activate Filter
Filter-->>Tool: [filtered_point_clouds]
deactivate Filter
end
rect rgba(220, 150, 100, 0.3)
Note over Estimate: Estimation Phase
Tool->>Estimate: run(filtered_point_clouds)
activate Estimate
Estimate-->>Tool: [gripping_points]
deactivate Estimate
end
Tool-->>User: formatted_result
deactivate Tool
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes The PR introduces substantial new functionality across multiple packages with varying complexity. Key factors: multiple new public APIs and classes with non-trivial logic (PCL pipeline with multiple strategies), ROS2 service integration and frame transformations, comprehensive test coverage with both unit and end-to-end tests, and heterogeneous changes spanning perception tools, utilities, exports, and examples requiring separate reasoning for each cohort. Suggested reviewers
🚥 Pre-merge checks | ✅ 2 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (2 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing touches
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 7
🤖 Fix all issues with AI agents
In @examples/manipulation-demo-v2.py:
- Line 12: Fix the typo in the license header comment: replace the incorrect
token "goveself.rning" with the correct word "governing" so the sentence reads
"See the License for the specific language governing permissions and" (update
the string in the header comment accordingly).
- Around line 133-145: The program initializes ROS2 inside create_agent() via
rclpy.init() but never calls rclpy.shutdown() or connector.shutdown() on exit;
wrap the interactive loop in a try/except KeyboardInterrupt/finally (or use
try/finally) and in the finally block call the agent's connector.shutdown() (or
the connector instance returned/owned by create_agent) and rclpy.shutdown() to
ensure ROS2 resources are cleaned up when the user exits (Ctrl+C) or the program
terminates.
In @src/rai_core/rai/tools/ros2/manipulation/custom.py:
- Line 263: The deprecation decorator string is pointing to the wrong exported
tool; update the @deprecated decorator on the affected symbol to say "Use
GetGrabbingPointTool from rai_perception instead" (replace the current
"GetObjectGrippingPointsTool" text) so it correctly references the exported tool
name used in the imports and rai_perception's __init__.py; locate the
@deprecated(...) decorator above the relevant function/class in custom.py and
change only the message text.
In @src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.py:
- Around line 122-130: The docstring for the function that publishes gripping
point debug data is out of sync: it states publish_duration defaults to 10.0
while the actual default value is 5.0; update the docstring to reflect the true
default (publish_duration=5.0) or change the function signature to use 10.0 so
they match (adjust the parameter named publish_duration in the publishing
function in pcl_detection.py and update its docstring accordingly).
- Around line 115-121: The type hint for obj_points_xyz in
_publish_gripping_point_debug_data is incorrect: change it from
NDArray[np.float32] to a sequence type that reflects how it is used (e.g.,
Sequence[NDArray[np.float32]] or List[NDArray[np.float32]]) so calls like
np.concatenate(obj_points_xyz, axis=0) are type-correct; update the function
signature accordingly, add the necessary typing import (from typing import
Sequence or List), and adjust any related docstring/comments to reflect that
obj_points_xyz is a list/sequence of arrays.
In @tests/rai_perception/test_gripping_points.py:
- Line 15: The shebang line "#!/usr/bin/env python3" must be the very first line
of the file; move that shebang from its current position after the license
header to line 1 (before any comments or license text) so the interpreter is
correctly recognized when the file is executed.
🧹 Nitpick comments (9)
tests/tools/test_timeout.py (1)
19-24: Clean up unnecessary noqa directives.The
noqadirectives on lines 20 and 22 are unnecessary. Sincerclpyis referenced on line 22, it's not an unused import (F401), and since_is conventionally used for values you don't care about, the F841 suppression is also not needed.♻️ Proposed cleanup
try: - import rclpy # noqa: F401 + import rclpy - _ = rclpy # noqa: F841 + _ = rclpy except ImportError: pytest.skip("ROS2 is not installed", allow_module_level=True)tests/rai_perception/test_pcl_detection_tools.py (1)
196-198: Prefix unused parameter with underscore.The
obj_nameparameter matches the mocked method's signature but is unused in the function body. Use_obj_nameor_to indicate this is intentional.♻️ Proposed fix
- def slow_operation(obj_name): + def slow_operation(_obj_name): time.sleep(2.0) # Longer than timeout return []src/rai_core/rai/tools/timeout.py (2)
49-100: Improve type hints and exception chaining.Two recommended improvements:
- Use explicit
str | Noneinstead of implicitOptionalfortimeout_message(line 49)- Use
raise ... from Noneto indicate intentional exception transformation (line 96)♻️ Proposed improvements
-def timeout(seconds: float, timeout_message: str = None) -> Callable[[F], F]: +def timeout(seconds: float, timeout_message: str | None = None) -> Callable[[F], F]: """ Decorator that adds timeout functionality to a function. Parameters ---------- seconds : float Timeout duration in seconds timeout_message : str, optional Custom timeout message. If not provided, a default message will be used. Returns ------- Callable Decorated function with timeout functionality Raises ------ RaiTimeoutError When the decorated function exceeds the specified timeout Examples -------- >>> @timeout(5.0, "Operation timed out") ... def slow_operation(): ... import time ... time.sleep(10) ... return "Done" >>> >>> try: ... result = slow_operation() ... except RaiTimeoutError as e: ... print(f"Timeout: {e}") """ def decorator(func: F) -> F: @wraps(func) def wrapper(*args, **kwargs): with concurrent.futures.ThreadPoolExecutor(max_workers=1) as executor: future = executor.submit(func, *args, **kwargs) try: return future.result(timeout=seconds) except concurrent.futures.TimeoutError: message = ( timeout_message or f"Function '{func.__name__}' timed out after {seconds} seconds" ) - raise RaiTimeoutError(message) + raise RaiTimeoutError(message) from None return wrapper return decorator
103-159: Improve type hints and exception chaining.Same improvements as the
timeoutdecorator:
- Use explicit
str | Noneinstead of implicitOptionalfortimeout_message(line 103)- Use
raise ... from Noneto indicate intentional exception transformation (line 155)♻️ Proposed improvements
-def timeout_method(seconds: float, timeout_message: str = None) -> Callable[[F], F]: +def timeout_method(seconds: float, timeout_message: str | None = None) -> Callable[[F], F]: """ Decorator that adds timeout functionality to an instance method. Similar to timeout but designed for instance methods. The default error message includes the class name for better debugging context. Parameters ---------- seconds : float Timeout duration in seconds timeout_message : str, optional Custom timeout message. If not provided, a default message will be used. Returns ------- Callable Decorated method with timeout functionality Raises ------ RaiTimeoutError When the decorated method exceeds the specified timeout Examples -------- >>> class MyClass: ... @timeout_method(3.0, "Method timed out") ... def slow_method(self): ... import time ... time.sleep(5) ... return "Done" >>> >>> obj = MyClass() >>> try: ... result = obj.slow_method() ... except RaiTimeoutError as e: ... print(f"Timeout: {e}") """ def decorator(func: F) -> F: @wraps(func) def wrapper(self, *args, **kwargs): with concurrent.futures.ThreadPoolExecutor(max_workers=1) as executor: future = executor.submit(func, self, *args, **kwargs) try: return future.result(timeout=seconds) except concurrent.futures.TimeoutError: message = ( timeout_message or f"Method '{func.__name__}' of {self.__class__.__name__} timed out after {seconds} seconds" ) - raise RaiTimeoutError(message) + raise RaiTimeoutError(message) from None return wrapper return decoratortests/rai_perception/test_gripping_points.py (3)
180-188: Use explicitOptionalor| Nonefor parameters withNonedefaults.Per PEP 484, implicit
Optional(using= Nonewithout type annotation includingNone) is prohibited. The type checker cannot infer the intended type.♻️ Suggested fix
def main( test_object: str = "cube", strategy: str = "centroid", - topics: dict = None, - frames: dict = None, - estimator_config: dict = None, - filter_config: dict = None, + topics: dict | None = None, + frames: dict | None = None, + estimator_config: dict | None = None, + filter_config: dict | None = None, debug_enabled: bool = False, ):
147-149: Broad exception handling silently masks transform failures.Catching all exceptions and returning original (untransformed) points could mask real issues during testing. Consider logging more context or re-raising specific exceptions that indicate configuration problems.
♻️ Consider narrowing the exception type
- except Exception as e: - print(f"Transform error: {e}") - return points + except (tf2_ros.LookupException, tf2_ros.ExtrapolationException) as e: + print(f"Transform lookup error: {e}") + return points
103-149: Consider reusing_quaternion_to_rotation_matrixfrompcl_detection.py.The quaternion-to-rotation-matrix logic here duplicates
PointCloudFromSegmentation._quaternion_to_rotation_matrixinpcl_detection.py. While test isolation can justify some duplication, this helper is deterministic and could be imported to reduce maintenance burden.src/rai_extensions/rai_perception/rai_perception/__init__.py (1)
21-35: Remove unusednoqadirectives.The
# noqa: E402comments are unnecessary since E402 (module level import not at top of file) is not enabled in the linter configuration. These can be safely removed for cleaner code.♻️ Suggested cleanup
-from .agents import GroundedSamAgent, GroundingDinoAgent # noqa: E402 -from .tools import GetDetectionTool, GetDistanceToObjectsTool # noqa: E402 -from .tools.pcl_detection import ( # noqa: E402 +from .agents import GroundedSamAgent, GroundingDinoAgent +from .tools import GetDetectionTool, GetDistanceToObjectsTool +from .tools.pcl_detection import ( GrippingPointEstimator, GrippingPointEstimatorConfig, PointCloudFilter, PointCloudFilterConfig, PointCloudFromSegmentation, PointCloudFromSegmentationConfig, depth_to_point_cloud, ) -from .tools.pcl_detection_tools import ( # noqa: E402 +from .tools.pcl_detection_tools import ( GetObjectGrippingPointsTool, GetObjectGrippingPointsToolInput, )src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection_tools.py (1)
193-199: Redundantexcept Exception: raiseclause.The bare
except Exception: raiseon lines 198-199 does nothing useful—it catches and immediately re-raises. This can be removed entirely.♻️ Suggested fix
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 '{object_name}' exceeded {self.timeout_sec} seconds" - except Exception: - raise
📜 Review details
Configuration used: Path: .coderabbit.yaml
Review profile: CHILL
Plan: Pro
📒 Files selected for processing (16)
.github/workflows/poetry-test.ymlexamples/manipulation-demo-v2.pypyproject.tomlsrc/rai_core/pyproject.tomlsrc/rai_core/rai/__init__.pysrc/rai_core/rai/tools/__init__.pysrc/rai_core/rai/tools/ros2/manipulation/custom.pysrc/rai_core/rai/tools/timeout.pysrc/rai_extensions/rai_perception/rai_perception/__init__.pysrc/rai_extensions/rai_perception/rai_perception/tools/__init__.pysrc/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.pysrc/rai_extensions/rai_perception/rai_perception/tools/pcl_detection_tools.pytests/conftest.pytests/rai_perception/test_gripping_points.pytests/rai_perception/test_pcl_detection_tools.pytests/tools/test_timeout.py
🧰 Additional context used
🧠 Learnings (3)
📚 Learning: 2025-01-25T14:33:56.413Z
Learnt from: maciejmajek
Repo: RobotecAI/rai PR: 386
File: src/rai/rai/tools/ros/tools.py:30-30
Timestamp: 2025-01-25T14:33:56.413Z
Learning: In the RAI package, ROS2-related Python packages (e.g., tf_transformations) are currently installed via rosdep rather than being declared in pyproject.toml.
Applied to files:
src/rai_core/pyproject.toml
📚 Learning: 2025-01-24T13:42:58.814Z
Learnt from: maciejmajek
Repo: RobotecAI/rai PR: 383
File: tests/tools/ros2/test_topic_tools.py:84-105
Timestamp: 2025-01-24T13:42:58.814Z
Learning: The ROS2 node discovery in the test suite is asynchronous, and the current implementation doesn't wait for topic discovery. This is a known limitation that causes tests like `test_receive_message_tool` and `test_receive_image_tool` to be marked as `xfail`. A proper discovery mechanism with timeout will be implemented in the future to ensure reliable topic communication.
Applied to files:
tests/rai_perception/test_pcl_detection_tools.py
📚 Learning: 2025-04-07T16:16:36.242Z
Learnt from: maciejmajek
Repo: RobotecAI/rai PR: 508
File: src/rai_core/rai/tools/ros2/nav2.py:34-38
Timestamp: 2025-04-07T16:16:36.242Z
Learning: The tools in src/rai_core/rai/tools/ros2/nav2.py are intentionally simplified versions of the more generic ROS2 tools, with a simplified implementation approach that uses global variables.
Applied to files:
src/rai_extensions/rai_perception/rai_perception/__init__.py
🧬 Code graph analysis (7)
tests/rai_perception/test_pcl_detection_tools.py (2)
src/rai_core/rai/communication/ros2/connectors/ros2_connector.py (1)
ROS2Connector(19-20)src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.py (6)
GrippingPointEstimator(365-474)GrippingPointEstimatorConfig(41-62)PointCloudFilter(477-575)PointCloudFilterConfig(65-97)PointCloudFromSegmentation(192-362)PointCloudFromSegmentationConfig(32-38)
src/rai_core/rai/__init__.py (1)
src/rai_core/rai/tools/timeout.py (1)
timeout(49-100)
tests/rai_perception/test_gripping_points.py (3)
src/rai_core/rai/communication/ros2/waiters.py (2)
wait_for_ros2_services(66-107)wait_for_ros2_topics(110-149)src/rai_core/rai/communication/ros2/connectors/ros2_connector.py (1)
ROS2Connector(19-20)src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.py (4)
GrippingPointEstimatorConfig(41-62)PointCloudFilterConfig(65-97)PointCloudFromSegmentationConfig(32-38)_publish_gripping_point_debug_data(115-189)
tests/tools/test_timeout.py (1)
src/rai_core/rai/tools/timeout.py (3)
timeout(49-100)RaiTimeoutError(43-46)timeout_method(103-159)
src/rai_extensions/rai_perception/rai_perception/__init__.py (4)
src/rai_extensions/rai_perception/rai_perception/agents/grounded_sam.py (1)
GroundedSamAgent(29-69)src/rai_extensions/rai_perception/rai_perception/agents/grounding_dino.py (1)
GroundingDinoAgent(26-70)src/rai_extensions/rai_perception/rai_perception/tools/gdino_tools.py (2)
GetDetectionTool(149-174)GetDistanceToObjectsTool(177-264)src/rai_extensions/rai_perception/rai_perception/tools/segmentation_tools.py (1)
depth_to_point_cloud(170-194)
src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.py (5)
src/rai_core/rai/communication/ros2/api/conversion.py (1)
convert_ros_img_to_ndarray(53-82)src/rai_core/rai/communication/ros2/connectors/ros2_connector.py (1)
ROS2Connector(19-20)src/rai_core/rai/communication/ros2/ros_async.py (1)
get_future_result(25-43)src/rai_core/rai/types/sensor.py (2)
Image(39-45)CameraInfo(26-36)src/rai_core/rai/communication/ros2/connectors/base.py (1)
get_transform(432-473)
src/rai_extensions/rai_perception/rai_perception/tools/__init__.py (2)
src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.py (4)
GrippingPointEstimator(365-474)GrippingPointEstimatorConfig(41-62)PointCloudFromSegmentation(192-362)PointCloudFromSegmentationConfig(32-38)src/rai_extensions/rai_perception/rai_perception/tools/segmentation_tools.py (2)
depth_to_point_cloud(170-194)GetSegmentationTool(70-167)
🪛 Ruff (0.14.10)
tests/rai_perception/test_pcl_detection_tools.py
20-20: Unused noqa directive (non-enabled: F401)
Remove unused noqa directive
(RUF100)
22-22: Unused noqa directive (unused: F841)
Remove unused noqa directive
(RUF100)
196-196: Unused function argument: obj_name
(ARG001)
src/rai_core/rai/tools/timeout.py
49-49: PEP 484 prohibits implicit Optional
Convert to T | None
(RUF013)
96-96: Within an except clause, raise exceptions with raise ... from err or raise ... from None to distinguish them from errors in exception handling
(B904)
103-103: PEP 484 prohibits implicit Optional
Convert to T | None
(RUF013)
155-155: Within an except clause, raise exceptions with raise ... from err or raise ... from None to distinguish them from errors in exception handling
(B904)
tests/rai_perception/test_gripping_points.py
15-15: Shebang should be at the beginning of the file
(EXE005)
146-146: Consider moving this statement to an else block
(TRY300)
147-147: Do not catch blind exception: Exception
(BLE001)
183-183: PEP 484 prohibits implicit Optional
Convert to T | None
(RUF013)
184-184: PEP 484 prohibits implicit Optional
Convert to T | None
(RUF013)
185-185: PEP 484 prohibits implicit Optional
Convert to T | None
(RUF013)
186-186: PEP 484 prohibits implicit Optional
Convert to T | None
(RUF013)
288-288: Do not catch blind exception: Exception
(BLE001)
src/rai_extensions/rai_perception/rai_perception/__init__.py
21-21: Unused noqa directive (non-enabled: E402)
Remove unused noqa directive
(RUF100)
22-22: Unused noqa directive (non-enabled: E402)
Remove unused noqa directive
(RUF100)
23-23: Unused noqa directive (non-enabled: E402)
Remove unused noqa directive
(RUF100)
32-32: Unused noqa directive (non-enabled: E402)
Remove unused noqa directive
(RUF100)
src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection_tools.py
116-118: Avoid specifying long messages outside the exception class
(TRY003)
src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.py
225-225: Avoid specifying long messages outside the exception class
(TRY003)
235-235: Avoid specifying long messages outside the exception class
(TRY003)
🔇 Additional comments (23)
src/rai_core/pyproject.toml (1)
7-7: LGTM!Version bump from 2.6.1 to 2.6.2 is appropriate for the new 3D gripping point detection features added in this PR.
src/rai_core/rai/tools/ros2/manipulation/custom.py (1)
19-19: LGTM!Import of the deprecated decorator is correct and appropriate for marking the deprecated tool.
pyproject.toml (1)
79-81: LGTM!The addition of the "manual" pytest marker and its exclusion from default test runs is appropriate for the new manual/integration tests introduced in this PR. The marker description is clear and the test selection logic is correct.
tests/conftest.py (1)
21-31: LGTM!The pytest command-line option and fixture for parameterizing gripping point strategies are implemented correctly. The default "centroid" strategy is sensible, and the implementation follows pytest best practices.
src/rai_core/rai/tools/__init__.py (1)
15-16: LGTM!The re-exports of timeout utilities are implemented correctly using explicit re-export syntax. This properly exposes the new timeout decorators at the package level.
.github/workflows/poetry-test.yml (1)
77-77: LGTM! CI filter correctly excludes manual tests.The addition of
not manualensures that tests requiring manual intervention or special setup are skipped during automated CI runs, which is appropriate.tests/tools/test_timeout.py (1)
21-61: LGTM! Comprehensive timeout decorator tests.The test suite thoroughly validates:
- Successful execution within timeout bounds
- Timeout error with default message formatting (method name, class name, duration)
- Custom timeout messages
- Proper argument and keyword argument handling
src/rai_core/rai/__init__.py (1)
23-23: LGTM! Proper public API exposure.The
timeoutdecorator is correctly imported and exported, making it available as part of the package's public API.Also applies to: 33-33
tests/rai_perception/test_pcl_detection_tools.py (4)
42-66: LGTM! Well-structured depth conversion test.The test validates both the standard conversion algorithm and edge-case handling (zero-depth pixels), with appropriate assertions on output shape and coordinate values.
68-122: LGTM! Comprehensive gripping point estimator tests.The test suite validates multiple estimation strategies (centroid, top_plane) with well-defined geometric test data and properly handles edge cases like empty point clouds.
124-164: LGTM! Thorough filter strategy tests.The test suite validates multiple filtering strategies with realistic noisy data and properly handles edge cases like insufficient points for filtering.
166-195: LGTM! Timeout behavior is well-tested.The test validates both successful fast execution and timeout scenarios with appropriate assertions on the timeout message format.
Also applies to: 199-207
src/rai_core/rai/tools/timeout.py (1)
15-34: LGTM! Excellent design documentation.The design considerations clearly explain the concurrency model, implementation rationale, and alternatives considered. This is valuable context for future maintainers.
examples/manipulation-demo-v2.py (1)
44-99: Well-structured tool initialization.The function properly declares ROS2 parameters and creates configuration objects with documented strategy options. The two-tier configuration approach (ROS2 params for runtime, Pydantic for algorithms) is a clean separation.
src/rai_extensions/rai_perception/rai_perception/tools/__init__.py (1)
16-47: Clean public API expansion.The new PCL detection APIs are properly imported and re-exported via
__all__. The organization is clean with a helpful comment separating the PCL Detection APIs section.tests/rai_perception/test_gripping_points.py (1)
300-331: Manual tests are well-structured.The tests are properly marked with
@pytest.mark.manualand provide good parameterization for different demo scenarios. The docstrings clearly indicate the prerequisites (demo app must be running).src/rai_extensions/rai_perception/rai_perception/__init__.py (1)
15-19: Good centralization of service/node constants.Defining these constants at the top-level
__init__.pywith a clear comment explaining the circular import avoidance is a clean pattern.src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection_tools.py (2)
160-191: Well-implemented timeout handling.The timeout decorator wrapping the inner function is a clean approach. The pipeline steps (segmentation → filter → estimate) are correctly ordered with early returns on empty results. The error message is user-friendly.
100-141: Clear parameter loading with fail-fast validation.Required parameters are validated upfront with a clear error message. Optional parameters have sensible defaults. This is a good pattern for ROS2 tools.
src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.py (4)
100-112: Clean depth-to-point-cloud implementation.Correct pinhole camera model with consistent
np.float32typing throughout. Filters zero-depth points properly. This is a cleaner, typed version compared to the similar function insegmentation_tools.py.
398-442: RANSAC implementation is correct but non-deterministic.The RANSAC plane fitting uses an unseeded
np.random.default_rng()(line 413), producing different results across runs. This is generally acceptable for production but may complicate debugging. Consider exposing an optional seed parameter inGrippingPointEstimatorConfigif reproducibility becomes important.
477-575: Robust point cloud filtering implementation.Good defensive programming throughout:
- All filters have
min_pointsguards- Filters fall back to original points if no valid output
- sklearn imports are lazy (inside methods) to handle missing dependency gracefully
IsolationForestuses fixedrandom_state=42for reproducibility- LOF correctly requires
n_neighbors + 1points minimum (line 545)
318-362: Well-structured segmentation pipeline.The
run()method correctly orchestrates the full pipeline: fetch images → call GDINO → call GSAM → convert depth to point cloud per mask → transform to target frame. Early returns on failures prevent unnecessary processing.
| # Unless required by applicable law or agreed to in writing, software | ||
| # distributed under the License is distributed on an "AS IS" BASIS, | ||
| # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| # See the License for the specific language goveself.rning permissions and |
There was a problem hiding this comment.
Typo in license header.
There's a typo: goveself.rning should be governing.
🔧 Suggested fix
-# See the License for the specific language goveself.rning permissions and
+# See the License for the specific language governing permissions and📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| # See the License for the specific language goveself.rning permissions and | |
| # See the License for the specific language governing permissions and |
🤖 Prompt for AI Agents
In @examples/manipulation-demo-v2.py at line 12, Fix the typo in the license
header comment: replace the incorrect token "goveself.rning" with the correct
word "governing" so the sentence reads "See the License for the specific
language governing permissions and" (update the string in the header comment
accordingly).
| def main(): | ||
| agent = create_agent() | ||
| messages: List[BaseMessage] = [] | ||
|
|
||
| while True: | ||
| prompt = input("Enter a prompt: ") | ||
| messages.append(HumanMessage(content=prompt)) | ||
| output = agent.invoke({"messages": messages}) | ||
| output["messages"][-1].pretty_print() | ||
|
|
||
|
|
||
| if __name__ == "__main__": | ||
| main() |
There was a problem hiding this comment.
Missing ROS2 cleanup on exit.
rclpy.init() is called in create_agent() but there's no corresponding rclpy.shutdown() or connector.shutdown() when the program exits. This can leave ROS2 resources dangling, especially on Ctrl+C.
🔧 Suggested fix
def main():
agent = create_agent()
messages: List[BaseMessage] = []
- while True:
- prompt = input("Enter a prompt: ")
- messages.append(HumanMessage(content=prompt))
- output = agent.invoke({"messages": messages})
- output["messages"][-1].pretty_print()
+ try:
+ while True:
+ prompt = input("Enter a prompt: ")
+ messages.append(HumanMessage(content=prompt))
+ output = agent.invoke({"messages": messages})
+ output["messages"][-1].pretty_print()
+ except KeyboardInterrupt:
+ print("\nExiting...")
+ finally:
+ rclpy.shutdown()🤖 Prompt for AI Agents
In @examples/manipulation-demo-v2.py around lines 133 - 145, The program
initializes ROS2 inside create_agent() via rclpy.init() but never calls
rclpy.shutdown() or connector.shutdown() on exit; wrap the interactive loop in a
try/except KeyboardInterrupt/finally (or use try/finally) and in the finally
block call the agent's connector.shutdown() (or the connector instance
returned/owned by create_agent) and rclpy.shutdown() to ensure ROS2 resources
are cleaned up when the user exits (Ctrl+C) or the program terminates.
| ) | ||
|
|
||
|
|
||
| @deprecated("Use GetObjectGrippingPointsTool from rai_perception instead") |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# Check the deprecation line and surrounding context
echo "=== Checking deprecation decorator at line 263 ==="
sed -n '260,265p' src/rai_core/rai/tools/ros2/manipulation/custom.py
echo -e "\n=== Checking imports at lines 35-39 ==="
sed -n '35,39p' src/rai_core/rai/tools/ros2/manipulation/custom.py
echo -e "\n=== Searching for class definitions in rai_perception ==="
rg -n --type=py "class Get.*(?:Gripping|Grabbing).*Tool" src/rai_extensions/rai_perception/
echo -e "\n=== Searching for exported names from rai_perception ==="
rg -n --type=py "from.*Get.*(?:Gripping|Grabbing).*Tool" src/rai_extensions/rai_perception/
echo -e "\n=== Searching broadly for tool classes in rai_perception ==="
rg -n --type=py "class Get.*Tool" src/rai_extensions/rai_perception/ | grep -i "gripp\|grab"Repository: RobotecAI/rai
Length of output: 1582
Update the deprecation message to reference the correct exported tool.
The deprecation message references "GetObjectGrippingPointsTool" but this tool is not exported from rai_perception. The exported tool is "GetGrabbingPointTool" (as shown in line 35-39 imports and rai_perception's init.py). Update the decorator to: @deprecated("Use GetGrabbingPointTool from rai_perception instead")
🤖 Prompt for AI Agents
In @src/rai_core/rai/tools/ros2/manipulation/custom.py at line 263, The
deprecation decorator string is pointing to the wrong exported tool; update the
@deprecated decorator on the affected symbol to say "Use GetGrabbingPointTool
from rai_perception instead" (replace the current "GetObjectGrippingPointsTool"
text) so it correctly references the exported tool name used in the imports and
rai_perception's __init__.py; locate the @deprecated(...) decorator above the
relevant function/class in custom.py and change only the message text.
| def _publish_gripping_point_debug_data( | ||
| connector: ROS2Connector, | ||
| obj_points_xyz: NDArray[np.float32], | ||
| gripping_points_xyz: list[NDArray[np.float32]], | ||
| base_frame_id: str = "egoarm_base_link", | ||
| publish_duration: float = 5.0, | ||
| ) -> None: |
There was a problem hiding this comment.
Incorrect type hint for obj_points_xyz.
The parameter obj_points_xyz is typed as NDArray[np.float32] but the code treats it as a list of arrays (line 146 uses np.concatenate(obj_points_xyz, axis=0)). The type should match the actual usage.
🔧 Suggested fix
def _publish_gripping_point_debug_data(
connector: ROS2Connector,
- obj_points_xyz: NDArray[np.float32],
+ obj_points_xyz: list[NDArray[np.float32]],
gripping_points_xyz: list[NDArray[np.float32]],
base_frame_id: str = "egoarm_base_link",
publish_duration: float = 5.0,
) -> None:🤖 Prompt for AI Agents
In @src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.py
around lines 115 - 121, The type hint for obj_points_xyz in
_publish_gripping_point_debug_data is incorrect: change it from
NDArray[np.float32] to a sequence type that reflects how it is used (e.g.,
Sequence[NDArray[np.float32]] or List[NDArray[np.float32]]) so calls like
np.concatenate(obj_points_xyz, axis=0) are type-correct; update the function
signature accordingly, add the necessary typing import (from typing import
Sequence or List), and adjust any related docstring/comments to reflect that
obj_points_xyz is a list/sequence of arrays.
| """Publish the gripping point debug data to ROS2 topics which can be visualized in RVIZ. | ||
| Args: | ||
| connector: The ROS2 connector. | ||
| obj_points_xyz: The list of objects found in the image. | ||
| gripping_points_xyz: The list of gripping points in the base frame. | ||
| base_frame_id: The base frame id. | ||
| publish_duration: Duration in seconds to publish the data (default: 10.0). | ||
| """ |
There was a problem hiding this comment.
Docstring default mismatch.
The docstring says publish_duration default is 10.0, but the actual default is 5.0.
🔧 Suggested fix
- publish_duration: Duration in seconds to publish the data (default: 10.0).
+ publish_duration: Duration in seconds to publish the data (default: 5.0).📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| """Publish the gripping point debug data to ROS2 topics which can be visualized in RVIZ. | |
| Args: | |
| connector: The ROS2 connector. | |
| obj_points_xyz: The list of objects found in the image. | |
| gripping_points_xyz: The list of gripping points in the base frame. | |
| base_frame_id: The base frame id. | |
| publish_duration: Duration in seconds to publish the data (default: 10.0). | |
| """ | |
| """Publish the gripping point debug data to ROS2 topics which can be visualized in RVIZ. | |
| Args: | |
| connector: The ROS2 connector. | |
| obj_points_xyz: The list of objects found in the image. | |
| gripping_points_xyz: The list of gripping points in the base frame. | |
| base_frame_id: The base frame id. | |
| publish_duration: Duration in seconds to publish the data (default: 5.0). | |
| """ |
🤖 Prompt for AI Agents
In @src/rai_extensions/rai_perception/rai_perception/tools/pcl_detection.py
around lines 122 - 130, The docstring for the function that publishes gripping
point debug data is out of sync: it states publish_duration defaults to 10.0
while the actual default value is 5.0; update the docstring to reflect the true
default (publish_duration=5.0) or change the function signature to use 10.0 so
they match (adjust the parameter named publish_duration in the publishing
function in pcl_detection.py and update its docstring accordingly).
| def _call_gdino_node( | ||
| self, camera_img_message: sensor_msgs.msg.Image, object_name: str | ||
| ) -> Future: | ||
| cli = self.connector.node.create_client(RAIGroundingDino, GDINO_SERVICE_NAME) # type: ignore[reportUnknownMemberType] | ||
| while not cli.wait_for_service(timeout_sec=1.0): | ||
| self.connector.node.get_logger().info( # type: ignore[reportUnknownMemberType] | ||
| f"service {GDINO_SERVICE_NAME} not available, waiting again..." | ||
| ) | ||
| req = RAIGroundingDino.Request() | ||
| req.source_img = camera_img_message | ||
| req.classes = object_name | ||
| req.box_threshold = self.config.box_threshold | ||
| req.text_threshold = self.config.text_threshold | ||
| return cli.call_async(req) |
There was a problem hiding this comment.
Service wait loop may block indefinitely.
The while not cli.wait_for_service(timeout_sec=1.0) loop (lines 251-254) has no overall timeout or maximum retry count. If the service never becomes available, this blocks forever. The same pattern appears in _call_gsam_node (lines 266-269).
🔧 Consider adding a max retry count or overall timeout
def _call_gdino_node(
self, camera_img_message: sensor_msgs.msg.Image, object_name: str
) -> Future:
cli = self.connector.node.create_client(RAIGroundingDino, GDINO_SERVICE_NAME)
- while not cli.wait_for_service(timeout_sec=1.0):
+ max_retries = 30 # 30 seconds max wait
+ retries = 0
+ while not cli.wait_for_service(timeout_sec=1.0):
+ retries += 1
+ if retries >= max_retries:
+ raise RuntimeError(f"Service {GDINO_SERVICE_NAME} not available after {max_retries} seconds")
self.connector.node.get_logger().info(
f"service {GDINO_SERVICE_NAME} not available, waiting again..."
)📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| def _call_gdino_node( | |
| self, camera_img_message: sensor_msgs.msg.Image, object_name: str | |
| ) -> Future: | |
| cli = self.connector.node.create_client(RAIGroundingDino, GDINO_SERVICE_NAME) # type: ignore[reportUnknownMemberType] | |
| while not cli.wait_for_service(timeout_sec=1.0): | |
| self.connector.node.get_logger().info( # type: ignore[reportUnknownMemberType] | |
| f"service {GDINO_SERVICE_NAME} not available, waiting again..." | |
| ) | |
| req = RAIGroundingDino.Request() | |
| req.source_img = camera_img_message | |
| req.classes = object_name | |
| req.box_threshold = self.config.box_threshold | |
| req.text_threshold = self.config.text_threshold | |
| return cli.call_async(req) | |
| def _call_gdino_node( | |
| self, camera_img_message: sensor_msgs.msg.Image, object_name: str | |
| ) -> Future: | |
| cli = self.connector.node.create_client(RAIGroundingDino, GDINO_SERVICE_NAME) # type: ignore[reportUnknownMemberType] | |
| max_retries = 30 # 30 seconds max wait | |
| retries = 0 | |
| while not cli.wait_for_service(timeout_sec=1.0): | |
| retries += 1 | |
| if retries >= max_retries: | |
| raise RuntimeError(f"Service {GDINO_SERVICE_NAME} not available after {max_retries} seconds") | |
| self.connector.node.get_logger().info( # type: ignore[reportUnknownMemberType] | |
| f"service {GDINO_SERVICE_NAME} not available, waiting again..." | |
| ) | |
| req = RAIGroundingDino.Request() | |
| req.source_img = camera_img_message | |
| req.classes = object_name | |
| req.box_threshold = self.config.box_threshold | |
| req.text_threshold = self.config.text_threshold | |
| return cli.call_async(req) |
| # See the License for the specific language governing permissions and | ||
| # limitations under the License. | ||
|
|
||
| #!/usr/bin/env python3 |
There was a problem hiding this comment.
Shebang should be at the beginning of the file.
The shebang #!/usr/bin/env python3 appears after the license header. If this file is intended to be executable, the shebang must be on line 1.
🔧 Suggested fix
Move the shebang to line 1, before the license header:
+#!/usr/bin/env python3
# Copyright (C) 2025 Julia Jia
#
# Licensed under the Apache License, Version 2.0 (the "License");
...
-#!/usr/bin/env python3
"""🧰 Tools
🪛 Ruff (0.14.10)
15-15: Shebang should be at the beginning of the file
(EXE005)
🤖 Prompt for AI Agents
In @tests/rai_perception/test_gripping_points.py at line 15, The shebang line
"#!/usr/bin/env python3" must be the very first line of the file; move that
shebang from its current position after the license header to line 1 (before any
comments or license text) so the interpreter is correctly recognized when the
file is executed.
Purpose
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:
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) togenerate 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 operationsThe 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.pyand integration/manual teststests/tools/ros2/test_gripping_points.pyUpdated examples/manipulation-demo-v2.py with the new configurable detection tool. We can remove after review.
Issues
Testing
To run unit tests
To run manual tests
Manipulation-demo-v2 has been run to validate the change.
TODOs
Summary by CodeRabbit
New Features
Deprecations
Updates
✏️ Tip: You can customize this high-level summary in your review settings.