Add region-coverage-path demo for GetRegionFromUser (wrist camera) - #835
Open
L4co77 wants to merge 1 commit into
Open
Add region-coverage-path demo for GetRegionFromUser (wrist camera)#835L4co77 wants to merge 1 commit into
L4co77 wants to merge 1 commit into
Conversation
📝 WalkthroughSummary by CodeRabbit
WalkthroughAdds two behavior-tree nodes for region-mask extraction and raster coverage-path generation. Registers them with the behavior factory and uses them in a new Select Region and Cover objective with planning, approval, execution, and Home recovery. ChangesRegion selection and surface coverage
Possibly related PRs
Suggested reviewers: Caution Pre-merge checks failedPlease resolve all errors before merging. Addressing warnings is optional.
❌ Failed checks (1 error)
✅ Passed checks (3 passed)
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: 1
🧹 Nitpick comments (2)
src/lab_sim_behaviors/include/lab_sim_behaviors/generate_surface_coverage_path.hpp (1)
43-44: 📐 Maintainability & Code Quality | 🔵 Trivial | 💤 Low valueDocument the remaining rejection cases.
generateRasterPathalso returns an empty vector for non-finite inputs, a negativemargin, and a negativestandoff. Add these cases so the contract matchessrc/lab_sim_behaviors/src/generate_surface_coverage_path.cpplines 56-66.📝 Proposed doc update
- * Returns an empty vector when the usable area (after `@p` margin) is non-positive, a spacing is - * non-positive, or the region/spacing combination would exceed the internal pose cap. + * Returns an empty vector when any input is non-finite, the usable area (after `@p` margin) is + * non-positive, a spacing is non-positive, `@p` margin or `@p` standoff is negative, or the + * region/spacing combination would exceed the internal pose cap.🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@src/lab_sim_behaviors/include/lab_sim_behaviors/generate_surface_coverage_path.hpp` around lines 43 - 44, Update the documentation for generateRasterPath to state that it also returns an empty vector when any input is non-finite, or when margin or standoff is negative. Keep the existing rejection cases documented so the contract matches the implementation.src/lab_sim_behaviors/test/test_generate_surface_coverage_path.cpp (1)
152-153: 📐 Maintainability & Code Quality | 🔵 Trivial | 💤 Low valueCorrect the comment: the implementation uses
ceil, notfloor.
generateRasterPathcomputes the counts withstd::ceil. The values match here only because 0.2/0.05 and 0.4/0.02 divide evenly. The wrong operator in the comment contradicts theCoversFarEdgeWhenSpacingDoesNotDivideEvenlytest.📝 Proposed comment fix
- // THEN it is a full grid: floor(0.2/0.05)+1 = 5 lines x floor(0.4/0.02)+1 = 21 points + // THEN it is a full grid: ceil(0.2/0.05)+1 = 5 lines x ceil(0.4/0.02)+1 = 21 points🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@src/lab_sim_behaviors/test/test_generate_surface_coverage_path.cpp` around lines 152 - 153, Correct the explanatory comment above the path size assertion to describe the grid counts using ceil rather than floor, while preserving the existing 5-by-21 calculation and ASSERT_THAT(path, SizeIs(105)) behavior.
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/lab_sim_behaviors/src/get_mask2d_from_region.cpp`:
- Around line 89-98: Update the polygon rasterization in regionToMask2D to use
cv::fillPoly so arbitrary, including concave, regions are filled correctly;
retain the existing local-coordinate construction and mask behavior, and add
coverage for a concave region plus documentation of the accepted polygon
contract.
---
Nitpick comments:
In
`@src/lab_sim_behaviors/include/lab_sim_behaviors/generate_surface_coverage_path.hpp`:
- Around line 43-44: Update the documentation for generateRasterPath to state
that it also returns an empty vector when any input is non-finite, or when
margin or standoff is negative. Keep the existing rejection cases documented so
the contract matches the implementation.
In `@src/lab_sim_behaviors/test/test_generate_surface_coverage_path.cpp`:
- Around line 152-153: Correct the explanatory comment above the path size
assertion to describe the grid counts using ceil rather than floor, while
preserving the existing 5-by-21 calculation and ASSERT_THAT(path, SizeIs(105))
behavior.
🪄 Autofix
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro Plus
Run ID: d776a562-4fae-44e0-be3b-4bad538df6e4
📒 Files selected for processing (13)
src/lab_sim/objectives/select_region_and_cover.xmlsrc/lab_sim/test/objectives_integration_test.pysrc/lab_sim_behaviors/CMakeLists.txtsrc/lab_sim_behaviors/include/lab_sim_behaviors/generate_surface_coverage_path.hppsrc/lab_sim_behaviors/include/lab_sim_behaviors/get_mask2d_from_region.hppsrc/lab_sim_behaviors/package.xmlsrc/lab_sim_behaviors/src/generate_surface_coverage_path.cppsrc/lab_sim_behaviors/src/get_mask2d_from_region.cppsrc/lab_sim_behaviors/src/register_behaviors.cppsrc/lab_sim_behaviors/test/CMakeLists.txtsrc/lab_sim_behaviors/test/test_behavior_plugins.cppsrc/lab_sim_behaviors/test/test_generate_surface_coverage_path.cppsrc/lab_sim_behaviors/test/test_get_mask2d_from_region.cpp
|
Member
|
See my feedback in the previous PR |
Author
select-2026-08-07_18.28.26.mp4 |
Draw a box on the wrist camera over a flat surface; the region is lifted to 3D, fitted to an oriented box, and swept with a serpentine coverage path (spray/wipe feel), then planned, approved, and executed, ending with a move back to Home. New lab_sim_behaviors: GetMask2DFromRegion (rasterizes the normalized [0..1] region into a Mask2D via cv::fillPoly) and GenerateSurfaceCoveragePath (boustrophedon raster over the fitted box top face at a configurable standoff). Geometry is in pure free functions with unit tests. CreatePoseStamped orientation_xyzw=1;0;0;0 keeps the OBB reference +Z outward so the coverage renders on the surface, not under it. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
L4co77
force-pushed
the
feat/region-coverage-path-demo
branch
from
August 7, 2026 16:49
a4e4320 to
b3ab65c
Compare
|
|
Member
|
I'm really excited about this example application!
|
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
[written by AI]
Changes since #819
CreatePoseStampedsetsorientation_xyzw="1;0;0;0"so the OBBreference_pose+Z is the outward surface normal (addresses @davetcoleman's "the green grid line seems to be under the table" note).Homeat the end — added aMove to Waypoint → Homestep so the demo resets for the next run (addresses @davetcoleman's "move back to the home position to restart it" note).Screencast.from.2026-08-07.13-57-37.webm
Demo requested by @davetcoleman on PR #20768 (GetRegionFromUser): rather than an orphan feature, an objective in lab sim that uses the wrist camera to let the user draw a bounding box and then runs a coverage sweep over that region.
What this adds
select_region_and_cover.xml— the user draws a box on the wrist camera image; the box is lifted to 3D and swept with a serpentine coverage path, then planned and executed with approval.GetMask2DFromRegion(lab_sim_behaviors) — rasterizes thePolygonStampedregion fromGetRegionFromUserinto aMask2D.GetRegionFromUserreturns normalized [0..1] image coordinates, so this behavior takescamera_infoand denormalizes them to pixels before filling the mask, so it aligns with the point-cloud projection inGetMasks3DFromMasks2D.GenerateSurfaceCoveragePath(lab_sim_behaviors) — pure geometry: a boustrophedon (serpentine) raster of tool poses over the top face of the fitted oriented box, at a configurable standoff/line/point spacing. NamedSurface...deliberately — core MoveIt Pro already ships aGenerateCoveragePath(a corner+area lawnmower); this one is OBB-driven with a standoff and tool-into-surface orientation.Pipeline
SwitchUIPrimaryView→SwitchController(activate JTAC) →GetRegionFromUser→GetPointCloud/GetCameraInfo→GetMask2DFromRegion→GetMasks3DFromMasks2D→GetPointCloudFromMask3D→GetOrientedBoundingBoxFromPointCloud→GenerateSurfaceCoveragePath→PlanCartesianPath→WaitForJointTrajectoryApproval→ExecuteTrajectory.The 2D→3D chain mirrors the shipped
ml_auto_grasp_object_from_clicked_point.xml, swapping the SAM click for the drawn region.GetMask2DFromRegionandGenerateSurfaceCoveragePathare the only new behaviors; everything else already exists. The oriented box'sreference_poseis an identity pose in the wrist-camera frame (CreatePoseStamped), which disambiguates the OBB orientation against the camera axes.Approach / why
The geometry is extracted into pure free functions (
regionToMask2D,generateRasterPath) and unit-tested in isolation — thetick()methods are thin I/O wrappers. This keeps the coverage math deterministic and testable without a running stack.regionToMask2Dusescv::fillConvexPoly, valid becauseGetRegionFromUserreturns a convex box.Testing
test_behavior_plugins.cppextended so pluginlib load is verified for both new behaviors.skip_objectivesinobjectives_integration_test.py(it needs an interactive UI prompt, likeMarker Visualization Example/Teleoperate).Live verification
Run end-to-end in
lab_sim(mock hardware) against a #20768-built backend: draw a box on the wrist camera → the full pipeline recovers the 3D region, fits an OBB, generates the coverage path, andPlanCartesianPathplans it into an approvable ~1300-point trajectory thatExecuteTrajectoryruns. Live testing surfaced and fixed four issues the objective-skipping CI could not: the reference-pose behavior name, the normalized-coordinate denormalization above,point_spacingvsblending_radius, and activating the admittance controller before execution.Limitations (demo, not production)
GenerateSurfaceCoveragePathassumes the fitted box's +Z is the outward surface normal. A PCA-fit OBB does not guarantee axis assignment or sign; the camera-framereference_poseis the only disambiguator today.line_spacing/point_spacing/standoff) are tuned forlab_sim.