Add collision_monitor lab_sim integration (key-gated, parallel objectives test)#718
Add collision_monitor lab_sim integration (key-gated, parallel objectives test)#718MikeWrock wants to merge 15 commits into
Conversation
unfavorite kinova_sim Velocity Force Controller Zero, and zero fts
Replace `GetMasks2DFromTextQuery` (CLIPSeg) with `GetMasks2DFromExemplar`
(SAM3 multimodal segmenter) in the `ML Move Boxes to Loading Zone`
Objective and its inner Subtree. SAM3 also accepts a text prompt but
returns SUCCESS with `mask_count=0` on empty detections, letting the
Subtree skip the downstream pick pipeline cleanly with `_skipIf`.
Restructure the outer loop from the `RetryUntilSuccessful + ForceFailure`
infinite-loop pattern to
`Fallback { RepeatUnlessFailureEachTick { ... ScriptCondition(!drained) }, ScriptCondition(drained) }`
so the loop exits with Objective SUCCESS when the scene is drained but
surfaces real pick errors as Objective FAILURE.
Fixes #19112.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
…Boxes
Updates the View Boxes text prompt from "brown cube" to
"a single small orange cube box" to better describe the sim assets,
then guards the SAM3 output with FilterMasks2DByArea so the
downstream pick pipeline only sees plausibly-sized box masks:
the cart frame measured ~26,285 px in calibration and was the
primary false positive driving wasted pick attempts; thin
edge-fragment masks measured ~778 px. Real boxes in this scene
measure 3,949-20,643 px, so thresholds are min=2,000 / max=23,000.
GetSizeOfVector refreshes {mask_count} after the filter so the
existing `_skipIf="mask_count == 0"` gate reflects the filtered
count rather than SAM3's raw count.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
…-filter Fix: Filter SAM3 mask false positives by area in hangar_sim
…sam3 Fix: Use SAM3 instead of CLIPSeg in ML Move Boxes Objective
bump: phoebe_ws to get fixes
add a wait to give TF time to be available
Pulls in PickNikRobotics/picknik_accessories#45 — makes the lab_sim burner pushable under realistic gripper force.
…ries-burner-v9.3 Fix: Bump picknik_accessories for lab_sim burner movable fix
Adds src/lab_sim_collision_monitor: a usage example that configures the vendor-neutral, separately-licensed collision_monitor node for lab_sim (UR-on-a-linear-rail). Robot specifics live only in launch/params. - config: StateProvider <- /joint_states (model-DOF order; lab_sim reports driving joint angles directly, no actuator conversion). default_accel_limit set to the arm's 30 deg/s^2 spec; rail uses its URDF accel limit. - launch: ComposableNodeContainer loading the licensed collision_monitor_ros::CollisionMonitorComponent (from the pre-bundled binary overlay/image, NOT built here). - test: parallel-monitor Layer-2 integration test reusing the objectives harness. Asserts BOTH (a) no false trips during a safe objective and (b) a deliberate self-collision pose MUST trip (PROJECTED_COLLISION). Doubles as the license gate: without MOVEIT_LICENSE_KEY the monitor dies on init and the test fails loudly. TripSink demo halts the JTC on trip. Adds .github/workflows/collision-monitor-integration.yaml: downstream Layer-2 workflow triggered by repository_dispatch (collision-monitor-integration) + workflow_dispatch. PULLS the licensed bundle image from client_payload.bundle_image, supplies MOVEIT_LICENSE_KEY from STUDIO_CI_LICENSE_KEY, builds the example overlay on top of the bundle, and runs the Layer-2 test. It never clones or builds collision_monitor source (this repo is public; the monitor is proprietary). Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
📝 WalkthroughSummary by CodeRabbit
WalkthroughAdds a new Changeslab_sim_collision_monitor Package and CI Workflow
Hangar Sim Behavior Tree Objective Updates
Kinova Sim Objective Wiring Updates
Submodule Pointer Updates
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)
✨ Finishing Touches⚔️ Resolve merge conflicts
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: 3
🤖 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 @.github/workflows/collision-monitor-integration.yaml:
- Around line 143-157: The docker run command in the colcon test execution is
losing test results because the container is being removed (--rm flag) after it
exits, making artifacts unavailable for the subsequent upload step. Add a volume
mount flag to bind the host workspace directory to the container filesystem so
that test results are accessible from the host after the container exits.
Additionally, ensure that the colcon test results are being written to a
location within the mounted volume so they persist after the container
terminates and can be collected by the artifact upload step.
- Around line 86-90: Disable credential persistence in the checkout step named
"Checkout example_ws (usage example only — NO monitor source)" by adding the
persist-credentials parameter set to false in the with block of the
actions/checkout action. This prevents unnecessary retention of Git credentials
since no git operations occur after this checkout step.
In `@src/lab_sim_collision_monitor/test/collision_monitor_integration_test.py`:
- Around line 316-321: After the rclpy.spin_until_future_complete call completes
for the goal_future returned by _send_arm_trajectory, retrieve the goal handle
result from the future and add an assertion to verify it is not None. This
ensures that if the action server rejects the goal, the test immediately fails
with a clear assertion error rather than misattributing the failure to collision
monitoring issues later in the test.
🪄 Autofix (Beta)
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: faffd8d1-e194-4d63-bbe0-f027ecf60580
📒 Files selected for processing (15)
.github/workflows/collision-monitor-integration.yamlsrc/external_dependencies/phoebe_wssrc/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xmlsrc/hangar_sim/objectives/move_boxes_looping.xmlsrc/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xmlsrc/lab_sim_collision_monitor/CMakeLists.txtsrc/lab_sim_collision_monitor/README.mdsrc/lab_sim_collision_monitor/config/lab_sim_collision_monitor.yamlsrc/lab_sim_collision_monitor/launch/collision_monitor.launch.pysrc/lab_sim_collision_monitor/package.xmlsrc/lab_sim_collision_monitor/test/collision_monitor_integration_test.pysrc/lab_sim_collision_monitor/test/conftest.pysrc/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xmlsrc/moveit_pro_kinova_configs/kinova_sim/objectives/write_picknik.xmlsrc/picknik_accessories
| - name: Checkout example_ws (usage example only — NO monitor source) | ||
| uses: actions/checkout@de0fac2e4500dabe0009e67214ff5f5447ce83dd # v6.0.2 | ||
| with: | ||
| submodules: recursive | ||
|
|
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
cat -n .github/workflows/collision-monitor-integration.yamlRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 8394
Disable persisted Git credentials for checkout.
This workflow does not perform any git operations after checkout (no push, pull, or clone), so persisting credentials is unnecessary security exposure.
Proposed fix
- name: Checkout example_ws (usage example only — NO monitor source)
uses: actions/checkout@de0fac2e4500dabe0009e67214ff5f5447ce83dd # v6.0.2
with:
submodules: recursive
+ persist-credentials: false📝 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.
| - name: Checkout example_ws (usage example only — NO monitor source) | |
| uses: actions/checkout@de0fac2e4500dabe0009e67214ff5f5447ce83dd # v6.0.2 | |
| with: | |
| submodules: recursive | |
| - name: Checkout example_ws (usage example only — NO monitor source) | |
| uses: actions/checkout@de0fac2e4500dabe0009e67214ff5f5447ce83dd # v6.0.2 | |
| with: | |
| submodules: recursive | |
| persist-credentials: false |
🧰 Tools
🪛 zizmor (1.25.2)
[warning] 86-89: credential persistence through GitHub Actions artifacts (artipacked): does not set persist-credentials: false
(artipacked)
🤖 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 @.github/workflows/collision-monitor-integration.yaml around lines 86 - 90,
Disable credential persistence in the checkout step named "Checkout example_ws
(usage example only — NO monitor source)" by adding the persist-credentials
parameter set to false in the with block of the actions/checkout action. This
prevents unnecessary retention of Git credentials since no git operations occur
after this checkout step.
Source: Linters/SAST tools
| docker run --rm \ | ||
| -e MOVEIT_CONFIG_PACKAGE=lab_sim \ | ||
| -e MOVEIT_LICENSE_KEY="${MOVEIT_LICENSE_KEY}" \ | ||
| -e MUJOCO_CI_TIMESTEP=0.003 \ | ||
| "${BUNDLE_IMAGE}" \ | ||
| bash -lc ' | ||
| set -euo pipefail | ||
| source /opt/ros/humble/setup.bash | ||
| source "${USER_WS}/install/setup.bash" | ||
| colcon test \ | ||
| --packages-select lab_sim_collision_monitor \ | ||
| --executor sequential \ | ||
| --event-handlers console_direct+ | ||
| colcon test-result --verbose | ||
| ' |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
cd /tmp && find . -name "collision-monitor-integration.yaml" -type f 2>/dev/null | head -5Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 63
🏁 Script executed:
# First, let's locate the workflow file in the repository
git ls-files | grep -i collision-monitor-integrationRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 134
🏁 Script executed:
# Get the workflow file and examine it around the specified lines
cat -n .github/workflows/collision-monitor-integration.yaml | sed -n '130,175p'Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 1842
Add volume mount and copy test results from container to host filesystem.
Test results written inside the container during colcon test are lost when docker run --rm exits, so the artifact upload step has nothing to collect. Bind mount the host workspace and copy results out after test completion.
Proposed fix
- name: Run the Layer-2 parallel-monitor test
env:
BUNDLE_IMAGE: lab-sim-with-monitor:ci
MOVEIT_LICENSE_KEY: ${{ secrets.STUDIO_CI_LICENSE_KEY }}
run: |
set -euo pipefail
+ mkdir -p build/lab_sim_collision_monitor/test_results log/latest_test
# Headless, license-keyed run of lab_sim + the licensed monitor.
# Coarsen the MuJoCo timestep on CI (matches ci.yaml) so the heavier
# 3.6.0 solver stays at-or-under realtime on the runner.
docker run --rm \
+ -v "${PWD}:/host_ws" \
-e MOVEIT_CONFIG_PACKAGE=lab_sim \
-e MOVEIT_LICENSE_KEY="${MOVEIT_LICENSE_KEY}" \
-e MUJOCO_CI_TIMESTEP=0.003 \
"${BUNDLE_IMAGE}" \
bash -lc '
set -euo pipefail
source /opt/ros/humble/setup.bash
source "${USER_WS}/install/setup.bash"
colcon test \
--packages-select lab_sim_collision_monitor \
--executor sequential \
--event-handlers console_direct+
colcon test-result --verbose
+ cp -r "${USER_WS}/build/lab_sim_collision_monitor/test_results/." \
+ /host_ws/build/lab_sim_collision_monitor/test_results/ || true
+ cp -r "${USER_WS}/log/latest_test/." /host_ws/log/latest_test/ || true
'Also applies to: 159-167
🤖 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 @.github/workflows/collision-monitor-integration.yaml around lines 143 - 157,
The docker run command in the colcon test execution is losing test results
because the container is being removed (--rm flag) after it exits, making
artifacts unavailable for the subsequent upload step. Add a volume mount flag to
bind the host workspace directory to the container filesystem so that test
results are accessible from the host after the container exits. Additionally,
ensure that the colcon test results are being written to a location within the
mounted volume so they persist after the container terminates and can be
collected by the artifact upload step.
| goal_future = _send_arm_trajectory( | ||
| monitor_probe.node, SELF_COLLISION_POSE, move_time_s=4.0 | ||
| ) | ||
| rclpy.spin_until_future_complete( | ||
| monitor_probe.node, goal_future, timeout_sec=30.0 | ||
| ) |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
# First, let's check the file and read the specific lines
wc -l src/lab_sim_collision_monitor/test/collision_monitor_integration_test.pyRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 158
🏁 Script executed:
# Read the target lines with context
sed -n '310,330p' src/lab_sim_collision_monitor/test/collision_monitor_integration_test.pyRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 896
🏁 Script executed:
# Find the _send_arm_trajectory function definition
rg "_send_arm_trajectory" src/lab_sim_collision_monitor/test/collision_monitor_integration_test.py -B 2 -A 10Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 1131
🏁 Script executed:
# Search for patterns of goal_handle usage and goal acceptance checks in the test file
rg "goal_handle|\.accepted|\.result()" src/lab_sim_collision_monitor/test/collision_monitor_integration_test.pyRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 63
🏁 Script executed:
# Get the full _send_arm_trajectory implementation
rg "_send_arm_trajectory" src/lab_sim_collision_monitor/test/collision_monitor_integration_test.py -A 25Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 2208
🏁 Script executed:
# Search for any goal acceptance validation patterns in the entire test file
rg "accepted|goal_handle" src/lab_sim_collision_monitor/test/collision_monitor_integration_test.pyRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 63
🏁 Script executed:
# Check what type of future is returned and how it's used elsewhere
sed -n '1,100p' src/lab_sim_collision_monitor/test/collision_monitor_integration_test.py | head -50Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 2920
🏁 Script executed:
# Search for other uses of send_goal_async in test files to see if goal acceptance is validated elsewhere
rg "send_goal_async|goal_handle|\.accepted" src/lab_sim_collision_monitor/test/ -B 2 -A 3Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 635
🏁 Script executed:
# Check if there are other integration tests that might validate goal acceptance
find src/lab_sim_collision_monitor/test -name "*.py" -type fRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 201
🏁 Script executed:
# Look for FollowJointTrajectory usage patterns in the codebase
rg "FollowJointTrajectory|ActionClient.*FollowJointTrajectory" --type py -B 2 -A 8Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 3143
🏁 Script executed:
# Check if goal_handle.result() is called or used anywhere after the spin_until_future_complete
sed -n '316,350p' src/lab_sim_collision_monitor/test/collision_monitor_integration_test.pyRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 1303
🏁 Script executed:
# Look for patterns of goal validation in other ROS 2 action client code in the repo
rg "goal_future|send_goal_async" src/ -B 2 -A 5 --type py | head -60Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 2076
🏁 Script executed:
# Verify the ROS 2 ActionClient API - check what send_goal_async returns
web_search ROS 2 ActionClient send_goal_async goal_handle future API documentationRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 132
🏁 Script executed:
# Check the actual behavior - read more of the context around the test
sed -n '290,350p' src/lab_sim_collision_monitor/test/collision_monitor_integration_test.pyRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 2209
🏁 Script executed:
# Check if there's documentation or examples in the codebase about goal handle validation
rg "goal_handle\|\.accepted" src/ --type py -B 1 -A 1Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 63
🏁 Script executed:
# Look at the test structure more broadly - how does the test detect failures
sed -n '300,360p' src/lab_sim_collision_monitor/test/collision_monitor_integration_test.pyRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 1930
🏁 Script executed:
# Check ROS 2 rclpy ActionClient behavior by looking at imports and usage
head -50 src/lab_sim_collision_monitor/test/collision_monitor_integration_test.py | grep -E "import|from"Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 158
🏁 Script executed:
# Get the full imports section
sed -n '1,80p' src/lab_sim_collision_monitor/test/collision_monitor_integration_test.py | grep -E "^import|^from"Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 911
🏁 Script executed:
# Search for ActionClient import
rg "ActionClient" src/lab_sim_collision_monitor/test/collision_monitor_integration_test.pyRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 186
🏁 Script executed:
# Let's check the ROS 2 documentation about send_goal_async by looking for related patterns
web_search
ROS 2 rclpy ActionClient send_goal_async returns GoalHandle future acceptedRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 174
🌐 Web query:
ROS 2 rclpy ActionClient send_goal_async GoalHandle future accepted API
💡 Result:
In ROS 2, the rclpy ActionClient's send_goal_async method is used to send a goal to an action server asynchronously [1][2]. It returns a Future object that completes when the action server acknowledges the request (either by accepting or rejecting it) [1][3]. When the future completes, its result is a ClientGoalHandle instance [1][2]. You can check if the goal was successfully accepted by the action server by accessing the accepted property on this ClientGoalHandle [1][3][4]. Example usage: def goal_response_callback(self, future): goal_handle = future.result if not goal_handle.accepted: self.get_logger.info('Goal rejected') return self.get_logger.info('Goal accepted') # Proceed to get result or monitor progress via goal_handle [5][6] Key points: - Return Type: send_goal_async returns an rclpy.task.Future [1][7]. - Future Resolution: The future resolves to a ClientGoalHandle [1][2]. - Acceptance Check: Use goal_handle.accepted (a boolean) to verify if the server accepted the goal [3][4]. - Workflow: Typically, you add a callback to the future using add_done_callback to handle the response once the server processes the request [4][5].
Citations:
- 1: https://docs.ros.org/en/jazzy/p/rclpy/rclpy.action.client.html
- 2: https://docs.ros.org/en/rolling/p/rclpy/rclpy.action.client.html
- 3: https://docs.ros.org/en/rolling/p/rclpy/api/actions.html
- 4: https://docs.ros.org/en/foxy/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html
- 5: https://github.com/ros2/examples/blob/master/rclpy/actions/minimal_action_client/examples_rclpy_minimal_action_client/client.py
- 6: https://deepwiki.com/ros2/rclpy/6.2-action-client
- 7: https://docs.ros.org/en/kilted/p/rclpy/rclpy.action.client.html
Assert the trajectory goal is accepted before waiting for trip.
If this goal is rejected by the controller, the test failure is misattributed to collision monitoring instead of action server rejection. The goal_future completes when the action server responds with acceptance or rejection, but the code never retrieves or validates the result. Validate the goal handle immediately after spin_until_future_complete() returns.
Proposed fix
goal_future = _send_arm_trajectory(
monitor_probe.node, SELF_COLLISION_POSE, move_time_s=4.0
)
rclpy.spin_until_future_complete(
monitor_probe.node, goal_future, timeout_sec=30.0
)
+ goal_handle = goal_future.result()
+ assert goal_handle is not None and goal_handle.accepted, (
+ "Self-collision trajectory goal was rejected by joint_trajectory_controller."
+ )🤖 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_collision_monitor/test/collision_monitor_integration_test.py`
around lines 316 - 321, After the rclpy.spin_until_future_complete call
completes for the goal_future returned by _send_arm_trajectory, retrieve the
goal handle result from the future and add an assertion to verify it is not
None. This ensures that if the action server rejects the goal, the test
immediately fails with a clear assertion error rather than misattributing the
failure to collision monitoring issues later in the test.
|
This needs a fresh rebase on |
5 participants
Layer-2 "deploy to customer" integration: runs the licensed collision_monitor in parallel with the lab_sim MoveIt Pro objectives integration test.
src/lab_sim_collision_monitorusage example (config/launch/params only — no proprietary source in this public repo).PROJECTED_COLLISION)..github/workflows/collision-monitor-integration.yaml: onrepository_dispatch(collision-monitor-integration), pulls the licensedbundle_image(never builds source), suppliesMOVEIT_LICENSE_KEYfrom secretSTUDIO_CI_LICENSE_KEY, runs the test. Without a valid key the gated node fails loudly — the intended IP protection.Consumes the bundle published by the collision_monitor deploy-pipeline PR.
🤖 Generated with Claude Code