diff --git a/.github/workflows/collision-monitor-integration.yaml b/.github/workflows/collision-monitor-integration.yaml new file mode 100644 index 000000000..f18101050 --- /dev/null +++ b/.github/workflows/collision-monitor-integration.yaml @@ -0,0 +1,167 @@ +# Layer-2 "deploy to customer" integration: run the LICENSED, pre-bundled +# collision_monitor node in parallel with the lab_sim MoveIt Pro integration +# test. +# +# IP boundary (this repo is PUBLIC; the monitor is PROPRIETARY): +# * This workflow PULLS a pre-built, license-gated monitor bundle image and +# uses it as the build base. It NEVER clones or builds collision_monitor +# source. The only collision-monitor-specific things in this public repo +# are the lab_sim_collision_monitor usage example (launch/params/test). +# * The monitor's engine constructor is license-gated, so the example only +# runs with a valid MOVEIT_LICENSE_KEY (supplied here from the +# STUDIO_CI_LICENSE_KEY secret). Without it the monitor node dies on init +# and the Layer-2 test fails loudly. +# +# Dispatch contract consumed (workstream C / upstream must match EXACTLY): +# repository_dispatch event_type: collision-monitor-integration +# client_payload: +# bundle_image (REQUIRED) container image ref of the licensed, +# pre-bundled monitor (monitor + licensing + coal + +# runtime deps, layered on the MoveIt Pro base). +# collision_monitor_ref (optional) upstream sha/branch, logged only. +# upstream_run_id (optional) upstream run id, logged only. +name: Collision Monitor Integration (Layer 2) + +on: + repository_dispatch: + types: [collision-monitor-integration] + workflow_dispatch: + inputs: + bundle_image: + description: >- + Licensed, pre-bundled collision_monitor image to use as the build + base (monitor + licensing + coal + runtime deps). + required: true + type: string + collision_monitor_ref: + description: 'Upstream collision_monitor ref (logged only).' + required: false + default: '' + type: string + +concurrency: + group: collision-monitor-integration-${{ github.ref }} + cancel-in-progress: true + +jobs: + layer2-parallel-monitor: + name: lab_sim + licensed monitor (parallel) + runs-on: picknik-16-amd64 + steps: + - name: Resolve dispatch inputs + id: inputs + env: + DISPATCH_BUNDLE: ${{ github.event.client_payload.bundle_image }} + DISPATCH_REF: ${{ github.event.client_payload.collision_monitor_ref }} + DISPATCH_RUN_ID: ${{ github.event.client_payload.upstream_run_id }} + MANUAL_BUNDLE: ${{ inputs.bundle_image }} + MANUAL_REF: ${{ inputs.collision_monitor_ref }} + run: | + set -euo pipefail + bundle="${DISPATCH_BUNDLE:-$MANUAL_BUNDLE}" + ref="${DISPATCH_REF:-$MANUAL_REF}" + run_id="${DISPATCH_RUN_ID:-n/a}" + if [ -z "${bundle}" ]; then + echo "::error::No bundle_image provided in client_payload or workflow_dispatch inputs." >&2 + echo "The licensed monitor bundle image is required; this workflow does NOT build monitor source." >&2 + exit 1 + fi + echo "Using licensed monitor bundle image: ${bundle}" + echo "Upstream collision_monitor ref (informational): ${ref:-n/a}" + echo "Upstream run id (informational): ${run_id}" + echo "bundle_image=${bundle}" >> "${GITHUB_OUTPUT}" + + - name: Verify license secret present (fail loudly if not) + env: + MOVEIT_LICENSE_KEY: ${{ secrets.STUDIO_CI_LICENSE_KEY }} + run: | + set -euo pipefail + if [ -z "${MOVEIT_LICENSE_KEY}" ]; then + echo "::error::STUDIO_CI_LICENSE_KEY secret is empty. The collision_monitor engine is" >&2 + echo "license-gated and the Layer-2 example cannot run without a valid key." >&2 + exit 1 + fi + echo "License key present (value masked)." + + - name: Checkout example_ws (usage example only — NO monitor source) + uses: actions/checkout@de0fac2e4500dabe0009e67214ff5f5447ce83dd # v6.0.2 + with: + submodules: recursive + + - name: Detect bundle registry credentials + id: regcreds + env: + REG_USER: ${{ secrets.BUNDLE_REGISTRY_USERNAME }} + run: | + set -euo pipefail + if [ -n "${REG_USER}" ]; then + echo "have_creds=true" >> "${GITHUB_OUTPUT}" + else + echo "have_creds=false" >> "${GITHUB_OUTPUT}" + fi + + - name: Log in to the bundle image registry + if: ${{ steps.regcreds.outputs.have_creds == 'true' }} + uses: docker/login-action@74a5d142397b4f367a81961eba4e8cd7edddf772 # v3.4.0 + with: + registry: ${{ secrets.BUNDLE_REGISTRY }} + username: ${{ secrets.BUNDLE_REGISTRY_USERNAME }} + password: ${{ secrets.BUNDLE_REGISTRY_PASSWORD }} + + - name: Pull the licensed monitor bundle image + env: + BUNDLE_IMAGE: ${{ steps.inputs.outputs.bundle_image }} + run: | + set -euo pipefail + echo "Pulling pre-bundled licensed monitor image (no source build): ${BUNDLE_IMAGE}" + docker pull "${BUNDLE_IMAGE}" + + - name: Build the example_ws overlay on top of the bundle image + env: + BUNDLE_IMAGE: ${{ steps.inputs.outputs.bundle_image }} + run: | + set -euo pipefail + # Layer the public example workspace (lab_sim + lab_sim_collision_monitor) + # on top of the licensed bundle. The bundle already contains the built, + # licensed collision_monitor overlay; we only build the example packages. + # NOTE: monitor source is never present here — only the prebuilt bundle. + docker build \ + --build-arg "MOVEIT_PRO_BASE_IMAGE=${BUNDLE_IMAGE}" \ + -f ./Dockerfile \ + -t lab-sim-with-monitor:ci \ + . + + - 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 + # 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 \ + -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 + ' + + - name: Upload test results + if: ${{ always() }} + uses: actions/upload-artifact@018cc2cf5baa6db3ef3c5f8a56943fffe632ef53 # v4.6.2 + with: + name: collision-monitor-integration-results + path: | + build/lab_sim_collision_monitor/test_results/** + log/latest_test/** + if-no-files-found: ignore diff --git a/src/external_dependencies/phoebe_ws b/src/external_dependencies/phoebe_ws index feb723561..2823ed1e3 160000 --- a/src/external_dependencies/phoebe_ws +++ b/src/external_dependencies/phoebe_ws @@ -1 +1 @@ -Subproject commit feb72356168298d412d8fe2cafe01bd4af09890e +Subproject commit 2823ed1e37ff380d3462768f2f71b7e7fce08807 diff --git a/src/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xml b/src/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xml index ed7347af1..a321217bc 100644 --- a/src/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xml +++ b/src/hangar_sim/objectives/ml_move_boxes_to_loading_zone.xml @@ -1,6 +1,5 @@ - - - + + + + + - - - - + + + + + + - + + diff --git a/src/hangar_sim/objectives/move_boxes_looping.xml b/src/hangar_sim/objectives/move_boxes_looping.xml index f416ca071..1814d9a42 100644 --- a/src/hangar_sim/objectives/move_boxes_looping.xml +++ b/src/hangar_sim/objectives/move_boxes_looping.xml @@ -11,11 +11,27 @@ _skipIf="false" ID="Sequence" > + + + + + + + - - - + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - + + + + + + + - + @@ -358,12 +430,13 @@ - - + + - - - + diff --git a/src/lab_sim_collision_monitor/CMakeLists.txt b/src/lab_sim_collision_monitor/CMakeLists.txt new file mode 100644 index 000000000..e359fc6e9 --- /dev/null +++ b/src/lab_sim_collision_monitor/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.22) +project(lab_sim_collision_monitor) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY + config + launch + DESTINATION + share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Parallel-monitor Layer-2 integration test: brings up the lab_sim MoveIt Pro + # backend AND the licensed collision_monitor node together, then asserts both + # (a) no false trips while a valid objective runs, and (b) a deliberate + # self-collision pose DOES trip. + # + # Requires a valid MOVEIT_LICENSE_KEY in the environment (the monitor engine + # ctor is license-gated). Long timeout: sim bring-up + objective execution. + ament_add_pytest_test( + collision_monitor_integration_test + test/collision_monitor_integration_test.py + TIMEOUT 900 + ENV + MOVEIT_CONFIG_PACKAGE=lab_sim + MOVEIT_HOST_USER_WORKSPACE=${CMAKE_SOURCE_DIR}) +endif() + +ament_package() diff --git a/src/lab_sim_collision_monitor/README.md b/src/lab_sim_collision_monitor/README.md new file mode 100644 index 000000000..f8322223d --- /dev/null +++ b/src/lab_sim_collision_monitor/README.md @@ -0,0 +1,85 @@ +# lab_sim_collision_monitor + +Usage example showing how to configure the vendor-neutral **collision monitor** +node for the `lab_sim` robot (a UR arm on a linear rail). + +> **This package does not contain the collision monitor.** The monitor is a +> separately licensed product, shipped as a pre-bundled binary image/overlay. +> This package contains only the robot-specific glue — launch, params, and an +> integration test — that *consumes* it. Running the example requires a valid +> `MOVEIT_LICENSE_KEY`; without one the monitor's engine constructor fails +> loudly by design. + +## What the monitor does + +Each tick (~100 Hz here) the monitor forward-projects the arm to its +max-effort braking-stop configuration and collision-checks that stopping +configuration. If a clean stop can no longer be guaranteed it trips. For +`lab_sim`, with the generic node's empty collision world, the collisions it +sees are **self-collisions** (robot-link vs robot-link, per the SRDF ACM). + +### Wiring (all robot specifics live here, not in the monitor core) + +- **StateProvider** ← `/joint_states`. `lab_sim` reports driving joint angles + (rad) and the rail position (m) directly, in model-DOF order, so no + actuator→joint conversion is needed. (If a robot reported actuator units, the + conversion would live in a Layer-C adapter that republishes `/joint_states`, + never in the monitor core.) +- **Robot model** ← `/robot_description` + `/robot_description_semantic` + (the SRDF supplies the allowed-collision matrix). +- **TripSink** → a `lab_sim` stop. The production stop action is a controller + halt: deactivate `joint_trajectory_controller` via + `/controller_manager/switch_controller`. The integration test demonstrates + this on the first trip. +- **Outputs**: `…/collision_monitor/trip` (`std_msgs/String`, one per trip with + reason + contacts) and `…/collision_monitor/status` (`std_msgs/Bool`, latched + engine state each tick). + +Parameters are in [`config/lab_sim_collision_monitor.yaml`](config/lab_sim_collision_monitor.yaml); +notably `default_accel_limit` is set to the lab_sim arm's 30 deg/s² braking +spec (the 6 UR joints carry no URDF acceleration attribute). + +## Run it locally + +The monitor must run **in parallel** with the lab_sim MoveIt Pro backend, which +supplies `/robot_description(_semantic)` and `/joint_states`. The integration +test does exactly this. + +```bash +# From the example_ws root, inside the MoveIt Pro dev container, with a valid +# MOVEIT_LICENSE_KEY in the environment (example_ws ships one in .env) AND the +# licensed monitor bundle present on the overlay: +export MOVEIT_LICENSE_KEY=... # see .env +colcon build --packages-select lab_sim_collision_monitor +colcon test --packages-select lab_sim_collision_monitor \ + --executor sequential --event-handlers console_direct+ +colcon test-result --verbose +``` + +To launch only the monitor (against an already-running lab_sim backend): + +```bash +ros2 launch lab_sim_collision_monitor collision_monitor.launch.py +``` + +## The integration test (Layer 2) + +`test/collision_monitor_integration_test.py` brings up lab_sim + the monitor and +asserts **both** directions: + +1. **No false trips** while the safe `Move Along Square` objective runs. +2. A **deliberate self-collision** pose (commanded straight to the + `joint_trajectory_controller`, bypassing planning) **must** trip the monitor + with a `PROJECTED_COLLISION`. + +`test_monitor_becomes_active` doubles as the **license gate**: if +`MOVEIT_LICENSE_KEY` is missing/invalid the monitor node dies on init, `status` +never publishes, and the test fails — the example refuses to run unlicensed. + +## CI + +`.github/workflows/collision-monitor-integration.yaml` (downstream, in this +repo) runs this test against a pre-bundled licensed monitor image. It is +triggered by `repository_dispatch` (`collision-monitor-integration`) from the +monitor's upstream deploy pipeline, or manually via `workflow_dispatch`. It +**pulls** the licensed bundle image and never clones or builds monitor source. diff --git a/src/lab_sim_collision_monitor/config/lab_sim_collision_monitor.yaml b/src/lab_sim_collision_monitor/config/lab_sim_collision_monitor.yaml new file mode 100644 index 000000000..e2367e643 --- /dev/null +++ b/src/lab_sim_collision_monitor/config/lab_sim_collision_monitor.yaml @@ -0,0 +1,56 @@ +# Copyright 2026 PickNik Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the PickNik Inc. nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DAMAGES ARISING IN ANY WAY +# OUT OF THE USE OF THIS SOFTWARE. + +# Parameters for the vendor-neutral collision_monitor node, specialized for the +# lab_sim UR-on-a-linear-rail robot. ALL robot specifics live here, never in the +# monitor's robot-agnostic core. +# +# The monitor reads /joint_states in MODEL-DOF order. lab_sim reports DRIVING +# joint angles (rad) / rail position (m) directly on /joint_states, so no +# actuator->joint conversion is required for this robot. (If a robot reported +# actuator/cylinder units, the conversion would belong in a Layer-C adapter that +# republishes /joint_states in joint space — never in the monitor core.) +/**: + ros__parameters: + # Monitor tick rate. lab_sim's controller_manager runs at 600 Hz; 100 Hz is + # a safe, low-cost monitor cadence for this example. + rate_hz: 100.0 + + # Per-tick wall budget. Exceeding it trips DEADLINE_OVERRUN (fail-safe). + deadline_us: 5000 + + # URDF/SRDF link names for lab_sim's picknik_ur are UNPREFIXED (e.g. + # base_link, wrist_3_link), so the robot_prefix is empty. + robot_prefix: "" + + # Default acceleration limit (rad/s^2 or m/s^2) used for any DOF whose URDF + # carries no acceleration attribute. The 6 UR joints have no URDF + # acceleration attr, so they fall back to this value. lab_sim's MoveIt-level + # joint_limits.yaml uses 30 deg/s^2 for the arm joints: + # 30 deg/s^2 = 30 * pi/180 = 0.5235987755982988 rad/s^2. + # The linear_rail_joint DOES carry a URDF acceleration limit (10.0 m/s^2), + # which the engine uses directly and this default does not override. + # NOTE: 0.0 would be fail-safe (trip on any motion of an attr-less joint); + # we set a real, conservative value verified against the arm's braking spec. + default_accel_limit: 0.5235987755982988 + + # 0 = check only the forward-projected braking-stop configuration. Set >0 to + # also check interpolated configs along the stopping path (tunneling + # mitigation), at N extra collision queries per tick. + swept_path_samples: 0 diff --git a/src/lab_sim_collision_monitor/launch/collision_monitor.launch.py b/src/lab_sim_collision_monitor/launch/collision_monitor.launch.py new file mode 100644 index 000000000..c44d88343 --- /dev/null +++ b/src/lab_sim_collision_monitor/launch/collision_monitor.launch.py @@ -0,0 +1,96 @@ +# Copyright 2026 PickNik Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the PickNik Inc. nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DAMAGES ARISING IN ANY WAY +# OUT OF THE USE OF THIS SOFTWARE. + +"""Launch the licensed, vendor-neutral collision_monitor node for lab_sim. + +This brings up ONLY the monitor node (and its component container), configured +by ``config/lab_sim_collision_monitor.yaml``. It is meant to run *in parallel* +with the lab_sim MoveIt Pro backend (robot drivers + agent + bridge), which is +what supplies the topics this node consumes: + + * /robot_description (URDF, transient_local) + * /robot_description_semantic (SRDF for the ACM, transient_local) + * /joint_states (live joint positions/velocities) + +The ``collision_monitor_component`` plugin is provided by the pre-bundled, +license-gated monitor overlay/image — it is NOT built from this workspace. If +that overlay is absent the container will fail to load the component and the +launch fails loudly. With the overlay present, the monitor's engine constructor +still requires a valid MOVEIT_LICENSE_KEY; without one it throws and the node +dies (the intended IP/licensing gate). + +Outputs: + * /trip (std_msgs/String) one message per trip with reason + contacts + * /status (std_msgs/Bool) latched engine state every tick +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + params_file = LaunchConfiguration("params_file") + monitor_namespace = LaunchConfiguration("monitor_namespace") + + declared_args = [ + DeclareLaunchArgument( + "params_file", + default_value=PathJoinSubstitution( + [ + FindPackageShare("lab_sim_collision_monitor"), + "config", + "lab_sim_collision_monitor.yaml", + ] + ), + description="Parameters for the collision_monitor node.", + ), + DeclareLaunchArgument( + "monitor_namespace", + default_value="collision_monitor", + description="Namespace for the monitor node. Trip/status topics are " + "published under /collision_monitor/{trip,status}.", + ), + ] + + monitor_container = ComposableNodeContainer( + name="collision_monitor_container", + namespace=monitor_namespace, + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + # Provided by the licensed, pre-bundled monitor overlay/image. + package="collision_monitor_ros", + plugin="collision_monitor_ros::CollisionMonitorComponent", + name="collision_monitor", + namespace=monitor_namespace, + parameters=[params_file], + # Robot description/semantic are published on absolute topics by + # the MoveIt Pro backend; the component subscribes to absolute + # /robot_description(_semantic) and /joint_states already. + ) + ], + output="screen", + ) + + return LaunchDescription([*declared_args, monitor_container]) diff --git a/src/lab_sim_collision_monitor/package.xml b/src/lab_sim_collision_monitor/package.xml new file mode 100644 index 000000000..d9d57f30d --- /dev/null +++ b/src/lab_sim_collision_monitor/package.xml @@ -0,0 +1,42 @@ + + + lab_sim_collision_monitor + 0.0.0 + + + Usage example: configures the vendor-neutral (and separately licensed) + collision_monitor ROS 2 node for the lab_sim UR-on-a-rail robot. + + This package contains ONLY robot-specific glue — launch, params, and a + parallel integration test. It does NOT contain or build the collision + monitor itself: the monitor node (collision_monitor_component) is provided + as a pre-bundled, license-gated binary image/overlay. Running the example + therefore requires a valid MOVEIT_LICENSE_KEY; without one the monitor's + engine constructor fails loudly (by design). + + + MoveIt Pro Maintainer + + BSD-3-Clause + + ament_cmake + + + lab_sim + + collision_monitor_ros + rclcpp_components + robot_state_publisher + controller_manager + + ament_cmake_pytest + ament_lint_auto + ament_flake8 + picknik_ament_copyright + + + ament_cmake + + diff --git a/src/lab_sim_collision_monitor/test/collision_monitor_integration_test.py b/src/lab_sim_collision_monitor/test/collision_monitor_integration_test.py new file mode 100644 index 000000000..ec06e4110 --- /dev/null +++ b/src/lab_sim_collision_monitor/test/collision_monitor_integration_test.py @@ -0,0 +1,343 @@ +# Copyright 2026 PickNik Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the PickNik Inc. nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DAMAGES ARISING IN ANY WAY +# OUT OF THE USE OF THIS SOFTWARE. + +"""Layer-2 parallel-monitor integration test for lab_sim. + +Runs the lab_sim MoveIt Pro backend (via the shared +``execute_objective_resource`` fixture) and the licensed, vendor-neutral +``collision_monitor`` node *together*, then asserts BOTH directions of +correctness the rearchitecture contract requires: + + (a) NO false trips while a valid lab_sim objective runs (the monitor must + not spuriously stop a known-safe motion); and + (b) a DELIBERATE self-collision pose MUST trip (proves the monitor catches a + real, ACM-allowed collision). + +The deliberate scenario is a *self-collision* pose, not a scene-obstacle +contact, because the generic monitor node initializes an empty collision world +(no param to inject scene obstacles); robot-link-vs-robot-link self-collisions +are exactly what it can see for this robot, and the contract explicitly allows +a self-collision pose. The pose folds the wrist/gripper into the upper arm / +base, a pair that is NOT disabled in the SRDF ACM. + +LICENSE GATE: the monitor's engine constructor is license-gated. This test +only passes when a valid ``MOVEIT_LICENSE_KEY`` is present in the environment +(example_ws ships one in ``.env``; CI maps the ``STUDIO_CI_LICENSE_KEY`` +secret). Without a key the monitor node dies on init, ``/status`` never +publishes, and ``test_monitor_becomes_active`` fails loudly — the intended IP +protection, surfaced as a test failure rather than a silent skip. + +The monitor's TripSink in production is wired to a lab_sim stop (controller +halt). The deliberate-trip test demonstrates that wiring: on the first trip it +deactivates ``joint_trajectory_controller`` via ``switch_controller``. The +trip/no-trip *assertions* themselves read the monitor's own ``trip`` (String) +and ``status`` (Bool) outputs, which are the authoritative monitor signals. +""" + +from __future__ import annotations + +import multiprocessing +import os +import signal +import time +from typing import Optional + +import pytest +import rclpy +from launch import LaunchDescription, LaunchService +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare +from rclpy.action import ActionClient +from rclpy.node import Node as ROSNode +from rclpy.qos import QoSProfile + +from builtin_interfaces.msg import Duration as DurationMsg +from control_msgs.action import FollowJointTrajectory +from controller_manager_msgs.srv import SwitchController +from std_msgs.msg import Bool, String +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +# ``execute_objective_resource`` (brings up the lab_sim MoveIt Pro backend) and +# the autouse ``reset_simulation_before_test`` are provided via conftest.py; +# the test references them through pytest fixture injection. +from moveit_pro_test_utils.objective_test_fixture import ( + ExecuteObjectiveResource, + run_objective, +) + +# Monitor topic surface (see collision_monitor_component.cpp). The launch puts +# the node at namespace "collision_monitor" with node name "collision_monitor", +# and the component declares ~/trip and ~/status, hence the doubled segment. +MONITOR_NS = "collision_monitor" +TRIP_TOPIC = f"/{MONITOR_NS}/collision_monitor/trip" +STATUS_TOPIC = f"/{MONITOR_NS}/collision_monitor/status" + +# lab_sim model-DOF order for the joint_trajectory_controller. +ARM_JOINTS = [ + "linear_rail_joint", + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] +JTC_ACTION = "/joint_trajectory_controller/follow_joint_trajectory" +SWITCH_CONTROLLER_SRV = "/controller_manager/switch_controller" + +# A known-safe arm motion objective shipped by lab_sim. Exercises real arm +# movement without driving into self-collision; the monitor must stay clear. +SAFE_OBJECTIVE = "Move Along Square" + +# Deliberate self-collision target. Curls the wrist back over the upper arm so +# the gripper/wrist links collide with upper_arm_link / base (NOT disabled in +# the SRDF ACM). rail at 0; shoulder folded; elbow + wrists curled hard. +# [linear_rail, shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2, wrist_3] +SELF_COLLISION_POSE = [0.0, 0.0, -2.6, 2.7, -2.8, 0.0, 0.0] + +# Monitor needs URDF + SRDF (transient_local) + a complete joint_state before it +# flips to "monitoring active" and starts publishing /status. Generous on CI. +MONITOR_ACTIVE_TIMEOUT_S = 120.0 +SAFE_RUN_TIMEOUT_S = 180.0 +TRIP_TIMEOUT_S = 60.0 + + +def _monitor_launch() -> None: + """Run only the collision_monitor launch in this subprocess.""" + launch_service = LaunchService() + launch_service.include_launch_description( + LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + FindPackageShare("lab_sim_collision_monitor"), + "/launch/collision_monitor.launch.py", + ] + ) + ) + ] + ) + ) + launch_service.run() + + +class MonitorProbe: + """Subscribes to the monitor's trip/status outputs and tracks them. + + Also owns the production-style TripSink demonstration: a controller halt + via ``switch_controller`` fired once on the first observed trip. + """ + + def __init__(self, node: ROSNode) -> None: + self.node = node + self.latest_status: Optional[bool] = None + self.trip_messages: list[str] = [] + self._halt_fired = False + self._switch_client = node.create_client( + SwitchController, SWITCH_CONTROLLER_SRV + ) + node.create_subscription( + Bool, STATUS_TOPIC, self._on_status, QoSProfile(depth=10) + ) + node.create_subscription( + String, TRIP_TOPIC, self._on_trip, QoSProfile(depth=10) + ) + + def _on_status(self, msg: Bool) -> None: + self.latest_status = msg.data + + def _on_trip(self, msg: String) -> None: + self.trip_messages.append(msg.data) + # TripSink -> lab_sim stop: halt the arm controller on the first trip. + # Clearly marked as the deliberate stop action; best-effort (the + # assertions below key off the monitor's own outputs, not this halt). + if not self._halt_fired and self._switch_client.service_is_ready(): + self._halt_fired = True + req = SwitchController.Request() + req.deactivate_controllers = ["joint_trajectory_controller"] + req.strictness = SwitchController.Request.BEST_EFFORT + self._switch_client.call_async(req) + + def tripped(self) -> bool: + return bool(self.trip_messages) or self.latest_status is True + + +def _spin(node: ROSNode, duration_s: float) -> None: + deadline = time.monotonic() + duration_s + while time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.05) + + +@pytest.fixture(scope="module") +def monitor_probe(execute_objective_resource: ExecuteObjectiveResource): + """Launch the monitor in parallel with the (already-up) lab_sim backend. + + Depends on ``execute_objective_resource`` so the MoveIt Pro backend — and + therefore /robot_description(_semantic) and /joint_states — is live before + the monitor starts. Yields a :class:`MonitorProbe`. + """ + proc = multiprocessing.Process(target=_monitor_launch) + proc.start() + print(f"[monitor] launched pid={proc.pid}", flush=True) + + node = ROSNode("collision_monitor_test_probe") + probe = MonitorProbe(node) + try: + yield probe + finally: + node.destroy_node() + if proc.pid is not None: + os.kill(proc.pid, signal.SIGINT) + proc.join(timeout=20) + if proc.is_alive(): + proc.kill() + proc.join(timeout=10) + + +def _wait_for_monitor_active(probe: MonitorProbe, timeout_s: float) -> bool: + """Block until the monitor publishes its first /status (== active).""" + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + rclpy.spin_once(probe.node, timeout_sec=0.2) + if probe.latest_status is not None: + return True + return False + + +def _send_arm_trajectory(node: ROSNode, positions: list[float], move_time_s: float): + """Command the JTC directly to ``positions`` (bypasses MoveIt planning). + + Returns the goal-handle future; the caller spins. Used to drive the robot + into the deliberate self-collision pose without MoveIt refusing the goal. + """ + client = ActionClient(node, FollowJointTrajectory, JTC_ACTION) + if not client.wait_for_server(timeout_sec=30.0): + raise TimeoutError(f"{JTC_ACTION} action server not available") + + goal = FollowJointTrajectory.Goal() + goal.trajectory = JointTrajectory() + goal.trajectory.joint_names = ARM_JOINTS + point = JointTrajectoryPoint() + point.positions = positions + secs = int(move_time_s) + point.time_from_start = DurationMsg( + sec=secs, nanosec=int((move_time_s - secs) * 1e9) + ) + goal.trajectory.points = [point] + return client.send_goal_async(goal) + + +# === Test (preconditions) === + + +def test_monitor_becomes_active(monitor_probe: MonitorProbe): + """The monitor must come up and publish /status. + + This is also the LICENSE GATE assertion: if MOVEIT_LICENSE_KEY is missing + or invalid the engine ctor throws, the node dies, /status never publishes, + and this fails — the example refuses to run unlicensed, loudly. + """ + assert _wait_for_monitor_active(monitor_probe, MONITOR_ACTIVE_TIMEOUT_S), ( + f"collision_monitor did not publish {STATUS_TOPIC} within " + f"{MONITOR_ACTIVE_TIMEOUT_S:.0f}s. Either the licensed monitor bundle " + "is not on the overlay, or MOVEIT_LICENSE_KEY is missing/invalid " + "(engine ctor is license-gated)." + ) + # Fresh start: not tripped. + assert monitor_probe.latest_status is False, ( + "Monitor reported tripped at startup on a safe home configuration." + ) + + +# === Test (a): no false trips on a valid objective === + + +def test_no_false_trip_during_safe_objective( + monitor_probe: MonitorProbe, + execute_objective_resource: ExecuteObjectiveResource, +): + """A known-safe objective must run without tripping the monitor.""" + assert _wait_for_monitor_active(monitor_probe, MONITOR_ACTIVE_TIMEOUT_S), ( + "Monitor not active before safe-objective run." + ) + monitor_probe.trip_messages.clear() + + run_objective( + SAFE_OBJECTIVE, + should_cancel=False, + execute_objective_resource=execute_objective_resource, + objective_wait_time=SAFE_RUN_TIMEOUT_S, + ) + # Drain any in-flight status/trip messages. + _spin(monitor_probe.node, 2.0) + + assert not monitor_probe.tripped(), ( + f"Monitor FALSE-TRIPPED during the safe objective " + f"'{SAFE_OBJECTIVE}'. Trip messages: {monitor_probe.trip_messages}" + ) + + +# === Test (b): deliberate self-collision MUST trip === + + +def test_deliberate_self_collision_trips( + monitor_probe: MonitorProbe, + execute_objective_resource: ExecuteObjectiveResource, +): + """Driving into a self-collision pose MUST trip the monitor. + + The reset fixture restores a safe keyframe before this test, so we start + clean, then command the JTC straight to a self-colliding configuration and + assert the monitor trips (its forward-projected braking stop collides). + """ + assert _wait_for_monitor_active(monitor_probe, MONITOR_ACTIVE_TIMEOUT_S), ( + "Monitor not active before deliberate-trip run." + ) + monitor_probe.trip_messages.clear() + + 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 + ) + + # Wait for the robot to traverse toward / arrive at the colliding pose and + # for the monitor to trip. + deadline = time.monotonic() + TRIP_TIMEOUT_S + while time.monotonic() < deadline and not monitor_probe.tripped(): + rclpy.spin_once(monitor_probe.node, timeout_sec=0.1) + + assert monitor_probe.tripped(), ( + "Monitor did NOT trip while the arm was driven into a self-collision " + f"pose {SELF_COLLISION_POSE}. Expected a PROJECTED_COLLISION trip. " + f"latest_status={monitor_probe.latest_status}, " + f"trips={monitor_probe.trip_messages}" + ) + # The trip reason should reflect a projected collision (not, say, a stale + # state). Accept any trip but surface the reason for diagnostics. + if monitor_probe.trip_messages: + assert any( + "PROJECTED_COLLISION" in m for m in monitor_probe.trip_messages + ), ( + "Monitor tripped but not for PROJECTED_COLLISION: " + f"{monitor_probe.trip_messages}" + ) diff --git a/src/lab_sim_collision_monitor/test/conftest.py b/src/lab_sim_collision_monitor/test/conftest.py new file mode 100644 index 000000000..18b132b4a --- /dev/null +++ b/src/lab_sim_collision_monitor/test/conftest.py @@ -0,0 +1,31 @@ +# Copyright 2026 PickNik Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the PickNik Inc. nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DAMAGES ARISING IN ANY WAY +# OUT OF THE USE OF THIS SOFTWARE. + +"""Shared pytest fixtures for the lab_sim_collision_monitor integration test. + +Re-exporting the MoveIt Pro backend fixtures here (rather than importing them +into the test module) lets the test reference them purely via pytest's fixture +injection — no module-level import of the fixture names, so no flake8 +redefinition (F811) noise from using them as test arguments. +""" + +from moveit_pro_test_utils.objective_test_fixture import ( # noqa: F401 + execute_objective_resource, + reset_simulation_before_test, +) diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xml index 7053616a3..af3bc8ff2 100644 --- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xml +++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/velocity_force_controller_zero.xml @@ -1,10 +1,9 @@ - + @@ -36,6 +56,7 @@ + diff --git a/src/moveit_pro_kinova_configs/kinova_sim/objectives/write_picknik.xml b/src/moveit_pro_kinova_configs/kinova_sim/objectives/write_picknik.xml index 353cb9464..a650804c8 100644 --- a/src/moveit_pro_kinova_configs/kinova_sim/objectives/write_picknik.xml +++ b/src/moveit_pro_kinova_configs/kinova_sim/objectives/write_picknik.xml @@ -1,14 +1,22 @@ - - + - + + - + @@ -78,6 +103,7 @@ + diff --git a/src/picknik_accessories b/src/picknik_accessories index aad29faca..3b0912dc3 160000 --- a/src/picknik_accessories +++ b/src/picknik_accessories @@ -1 +1 @@ -Subproject commit aad29faca2ba7ed632ad3bbe86db4700b52a60aa +Subproject commit 3b0912dc3daf88318ba8594768c9689c9a769e75