From 73a5ad28b4f81154b4762b0657139d7f4b7adbb2 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 7 Feb 2026 21:27:13 +0000 Subject: [PATCH 1/6] Add MoveIt 2 Panda pick-and-place demo with ros2_medkit integration Full Docker-based demo of a Panda 7-DOF robot arm performing continuous pick-place-home cycles with ros2_medkit diagnostics and fault injection. Simulation modes: - Gazebo Harmonic (default): physics simulation with gz_ros2_control - Fake hardware (--fake flag): mock controllers with RViz visualization Components: - MoveIt 2 move_group with OMPL planner for motion planning - Joint-space targets for reliable pick/place/home cycling - ros2_medkit gateway, fault_manager, diagnostic_bridge - Manipulation monitor tracking joint limits, velocity, collision, planning failures, and controller timeouts - Manifest with 4 areas, 7 components, 10 apps, 4 functions Scripts: - run-demo.sh / stop-demo.sh: Docker lifecycle management - move-arm.sh: interactive joint preset commands - inject-*.sh / restore-normal.sh: fault injection helpers - check-entities.sh / check-faults.sh: REST API inspection Docker: - CPU and NVIDIA GPU profiles - Web UI (port 3000) and REST API (port 8080) - Headless and GUI modes supported --- README.md | 27 ++ demos/moveit_pick_place/CMakeLists.txt | 23 ++ demos/moveit_pick_place/Dockerfile | 66 +++ demos/moveit_pick_place/README.md | 380 ++++++++++++++++++ demos/moveit_pick_place/check-entities.sh | 69 ++++ demos/moveit_pick_place/check-faults.sh | 62 +++ .../config/medkit_params.yaml | 29 ++ .../config/moveit_controllers.yaml | 35 ++ .../config/panda_manifest.yaml | 226 +++++++++++ .../config/panda_moveit.yaml | 11 + demos/moveit_pick_place/docker-compose.yml | 66 +++ demos/moveit_pick_place/inject-collision.sh | 55 +++ .../inject-controller-timeout.sh | 52 +++ .../moveit_pick_place/inject-grasp-failure.sh | 55 +++ demos/moveit_pick_place/inject-joint-limit.sh | 50 +++ .../inject-planning-failure.sh | 55 +++ demos/moveit_pick_place/launch/demo.launch.py | 149 +++++++ .../launch/demo_gazebo.launch.py | 368 +++++++++++++++++ demos/moveit_pick_place/move-arm.sh | 152 +++++++ demos/moveit_pick_place/package.xml | 29 ++ demos/moveit_pick_place/restore-normal.sh | 80 ++++ demos/moveit_pick_place/run-demo.sh | 177 ++++++++ .../scripts/manipulation_monitor.py | 352 ++++++++++++++++ .../scripts/pick_place_loop.py | 192 +++++++++ demos/moveit_pick_place/stop-demo.sh | 74 ++++ 25 files changed, 2834 insertions(+) create mode 100644 demos/moveit_pick_place/CMakeLists.txt create mode 100644 demos/moveit_pick_place/Dockerfile create mode 100644 demos/moveit_pick_place/README.md create mode 100755 demos/moveit_pick_place/check-entities.sh create mode 100755 demos/moveit_pick_place/check-faults.sh create mode 100644 demos/moveit_pick_place/config/medkit_params.yaml create mode 100644 demos/moveit_pick_place/config/moveit_controllers.yaml create mode 100644 demos/moveit_pick_place/config/panda_manifest.yaml create mode 100644 demos/moveit_pick_place/config/panda_moveit.yaml create mode 100644 demos/moveit_pick_place/docker-compose.yml create mode 100755 demos/moveit_pick_place/inject-collision.sh create mode 100755 demos/moveit_pick_place/inject-controller-timeout.sh create mode 100755 demos/moveit_pick_place/inject-grasp-failure.sh create mode 100755 demos/moveit_pick_place/inject-joint-limit.sh create mode 100755 demos/moveit_pick_place/inject-planning-failure.sh create mode 100644 demos/moveit_pick_place/launch/demo.launch.py create mode 100644 demos/moveit_pick_place/launch/demo_gazebo.launch.py create mode 100755 demos/moveit_pick_place/move-arm.sh create mode 100644 demos/moveit_pick_place/package.xml create mode 100755 demos/moveit_pick_place/restore-normal.sh create mode 100755 demos/moveit_pick_place/run-demo.sh create mode 100755 demos/moveit_pick_place/scripts/manipulation_monitor.py create mode 100755 demos/moveit_pick_place/scripts/pick_place_loop.py create mode 100755 demos/moveit_pick_place/stop-demo.sh diff --git a/README.md b/README.md index e7f7113..91e9bde 100644 --- a/README.md +++ b/README.md @@ -18,6 +18,7 @@ to complete mobile robot integration: - **Sensor Diagnostics** — Lightweight demo focusing on data monitoring and fault injection - **TurtleBot3 Integration** — Full-featured demo with Nav2 navigation, showing entity hierarchy and real-time control +- **MoveIt Pick-and-Place** — Panda 7-DOF arm manipulation with MoveIt 2, fault monitoring for planning, controllers, and joint limits **Key Capabilities Demonstrated:** @@ -42,6 +43,7 @@ Both demos support: |------|-------------|----------|--------| | [Sensor Diagnostics](demos/sensor_diagnostics/) | Lightweight sensor diagnostics demo (no Gazebo required) | Data monitoring, fault injection, dual fault reporting paths | ✅ Ready | | [TurtleBot3 Integration](demos/turtlebot3_integration/) | Full ros2_medkit integration with TurtleBot3 and Nav2 | SOVD-compliant API, manifest-based discovery, fault management | ✅ Ready | +| [MoveIt Pick-and-Place](demos/moveit_pick_place/) | Panda 7-DOF arm with MoveIt 2 manipulation and ros2_medkit | Planning fault detection, controller monitoring, joint limits | ✅ Ready | ### Quick Start @@ -96,6 +98,31 @@ cd demos/turtlebot3_integration - Fault injection scenarios for Nav2 components - Real-time robot control via HTTP +#### MoveIt 2 Pick-and-Place Demo (Manipulation Stack) + +Panda robot arm demo with pick-and-place manipulation: + +```bash +cd demos/moveit_pick_place +./run-demo.sh +# RViz will open with Panda arm (or use --headless), Web UI at http://localhost:3000 +# Move the arm: ./move-arm.sh demo +# Inject faults: ./inject-planning-failure.sh +# Check faults: ./check-faults.sh + +# To stop +./stop-demo.sh +``` + +**Features:** + +- Panda 7-DOF arm with MoveIt 2 and mock hardware (no physics sim) +- Interactive arm control via `move-arm.sh` +- Continuous pick-and-place task loop +- Manipulation fault monitoring (planning, controller, joint limits) +- 5 fault injection scenarios with one-click scripts +- SOVD-compliant REST API with rich entity hierarchy (4 areas, 7 components) + ## Getting Started ### Prerequisites diff --git a/demos/moveit_pick_place/CMakeLists.txt b/demos/moveit_pick_place/CMakeLists.txt new file mode 100644 index 0000000..421c1b5 --- /dev/null +++ b/demos/moveit_pick_place/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.8) +project(moveit_medkit_demo) + +find_package(ament_cmake REQUIRED) + +# Install launch files +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +# Install config files +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config +) + +# Install scripts +install(PROGRAMS + scripts/manipulation_monitor.py + scripts/pick_place_loop.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/demos/moveit_pick_place/Dockerfile b/demos/moveit_pick_place/Dockerfile new file mode 100644 index 0000000..ffcdfb6 --- /dev/null +++ b/demos/moveit_pick_place/Dockerfile @@ -0,0 +1,66 @@ +# MoveIt 2 Panda + ros2_medkit Integration Demo +# Supports fake hardware (default) and Gazebo Harmonic simulation (--gazebo) + +FROM osrf/ros:jazzy-desktop + +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=jazzy +ENV COLCON_WS=/root/demo_ws + +# Install MoveIt 2, Panda, ros2_control, Gazebo, and build dependencies +RUN apt-get update && apt-get install -y \ + ros-jazzy-moveit \ + ros-jazzy-moveit-resources-panda-moveit-config \ + ros-jazzy-moveit-resources-panda-description \ + ros-jazzy-moveit-planners-ompl \ + ros-jazzy-moveit-ros-planning-interface \ + ros-jazzy-moveit-ros-visualization \ + ros-jazzy-moveit-simple-controller-manager \ + ros-jazzy-moveit-servo \ + ros-jazzy-ros2-controllers \ + ros-jazzy-ros2-control \ + ros-jazzy-joint-state-publisher \ + ros-jazzy-joint-state-publisher-gui \ + ros-jazzy-ros-gz-sim \ + ros-jazzy-ros-gz-bridge \ + ros-jazzy-gz-ros2-control \ + ros-jazzy-ament-lint-auto \ + ros-jazzy-ament-lint-common \ + python3-colcon-common-extensions \ + nlohmann-json3-dev \ + libcpp-httplib-dev \ + sqlite3 libsqlite3-dev git curl \ + && rm -rf /var/lib/apt/lists/* + +# Clone ros2_medkit from GitHub +WORKDIR ${COLCON_WS}/src +RUN git clone --depth 1 https://github.com/selfpatch/ros2_medkit.git && \ + mv ros2_medkit/src/ros2_medkit_gateway \ + ros2_medkit/src/ros2_medkit_msgs \ + ros2_medkit/src/ros2_medkit_serialization \ + ros2_medkit/src/ros2_medkit_fault_manager \ + ros2_medkit/src/ros2_medkit_fault_reporter \ + ros2_medkit/src/ros2_medkit_diagnostic_bridge . && \ + rm -rf ros2_medkit + +# Copy demo package from local context +COPY package.xml CMakeLists.txt ${COLCON_WS}/src/moveit_medkit_demo/ +COPY config/ ${COLCON_WS}/src/moveit_medkit_demo/config/ +COPY launch/ ${COLCON_WS}/src/moveit_medkit_demo/launch/ +COPY scripts/ ${COLCON_WS}/src/moveit_medkit_demo/scripts/ + +# Build ros2_medkit and demo package +WORKDIR ${COLCON_WS} +RUN bash -c "source /opt/ros/jazzy/setup.bash && \ + rosdep update && \ + rosdep install --from-paths src --ignore-src -r -y || true" && \ + bash -c "source /opt/ros/jazzy/setup.bash && \ + colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF" + +# Setup environment +RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc && \ + echo "source ${COLCON_WS}/install/setup.bash" >> ~/.bashrc + +EXPOSE 8080 + +CMD ["bash"] diff --git a/demos/moveit_pick_place/README.md b/demos/moveit_pick_place/README.md new file mode 100644 index 0000000..4345cd1 --- /dev/null +++ b/demos/moveit_pick_place/README.md @@ -0,0 +1,380 @@ +# MoveIt 2 Pick-and-Place Integration Demo + +A comprehensive integration demo combining a **Panda 7-DOF robot arm** with **MoveIt 2** motion planning and **ros2_medkit** SOVD-compliant diagnostics. The robot performs continuous pick-and-place cycles while a manipulation monitor detects faults — planning failures, controller timeouts, joint limit violations — and reports them through the SOVD REST API. + +## Status + +✅ **Demo Ready** — Docker-based deployment with MoveIt 2, RViz visualization, mock hardware, and full ros2_medkit stack. + +## Overview + +This demo demonstrates: + +- **MoveIt 2 motion planning** with the Panda 7-DOF arm and gripper +- **Continuous pick-and-place** loop as a realistic manipulation workload +- **Manipulation fault monitoring** (planning failures, trajectory errors, joint limits) +- **SOVD-compliant REST API** with Areas → Components → Apps → Functions hierarchy +- **Manifest-based entity discovery** (hybrid mode with runtime enrichment) +- **5 fault injection scenarios** with one-click scripts +- **Web UI** for visual entity browsing and fault monitoring + +## Prerequisites + +- Docker and docker-compose +- X11 display server (for RViz GUI) or `--headless` mode +- (Optional) NVIDIA GPU + [nvidia-container-toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html) +- ~5-6 GB disk space for Docker image + +## Quick Start + +### 1. Start the Demo + +```bash +cd demos/moveit_pick_place +./run-demo.sh +``` + +That's it! The script will: +1. Build the Docker image (first run: ~15-20 min, ~5-6 GB) +2. Set up X11 forwarding for RViz GUI +3. Launch Panda robot + MoveIt 2 + ros2_medkit gateway +4. Launch sovd_web_ui at http://localhost:3000 + +**REST API:** http://localhost:8080/api/v1/ +**Web UI:** http://localhost:3000/ + +### 2. Available Options + +```bash +./run-demo.sh # Default: CPU, fake hardware, daemon mode +./run-demo.sh --gazebo # Gazebo Harmonic physics simulation +./run-demo.sh --gazebo --nvidia # Gazebo + GPU acceleration +./run-demo.sh --nvidia # GPU acceleration (fake hardware) +./run-demo.sh --headless # No GUI (CI/server) +./run-demo.sh --attached # Foreground with logs +./run-demo.sh --no-cache # Rebuild without cache +./run-demo.sh --update # Pull latest images first +``` + +**Simulation modes:** +- **Default (fake hardware)** — Mock controllers echo commanded positions instantly. Fast startup (~10s), works headless, no physics. Good for diagnostics testing. +- **Gazebo (`--gazebo`)** — Gazebo Harmonic physics simulation with `gz_ros2_control`. Realistic dynamics, 3D world view. Slower startup (~30s), needs X11 or `--headless`. Recommended with `--nvidia` for GPU acceleration. + +### 3. Moving the Arm + +Use the interactive arm controller to send joint trajectories: + +```bash +./move-arm.sh # Interactive menu +./move-arm.sh ready # Go to ready pose +./move-arm.sh pick # Go to pick pose +./move-arm.sh demo # Run full pick → place → home cycle +``` + +The script sends goals directly to the `panda_arm_controller/follow_joint_trajectory` action. +It works both from outside (via `docker exec`) and from inside the container. + +### 4. Viewing Logs + +```bash +docker compose --profile cpu logs -f # CPU version +docker compose --profile nvidia logs -f # NVIDIA version +docker exec -it moveit_medkit_demo bash # Shell into container +``` + +### 5. Stopping the Demo + +```bash +./stop-demo.sh # Stop containers +./stop-demo.sh --volumes # Stop and remove volumes +./stop-demo.sh --images # Stop and remove images +``` + +## Architecture + +``` +┌─────────────────────────────────────────────────────────────────┐ +│ Docker Container │ +│ │ +│ ┌──────────┐ ┌──────────────┐ ┌──────────────────────────┐ │ +│ │ Fake HW │ │ ros2_control │ │ MoveIt 2 │ │ +│ │ (mock │──│ Controllers │──│ move_group (OMPL) │ │ +│ │ controllers)│ arm+gripper │ │ │ │ +│ └──────────┘ └──────────────┘ └──────────┬───────────────┘ │ +│ │ │ +│ ┌──────────────────────────────┐ │ │ +│ │ pick_place_loop.py │◄───────────┘ │ +│ │ (continuous manipulation) │ MoveGroup action │ +│ └──────────────────────────────┘ │ +│ │ +│ ┌──────────────────────────────────────────────────────────┐ │ +│ │ ros2_medkit Stack │ │ +│ │ │ │ +│ │ manipulation_monitor.py ──► fault_manager ◄── diag_bridge│ │ +│ │ │ │ │ │ +│ │ │ monitors: │ stores faults │ │ +│ │ │ • /move_group status │ │ │ +│ │ │ • /controller status ▼ │ │ +│ │ │ • /joint_states gateway_node ──► REST API │ │ +│ └─────────┴────────────────────────┬───────────────────────┘ │ +│ │ :8080 │ +└─────────────────────────────────────┼────────────────────────────┘ + │ + ┌─────────────┼─────────────┐ + │ │ │ + sovd_web_ui curl/httpie MCP Server + :3000 (LLM tools) +``` + +## Entity Hierarchy (SOVD) + +``` +Areas +├── manipulation/ — Robot arm and gripper hardware +│ Components +│ ├── panda-arm — 7-DOF Franka Emika Panda +│ │ Apps: joint-state-broadcaster, panda-arm-controller, robot-state-publisher +│ └── panda-gripper — 2-finger parallel gripper +│ Apps: panda-hand-controller +│ +├── planning/ — MoveIt 2 motion planning stack +│ Components +│ ├── moveit-planning — OMPL planning pipeline +│ │ Apps: move-group +│ └── task-constructor — Pick-and-place coordinator +│ Apps: mtc-pick-place +│ +├── diagnostics/ — ros2_medkit gateway and fault management +│ Components +│ ├── gateway — REST API +│ │ Apps: medkit-gateway +│ └── fault-manager — Fault aggregation +│ Apps: medkit-fault-manager +│ +└── bridge/ — Legacy diagnostics bridge + Components + └── diagnostic-bridge + Apps: diagnostic-bridge-app, manipulation-monitor + +Functions +├── pick-and-place — Pick objects and place at target positions +├── motion-planning — Plan collision-free motion trajectories +├── gripper-control — Open and close the Panda gripper +└── fault-management — Collect and expose faults via SOVD API +``` + +## REST API Examples + +### Health Check + +```bash +curl http://localhost:8080/api/v1/health | jq +``` + +### Explore Entities + +```bash +# Or use the helper script: ./check-entities.sh + +curl http://localhost:8080/api/v1/areas | jq +curl http://localhost:8080/api/v1/components | jq +curl http://localhost:8080/api/v1/apps | jq +curl http://localhost:8080/api/v1/functions | jq +``` + +### Read Joint States + +```bash +curl http://localhost:8080/api/v1/apps/joint-state-broadcaster/data/joint_states | jq +``` + +### List MoveIt Configurations + +```bash +curl http://localhost:8080/api/v1/apps/move-group/configurations | jq +``` + +### List Available Operations + +```bash +curl http://localhost:8080/api/v1/apps/move-group/operations | jq +``` + +### View Active Faults + +```bash +# Or use the helper script: ./check-faults.sh + +curl http://localhost:8080/api/v1/faults | jq +``` + +### Clear All Faults + +```bash +curl -X DELETE http://localhost:8080/api/v1/faults +``` + +## Fault Injection Scenarios + +### 1. Planning Failure + +Blocks the robot's path with a large collision wall. + +```bash +./inject-planning-failure.sh +``` + +| Code | Severity | Description | +|------|----------|-------------| +| `MOTION_PLANNING_FAILED` | ERROR | MoveGroup goal ABORTED — no collision-free path | + +### 2. Grasp Failure + +Moves the target object far outside the arm's reachable workspace. + +```bash +./inject-grasp-failure.sh +``` + +| Code | Severity | Description | +|------|----------|-------------| +| `MOTION_PLANNING_FAILED` | ERROR | Cannot plan to unreachable target position | + +### 3. Controller Timeout + +Sets extremely tight goal time tolerance on the arm controller. + +```bash +./inject-controller-timeout.sh +``` + +| Code | Severity | Description | +|------|----------|-------------| +| `CONTROLLER_TIMEOUT` | ERROR | Joint trajectory controller timed out | +| `TRAJECTORY_EXECUTION_FAILED` | ERROR | Arm controller ABORTED trajectory | + +### 4. Joint Limit Violation + +Commands the arm to reach extreme joint positions near/beyond URDF limits. + +```bash +./inject-joint-limit.sh +``` + +| Code | Severity | Description | +|------|----------|-------------| +| `JOINT_LIMIT_APPROACHING` | WARN | Joint within 0.1 rad of limit | +| `JOINT_LIMIT_VIOLATED` | ERROR | Joint position beyond URDF limit | + +### 5. Collision Detection + +Spawns a surprise obstacle in the robot's active workspace mid-motion. + +```bash +./inject-collision.sh +``` + +| Code | Severity | Description | +|------|----------|-------------| +| `MOTION_PLANNING_FAILED` | ERROR | Cannot find collision-free path around obstacle | + +### 6. Restore Normal + +Removes all injected objects, restores parameters, and clears faults. + +```bash +./restore-normal.sh +``` + +### Verification + +After any injection, verify faults: + +```bash +./check-faults.sh +# OR +curl http://localhost:8080/api/v1/faults | jq '.items[] | {code, severity, message}' +``` + +## Web UI + +The sovd_web_ui container starts automatically at **http://localhost:3000**. + +Connect it to the gateway at `http://localhost:8080` to browse: +- Entity tree (Areas → Components → Apps) +- Real-time joint state data +- Active faults with severity indicators +- Configuration parameters + +## Utility Scripts + +| Script | Description | +|--------|-------------| +| `move-arm.sh` | **Interactive arm controller** — move to preset positions | +| `check-entities.sh` | Explore the full SOVD entity hierarchy with sample data | +| `check-faults.sh` | View active faults with severity summary | +| `inject-planning-failure.sh` | Block robot path with collision wall | +| `inject-grasp-failure.sh` | Move target object out of reach | +| `inject-controller-timeout.sh` | Set extremely tight goal time tolerance | +| `inject-joint-limit.sh` | Command extreme joint positions | +| `inject-collision.sh` | Spawn surprise obstacle | +| `restore-normal.sh` | Remove all injected faults and restore defaults | + +## Troubleshooting + +| Problem | Cause | Solution | +|---------|-------|----------| +| RViz window doesn't appear | X11 not set up | Run `xhost +local:docker` or use `--headless` | +| "Package not found" error | Build failed | Rebuild with `./run-demo.sh --no-cache` | +| No faults appearing | Monitor not connected | Check `ros2 node list` includes `manipulation_monitor` | +| Docker build fails | Apt package missing | Check if MoveIt 2 Jazzy packages are available | +| "MoveGroup not available" | Slow startup | Wait 60-90 seconds after container starts | +| Controller not loading | Missing config | Verify `moveit_controllers.yaml` is correct | +| Joint states empty | Controllers not loaded | Check `ros2 control list_controllers` inside container | + +## Comparison with Other Demos + +| Feature | Sensor Diagnostics | TurtleBot3 + Nav2 | **MoveIt Pick-and-Place** | +|---------|-------------------|-------------------|---------------------------| +| Robot | Simulated sensors | TurtleBot3 Burger | Panda 7-DOF arm | +| Simulation | None (pure ROS 2) | Gazebo Harmonic | Fake HW or Gazebo Harmonic | +| Task | Sensor monitoring | Autonomous navigation | Pick-and-place manipulation | +| Fault types | Sensor drift, noise | Nav failures, localization | Planning, controller, joint limits | +| Entity complexity | Simple (flat) | Medium (3 areas) | High (4 areas, 7 components) | +| SOVD manifest | No | Yes (hybrid) | Yes (hybrid) | +| Docker image | ~2 GB | ~4 GB | ~5-6 GB | +| GPU recommended | No | Optional | Optional | + +## Technical Details + +### Monitored Topics + +| Topic | What it tells us | Fault codes | +|-------|------------------|-------------| +| `/move_group/_action/status` | Planning success/failure | `MOTION_PLANNING_FAILED` | +| `/panda_arm_controller/follow_joint_trajectory/_action/status` | Trajectory execution | `TRAJECTORY_EXECUTION_FAILED`, `CONTROLLER_TIMEOUT` | +| `/joint_states` | Current joint positions | `JOINT_LIMIT_APPROACHING`, `JOINT_LIMIT_VIOLATED` | + +### Fault Codes Reference + +| Code | Severity | Trigger | +|------|----------|---------| +| `MOTION_PLANNING_FAILED` | ERROR | MoveGroup goal ABORTED | +| `TRAJECTORY_EXECUTION_FAILED` | ERROR | Controller action ABORTED | +| `CONTROLLER_TIMEOUT` | ERROR | Controller action ABORTED (timeout) | +| `JOINT_LIMIT_APPROACHING` | WARN | Joint within warn margin of URDF limit | +| `JOINT_LIMIT_VIOLATED` | ERROR | Joint position beyond URDF limit | + +### Docker Image Contents + +- ROS 2 Jazzy Desktop (Ubuntu 24.04) +- MoveIt 2 + OMPL planner +- Panda URDF + MoveIt config +- Gazebo Harmonic + gz_ros2_control (for `--gazebo` mode) +- ros2_control with mock hardware (for default mode) +- ros2_medkit stack (gateway, fault_manager, diagnostic_bridge) +- Demo package (launch, config, scripts) + +## License + +This project is licensed under the Apache License 2.0. See the [LICENSE](../../LICENSE) file for details. diff --git a/demos/moveit_pick_place/check-entities.sh b/demos/moveit_pick_place/check-entities.sh new file mode 100755 index 0000000..66dfa3a --- /dev/null +++ b/demos/moveit_pick_place/check-entities.sh @@ -0,0 +1,69 @@ +#!/bin/bash +# Explore SOVD entity hierarchy from ros2_medkit gateway +# Demonstrates: Areas → Components → Apps → Functions + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +# Colors for output +BLUE='\033[0;34m' +GREEN='\033[0;32m' +NC='\033[0m' + +echo_step() { + echo -e "\n${BLUE}=== $1 ===${NC}\n" +} + +echo "╔══════════════════════════════════════════════════════════╗" +echo "║ SOVD Entity Hierarchy Explorer ║" +echo "║ MoveIt 2 Panda + ros2_medkit Demo ║" +echo "╚══════════════════════════════════════════════════════════╝" + +# Check for jq dependency +if ! command -v jq >/dev/null 2>&1; then + echo "❌ 'jq' is required but not installed." + echo " Please install jq (e.g., 'sudo apt-get install jq') and retry." + exit 1 +fi + +# Wait for gateway +echo "" +echo "Checking gateway health..." +if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then + echo "❌ Gateway not available at ${GATEWAY_URL}" + echo " Start with: ./run-demo.sh" + exit 1 +fi +echo "✓ Gateway is healthy" + +echo_step "1. Areas (Functional Groupings)" +curl -s "${API_BASE}/areas" | jq '.items[] | {id: .id, name: .name, description: .description}' + +echo_step "2. Components (Hardware/Logical Units)" +curl -s "${API_BASE}/components" | jq '.items[] | {id: .id, name: .name, type: .type, area: .area}' + +echo_step "3. Apps (ROS 2 Nodes)" +curl -s "${API_BASE}/apps" | jq '.items[] | {id: .id, name: .name, category: .category, component: .is_located_on}' + +echo_step "4. Functions (High-level Capabilities)" +curl -s "${API_BASE}/functions" | jq '.items[] | {id: .id, name: .name, category: .category, hosted_by: .hosted_by}' + +echo_step "5. Sample Data (Joint States)" +echo "Getting latest joint states from Panda arm..." +curl -s "${API_BASE}/apps/joint-state-broadcaster/data/joint_states" 2>/dev/null | jq '{ + joint_names: .name, + positions: .position, + velocities: .velocity +}' || echo " (Joint state data not available — robot may still be starting)" + +echo_step "6. Faults" +curl -s "${API_BASE}/faults" | jq '.items[] | {code: .code, severity: .severity, reporter: .reporter_id}' + +echo "" +echo -e "${GREEN}✓ Entity hierarchy exploration complete!${NC}" +echo "" +echo "Try more commands:" +echo " curl ${API_BASE}/apps/move-group/configurations | jq # MoveIt parameters" +echo " curl ${API_BASE}/apps/move-group/operations | jq # MoveGroup operations" +echo " curl ${API_BASE}/components/panda-arm/hosts | jq # Apps on Panda arm" +echo " curl ${API_BASE}/functions/pick-and-place | jq # Pick-and-place function" diff --git a/demos/moveit_pick_place/check-faults.sh b/demos/moveit_pick_place/check-faults.sh new file mode 100755 index 0000000..0374c85 --- /dev/null +++ b/demos/moveit_pick_place/check-faults.sh @@ -0,0 +1,62 @@ +#!/bin/bash +# Check current faults from ros2_medkit gateway +# Faults are collected from MoveIt/Panda via manipulation_monitor + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "🔍 Checking faults from ros2_medkit gateway..." +echo "" + +# Check for jq dependency +if ! command -v jq >/dev/null 2>&1; then + echo "❌ 'jq' is required but not installed." + echo " Please install jq (e.g., 'sudo apt-get install jq') and retry." + exit 1 +fi + +# Wait for gateway +echo "Checking gateway health..." +if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then + echo "❌ Gateway not available at ${GATEWAY_URL}" + echo " Start with: ./run-demo.sh" + exit 1 +fi +echo "✓ Gateway is healthy" +echo "" + +# Get all faults +echo "📋 Active Faults:" +FAULTS=$(curl -s "${API_BASE}/faults") + +# Check if there are any faults +FAULT_COUNT=$(echo "$FAULTS" | jq '.items | length') + +if [ "$FAULT_COUNT" = "0" ]; then + echo " No active faults — system is healthy! ✅" +else + echo "$FAULTS" | jq '.items[] | { + code: .code, + severity: .severity, + reporter: .reporter_id, + message: .message, + timestamp: .timestamp + }' +fi + +echo "" +echo "📊 Fault Summary:" +echo " Total active faults: $FAULT_COUNT" + +# Show fault counts by severity if any exist +if [ "$FAULT_COUNT" != "0" ]; then + echo "" + echo " By severity:" + echo "$FAULTS" | jq -r '.items | group_by(.severity) | .[] | " \(.[0].severity): \(length)"' +fi + +echo "" +echo "Commands:" +echo " Clear all faults: curl -X DELETE ${API_BASE}/faults" +echo " Check area faults: curl ${API_BASE}/areas/manipulation/faults | jq" +echo " Check component faults: curl ${API_BASE}/components/panda-arm/faults | jq" diff --git a/demos/moveit_pick_place/config/medkit_params.yaml b/demos/moveit_pick_place/config/medkit_params.yaml new file mode 100644 index 0000000..2ae4d5b --- /dev/null +++ b/demos/moveit_pick_place/config/medkit_params.yaml @@ -0,0 +1,29 @@ +# ros2_medkit gateway configuration for MoveIt 2 Panda demo +# Node runs under /diagnostics namespace, so we need to match that here +diagnostics: + ros2_medkit_gateway: + ros__parameters: + server: + # Bind to all interfaces for Docker networking + host: "0.0.0.0" + port: 8080 + + refresh_interval_ms: 10000 # 10 seconds (default), reduces log spam + + cors: + allowed_origins: ["*"] + allowed_methods: ["GET", "PUT", "POST", "DELETE", "OPTIONS"] + allowed_headers: ["Content-Type", "Accept"] + allow_credentials: false + max_age_seconds: 86400 + + max_parallel_topic_samples: 10 + + # Discovery configuration + discovery_mode: "hybrid" # runtime_only, manifest_only, or hybrid + manifest_path: "" # Will be set via launch argument + manifest_strict_validation: true + + discovery: + runtime: + create_synthetic_components: false # Manifest defines components diff --git a/demos/moveit_pick_place/config/moveit_controllers.yaml b/demos/moveit_pick_place/config/moveit_controllers.yaml new file mode 100644 index 0000000..13dbd73 --- /dev/null +++ b/demos/moveit_pick_place/config/moveit_controllers.yaml @@ -0,0 +1,35 @@ +# MoveIt 2 controller configuration for Panda arm and gripper +# These are loaded by ros2_control via the controller_manager +controller_manager: + ros__parameters: + update_rate: 100 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + panda_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + panda_hand_controller: + type: position_controllers/GripperActionController + +panda_arm_controller: + ros__parameters: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + +panda_hand_controller: + ros__parameters: + joint: panda_finger_joint1 + action_monitor_rate: 20.0 diff --git a/demos/moveit_pick_place/config/panda_manifest.yaml b/demos/moveit_pick_place/config/panda_manifest.yaml new file mode 100644 index 0000000..5954e17 --- /dev/null +++ b/demos/moveit_pick_place/config/panda_manifest.yaml @@ -0,0 +1,226 @@ +# SOVD Manifest for MoveIt 2 Panda + ros2_medkit Integration Demo +# Defines the entity hierarchy for ros2_medkit gateway +manifest_version: "1.0" + +metadata: + name: "panda-moveit-demo" + description: "Panda robot arm with MoveIt 2 manipulation and ros2_medkit diagnostics" + version: "0.1.0" + +config: + unmanifested_nodes: warn + inherit_runtime_resources: true + +# ============================================================================= +# AREAS - Functional groupings +# ============================================================================= +areas: + - id: manipulation + name: "Manipulation" + description: "Robot arm and gripper hardware" + namespace: / + + - id: planning + name: "Planning" + description: "MoveIt 2 motion planning stack" + namespace: / + + - id: diagnostics + name: "Diagnostics" + description: "ros2_medkit gateway and fault management" + namespace: /diagnostics + + - id: bridge + name: "Bridge" + description: "Legacy diagnostics bridge" + namespace: /bridge + +# ============================================================================= +# COMPONENTS - Hardware/logical units +# ============================================================================= +components: + - id: panda-arm + name: "Panda Arm" + type: "actuator" + description: "7-DOF Franka Emika Panda robot arm" + area: manipulation + + - id: panda-gripper + name: "Panda Gripper" + type: "actuator" + description: "Panda 2-finger parallel gripper" + area: manipulation + + - id: moveit-planning + name: "MoveIt Planning" + type: "controller" + description: "MoveIt 2 motion planning pipeline (OMPL)" + area: planning + + - id: task-constructor + name: "Task Constructor" + type: "controller" + description: "MoveIt Task Constructor pick-and-place coordinator" + area: planning + + - id: gateway + name: "SOVD Gateway" + type: "controller" + description: "ros2_medkit REST API gateway" + area: diagnostics + + - id: fault-manager + name: "Fault Manager" + type: "controller" + description: "ros2_medkit fault aggregation service" + area: diagnostics + + - id: diagnostic-bridge + name: "Diagnostic Bridge" + type: "controller" + description: "Bridges legacy /diagnostics topic to fault_manager" + area: bridge + +# ============================================================================= +# APPS - ROS 2 nodes with runtime binding +# ============================================================================= +apps: + # === Manipulation Apps === + - id: joint-state-broadcaster + name: "Joint State Broadcaster" + category: "driver" + is_located_on: panda-arm + description: "Broadcasts Panda joint states" + ros_binding: + node_name: joint_state_broadcaster + namespace: / + + - id: panda-arm-controller + name: "Panda Arm Controller" + category: "driver" + is_located_on: panda-arm + description: "JointTrajectoryController for 7-DOF arm" + ros_binding: + node_name: panda_arm_controller + namespace: / + + - id: panda-hand-controller + name: "Panda Hand Controller" + category: "driver" + is_located_on: panda-gripper + description: "GripperActionController for 2-finger gripper" + ros_binding: + node_name: panda_hand_controller + namespace: / + + - id: robot-state-publisher + name: "Robot State Publisher" + category: "driver" + is_located_on: panda-arm + description: "Publishes robot TF tree from URDF" + ros_binding: + node_name: robot_state_publisher + namespace: / + + # === Planning Apps === + - id: move-group + name: "MoveGroup" + category: "planning" + is_located_on: moveit-planning + description: "MoveIt 2 motion planning and execution node" + depends_on: + - joint-state-broadcaster + - panda-arm-controller + ros_binding: + node_name: move_group + namespace: / + + - id: mtc-pick-place + name: "Pick-and-Place Loop" + category: "application" + is_located_on: task-constructor + description: "Continuous pick-and-place task executor" + depends_on: + - move-group + - panda-hand-controller + ros_binding: + node_name: pick_place_loop + namespace: / + + # === Diagnostics Apps === + - id: medkit-gateway + name: "ros2_medkit Gateway" + category: "gateway" + is_located_on: gateway + description: "REST API gateway for SOVD protocol" + ros_binding: + node_name: ros2_medkit_gateway + namespace: /diagnostics + + - id: medkit-fault-manager + name: "Fault Manager" + category: "diagnostics" + is_located_on: fault-manager + description: "Manages and stores fault information" + ros_binding: + node_name: fault_manager + namespace: / + + - id: diagnostic-bridge-app + name: "Diagnostic Bridge" + category: "diagnostics" + is_located_on: diagnostic-bridge + description: "Bridges /diagnostics topic (DiagnosticArray) to fault_manager" + ros_binding: + node_name: diagnostic_bridge + namespace: /bridge + + - id: manipulation-monitor + name: "Manipulation Monitor" + category: "diagnostics" + is_located_on: diagnostic-bridge + description: "Monitors MoveIt actions and robot state, reports faults" + depends_on: + - move-group + - panda-arm-controller + ros_binding: + node_name: manipulation_monitor + namespace: /bridge + +# ============================================================================= +# FUNCTIONS - High-level capabilities +# ============================================================================= +functions: + - id: pick-and-place + name: "Pick and Place" + category: "manipulation" + description: "Pick objects and place them at target positions" + hosted_by: + - mtc-pick-place + - move-group + - panda-arm-controller + - panda-hand-controller + + - id: motion-planning + name: "Motion Planning" + category: "planning" + description: "Plan collision-free motion trajectories" + hosted_by: + - move-group + + - id: gripper-control + name: "Gripper Control" + category: "manipulation" + description: "Open and close the Panda gripper" + hosted_by: + - panda-hand-controller + + - id: fault-management + name: "Fault Management" + category: "diagnostics" + description: "Collect and expose fault information via SOVD API" + hosted_by: + - medkit-gateway + - medkit-fault-manager + - diagnostic-bridge-app + - manipulation-monitor diff --git a/demos/moveit_pick_place/config/panda_moveit.yaml b/demos/moveit_pick_place/config/panda_moveit.yaml new file mode 100644 index 0000000..3f930e8 --- /dev/null +++ b/demos/moveit_pick_place/config/panda_moveit.yaml @@ -0,0 +1,11 @@ +# MoveIt 2 planning config overrides for Panda demo +# Only needed if you want to override default MoveIt planning parameters +move_group: + ros__parameters: + planning_scene_monitor: + publish_planning_scene: true + publish_geometry_updates: true + publish_state_updates: true + + # Use OMPL planner + default_planning_pipeline: ompl diff --git a/demos/moveit_pick_place/docker-compose.yml b/demos/moveit_pick_place/docker-compose.yml new file mode 100644 index 0000000..75b87d0 --- /dev/null +++ b/demos/moveit_pick_place/docker-compose.yml @@ -0,0 +1,66 @@ +services: + # CPU-only version (default) + # Use with: docker compose --profile cpu up + moveit-demo: + profiles: ["cpu"] + build: + context: . + dockerfile: Dockerfile + container_name: moveit_medkit_demo + environment: + - DISPLAY=${DISPLAY} + - ROS_DOMAIN_ID=40 + - HEADLESS=${HEADLESS:-false} + - LAUNCH_FILE=${LAUNCH_FILE:-demo.launch.py} + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix:rw + ports: + - "8080:8080" + stdin_open: true + tty: true + command: > + bash -c "source /opt/ros/jazzy/setup.bash && + source /root/demo_ws/install/setup.bash && + ros2 launch moveit_medkit_demo $${LAUNCH_FILE} headless:=$${HEADLESS}" + + # NVIDIA GPU accelerated version + # Use with: docker compose --profile nvidia up + # Requires: nvidia-container-toolkit + # Install: https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html + moveit-demo-nvidia: + profiles: ["nvidia"] + build: + context: . + dockerfile: Dockerfile + container_name: moveit_medkit_demo_nvidia + environment: + - DISPLAY=${DISPLAY} + - ROS_DOMAIN_ID=40 + - HEADLESS=${HEADLESS:-false} + - LAUNCH_FILE=${LAUNCH_FILE:-demo.launch.py} + - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix:rw + ports: + - "8080:8080" + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all + capabilities: [gpu] + stdin_open: true + tty: true + command: > + bash -c "source /opt/ros/jazzy/setup.bash && + source /root/demo_ws/install/setup.bash && + ros2 launch moveit_medkit_demo $${LAUNCH_FILE} headless:=$${HEADLESS}" + + # SOVD Web UI — pre-built from GHCR + sovd-web-ui: + image: ghcr.io/selfpatch/sovd_web_ui:latest + container_name: sovd_web_ui + ports: + - "3000:80" diff --git a/demos/moveit_pick_place/inject-collision.sh b/demos/moveit_pick_place/inject-collision.sh new file mode 100755 index 0000000..2d5d464 --- /dev/null +++ b/demos/moveit_pick_place/inject-collision.sh @@ -0,0 +1,55 @@ +#!/bin/bash +# Inject Collision - Spawn a surprise obstacle in the robot's workspace +# This will cause planning/execution failures due to unexpected collision object + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "🚫 Injecting COLLISION fault..." +echo " Spawning surprise obstacle in robot workspace" +echo "" + +# Check for jq dependency +if ! command -v jq >/dev/null 2>&1; then + echo "❌ 'jq' is required but not installed." + echo " Please install jq (e.g., 'sudo apt-get install jq') and retry." + exit 1 +fi + +# Check gateway +if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then + echo "❌ Gateway not available at ${GATEWAY_URL}" + exit 1 +fi + +# Add a sphere right in the arm's working area +echo "Adding surprise obstacle (sphere) to planning scene..." +docker exec moveit_medkit_demo bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ + world: { + collision_objects: [ + { + id: \"surprise_obstacle\", + header: {frame_id: \"panda_link0\"}, + primitives: [{type: 2, dimensions: [0.15]}], + primitive_poses: [{ + position: {x: 0.4, y: 0.0, z: 0.4}, + orientation: {w: 1.0} + }], + operation: 0 + } + ] + }, + is_diff: true +}'" + +echo "" +echo "✓ Collision fault injected!" +echo "" +echo "Expected faults (via manipulation_monitor → FaultManager):" +echo " - MOTION_PLANNING_FAILED: Cannot find collision-free path" +echo "" +echo "Check faults with: curl ${API_BASE}/faults | jq" +echo "Restore with: ./restore-normal.sh" diff --git a/demos/moveit_pick_place/inject-controller-timeout.sh b/demos/moveit_pick_place/inject-controller-timeout.sh new file mode 100755 index 0000000..a8d2782 --- /dev/null +++ b/demos/moveit_pick_place/inject-controller-timeout.sh @@ -0,0 +1,52 @@ +#!/bin/bash +# Inject Controller Timeout - Set extremely low velocity scaling +# This will cause the arm controller to timeout during trajectory execution + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "🚫 Injecting CONTROLLER TIMEOUT fault..." +echo " Setting extremely tight goal time tolerance (0.001s)" +echo " This forces the controller to abort — it can't reach the target in time" +echo "" + +# Check for jq dependency +if ! command -v jq >/dev/null 2>&1; then + echo "❌ 'jq' is required but not installed." + echo " Please install jq (e.g., 'sudo apt-get install jq') and retry." + exit 1 +fi + +# Check gateway +if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then + echo "❌ Gateway not available at ${GATEWAY_URL}" + exit 1 +fi + +# Set an extremely tight goal_time constraint on the controller +# This causes FollowJointTrajectory to abort because the robot can't +# reach the target within the allowed time window +echo "Setting controller goal_time constraint via ROS 2 parameter..." +docker exec moveit_medkit_demo bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 param set /panda_arm_controller constraints.goal_time 0.001" 2>/dev/null || \ + echo " Warning: Could not set goal_time parameter" + +# Also try setting stopped_velocity_tolerance to near-zero to make it +# harder for the controller to consider the trajectory complete +docker exec moveit_medkit_demo bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 param set /panda_arm_controller constraints.stopped_velocity_tolerance 0.0001" 2>/dev/null || \ + echo " Warning: Could not set stopped_velocity_tolerance parameter" + +echo "" +echo "✓ Controller timeout injected!" +echo "" +echo "Expected faults (via manipulation_monitor → FaultManager):" +echo " - CONTROLLER_TIMEOUT: Joint trajectory controller timed out" +echo " - TRAJECTORY_EXECUTION_FAILED: Arm controller ABORTED trajectory" +echo "" +echo "Check faults with: curl ${API_BASE}/faults | jq" +echo "Restore with: ./restore-normal.sh" diff --git a/demos/moveit_pick_place/inject-grasp-failure.sh b/demos/moveit_pick_place/inject-grasp-failure.sh new file mode 100755 index 0000000..469b0d7 --- /dev/null +++ b/demos/moveit_pick_place/inject-grasp-failure.sh @@ -0,0 +1,55 @@ +#!/bin/bash +# Inject Grasp Failure - Move target object out of robot's workspace +# This will cause MoveGroup to fail planning to an unreachable target + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "🚫 Injecting GRASP FAILURE fault..." +echo " Moving target object far outside robot's workspace" +echo "" + +# Check for jq dependency +if ! command -v jq >/dev/null 2>&1; then + echo "❌ 'jq' is required but not installed." + echo " Please install jq (e.g., 'sudo apt-get install jq') and retry." + exit 1 +fi + +# Check gateway +if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then + echo "❌ Gateway not available at ${GATEWAY_URL}" + exit 1 +fi + +# Move the target cylinder far away from the robot +echo "Moving target object to unreachable position (5.0, 5.0, 0.1)..." +docker exec moveit_medkit_demo bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ + world: { + collision_objects: [ + { + id: \"target_cylinder\", + header: {frame_id: \"panda_link0\"}, + primitives: [{type: 3, dimensions: [0.1, 0.02]}], + primitive_poses: [{ + position: {x: 5.0, y: 5.0, z: 0.1}, + orientation: {w: 1.0} + }], + operation: 0 + } + ] + }, + is_diff: true +}'" + +echo "" +echo "✓ Grasp failure injected!" +echo "" +echo "Expected faults (via manipulation_monitor → FaultManager):" +echo " - MOTION_PLANNING_FAILED: Cannot plan to unreachable target" +echo "" +echo "Check faults with: curl ${API_BASE}/faults | jq" +echo "Restore with: ./restore-normal.sh" diff --git a/demos/moveit_pick_place/inject-joint-limit.sh b/demos/moveit_pick_place/inject-joint-limit.sh new file mode 100755 index 0000000..cb44005 --- /dev/null +++ b/demos/moveit_pick_place/inject-joint-limit.sh @@ -0,0 +1,50 @@ +#!/bin/bash +# Inject Joint Limit Violation - Command extreme joint positions +# This will trigger joint limit approaching/violation faults + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "🚫 Injecting JOINT LIMIT fault..." +echo " Commanding extreme joint positions near limits" +echo "" + +# Check for jq dependency +if ! command -v jq >/dev/null 2>&1; then + echo "❌ 'jq' is required but not installed." + echo " Please install jq (e.g., 'sudo apt-get install jq') and retry." + exit 1 +fi + +# Check gateway +if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then + echo "❌ Gateway not available at ${GATEWAY_URL}" + exit 1 +fi + +# Send a goal with extreme joint positions via ROS 2 CLI +echo "Sending joint trajectory goal near limits..." +docker exec moveit_medkit_demo bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 action send_goal /panda_arm_controller/follow_joint_trajectory \ + control_msgs/action/FollowJointTrajectory '{ + trajectory: { + joint_names: [panda_joint1, panda_joint2, panda_joint3, + panda_joint4, panda_joint5, panda_joint6, panda_joint7], + points: [{ + positions: [2.85, 1.70, 2.85, -0.10, 2.85, 3.70, 2.85], + time_from_start: {sec: 5, nanosec: 0} + }] + } + }'" 2>/dev/null & + +echo "" +echo "✓ Joint limit fault injected!" +echo "" +echo "Expected faults (via manipulation_monitor → FaultManager):" +echo " - JOINT_LIMIT_APPROACHING: Joint near limit (WARN)" +echo " - JOINT_LIMIT_VIOLATED: Joint beyond limit (ERROR)" +echo "" +echo "Check faults with: curl ${API_BASE}/faults | jq" +echo "Restore with: ./restore-normal.sh" diff --git a/demos/moveit_pick_place/inject-planning-failure.sh b/demos/moveit_pick_place/inject-planning-failure.sh new file mode 100755 index 0000000..b3794cf --- /dev/null +++ b/demos/moveit_pick_place/inject-planning-failure.sh @@ -0,0 +1,55 @@ +#!/bin/bash +# Inject Planning Failure - Add collision wall blocking the pick-place path +# This will cause MoveGroup planning to fail + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "🚫 Injecting PLANNING FAILURE fault..." +echo " Adding collision wall between pick and place positions" +echo "" + +# Check for jq dependency +if ! command -v jq >/dev/null 2>&1; then + echo "❌ 'jq' is required but not installed." + echo " Please install jq (e.g., 'sudo apt-get install jq') and retry." + exit 1 +fi + +# Check gateway +if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then + echo "❌ Gateway not available at ${GATEWAY_URL}" + exit 1 +fi + +# Add a large wall via PlanningScene topic inside the container +echo "Adding collision wall to planning scene..." +docker exec moveit_medkit_demo bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ + world: { + collision_objects: [ + { + id: \"injected_wall\", + header: {frame_id: \"panda_link0\"}, + primitives: [{type: 1, dimensions: [2.0, 0.05, 1.0]}], + primitive_poses: [{ + position: {x: 0.3, y: 0.25, z: 0.5}, + orientation: {w: 1.0} + }], + operation: 0 + } + ] + }, + is_diff: true +}'" + +echo "" +echo "✓ Planning failure injected!" +echo "" +echo "Expected faults (via manipulation_monitor → FaultManager):" +echo " - MOTION_PLANNING_FAILED: MoveGroup goal ABORTED" +echo "" +echo "Check faults with: curl ${API_BASE}/faults | jq" +echo "Restore with: ./restore-normal.sh" diff --git a/demos/moveit_pick_place/launch/demo.launch.py b/demos/moveit_pick_place/launch/demo.launch.py new file mode 100644 index 0000000..06bf7f0 --- /dev/null +++ b/demos/moveit_pick_place/launch/demo.launch.py @@ -0,0 +1,149 @@ +"""Launch Panda robot with MoveIt 2 and ros2_medkit for diagnostics demo. + +This launch file starts: + - Panda 7-DOF arm with fake hardware (mock controllers, no physics sim) + - MoveIt 2 move_group for motion planning (via included demo launch) + - RViz for visualization (non-headless mode only) + - Continuous pick-and-place task loop + - ros2_medkit gateway, fault_manager, diagnostic_bridge + - Manipulation anomaly monitor +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get package share directories + demo_pkg_dir = get_package_share_directory("moveit_medkit_demo") + panda_moveit_config_dir = get_package_share_directory( + "moveit_resources_panda_moveit_config" + ) + + # Config file paths + medkit_params_file = os.path.join(demo_pkg_dir, "config", "medkit_params.yaml") + manifest_file = os.path.join(demo_pkg_dir, "config", "panda_manifest.yaml") + + # Launch configuration variables + use_sim_time = LaunchConfiguration("use_sim_time", default="False") + headless = LaunchConfiguration("headless", default="False") + + return LaunchDescription( + [ + # Declare launch arguments + DeclareLaunchArgument( + "use_sim_time", + default_value="False", + description="Use simulation clock if true (False for fake hardware)", + ), + DeclareLaunchArgument( + "headless", + default_value="False", + description="Run without GUI (True for Docker/CI)", + ), + # Suppress GUI environment when headless + SetEnvironmentVariable( + name="QT_QPA_PLATFORM", + value="offscreen", + condition=IfCondition(headless), + ), + # Include MoveIt 2 Panda demo launch (with GUI) + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + panda_moveit_config_dir, "launch", "demo.launch.py" + ) + ), + launch_arguments={ + "use_sim_time": use_sim_time, + "use_rviz": "true", + }.items(), + condition=UnlessCondition(headless), + ), + # Include MoveIt 2 Panda demo launch (headless — no RViz) + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + panda_moveit_config_dir, "launch", "demo.launch.py" + ) + ), + launch_arguments={ + "use_sim_time": use_sim_time, + "use_rviz": "false", + }.items(), + condition=IfCondition(headless), + ), + # === ros2_medkit stack === + # Fault manager — root namespace + Node( + package="ros2_medkit_fault_manager", + executable="fault_manager_node", + name="fault_manager", + namespace="", + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + ), + # Diagnostic bridge — /bridge namespace + # auto_generate_codes=False prevents noisy controller_manager + # periodicity diagnostics from being reported as faults. + # The manipulation_monitor handles all demo-relevant faults directly. + Node( + package="ros2_medkit_diagnostic_bridge", + executable="diagnostic_bridge_node", + name="diagnostic_bridge", + namespace="bridge", + output="screen", + parameters=[ + { + "use_sim_time": use_sim_time, + "auto_generate_codes": False, + } + ], + ), + # Gateway — /diagnostics namespace + Node( + package="ros2_medkit_gateway", + executable="gateway_node", + name="ros2_medkit_gateway", + namespace="diagnostics", + output="screen", + parameters=[ + medkit_params_file, + { + "use_sim_time": use_sim_time, + "manifest_path": manifest_file, + }, + ], + ), + # === Demo scripts === + # Pick-and-place loop + Node( + package="moveit_medkit_demo", + executable="pick_place_loop.py", + name="pick_place_loop", + namespace="", + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + ), + # Manipulation monitor (anomaly detector) + Node( + package="moveit_medkit_demo", + executable="manipulation_monitor.py", + name="manipulation_monitor", + namespace="bridge", + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + ) diff --git a/demos/moveit_pick_place/launch/demo_gazebo.launch.py b/demos/moveit_pick_place/launch/demo_gazebo.launch.py new file mode 100644 index 0000000..34c777e --- /dev/null +++ b/demos/moveit_pick_place/launch/demo_gazebo.launch.py @@ -0,0 +1,368 @@ +"""Launch Panda robot with MoveIt 2, Gazebo Harmonic, and ros2_medkit. + +Runs the Panda 7-DOF arm in Gazebo Harmonic physics simulation with +gz_ros2_control hardware interface. Provides realistic joint dynamics +and 3D visualization via Gazebo client. + +This launch file starts: + - Gazebo Harmonic with empty world (or server-only in headless mode) + - Panda robot spawned via gz_ros2_control plugin (fixed to world) + - ros2_control controllers (joint_state_broadcaster, arm, gripper) + - MoveIt 2 move_group for motion planning + - Continuous pick-and-place task loop + - ros2_medkit gateway, fault_manager, diagnostic_bridge + - Manipulation anomaly monitor + +Prerequisites: + ros-jazzy-ros-gz-sim, ros-jazzy-ros-gz-bridge, ros-jazzy-gz-ros2-control +""" + +import os + +import xacro +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + SetEnvironmentVariable, + TimerAction, +) +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # ── Package directories ────────────────────────────────────────── + demo_pkg_dir = get_package_share_directory("moveit_medkit_demo") + panda_config_dir = get_package_share_directory( + "moveit_resources_panda_moveit_config" + ) + + # Config file paths — ros2_control controller definitions + controllers_file = os.path.join( + demo_pkg_dir, "config", "moveit_controllers.yaml" + ) + medkit_params_file = os.path.join( + demo_pkg_dir, "config", "medkit_params.yaml" + ) + manifest_file = os.path.join( + demo_pkg_dir, "config", "panda_manifest.yaml" + ) + + headless = LaunchConfiguration("headless", default="False") + + # ── Robot description (URDF with ros2_control + Gazebo hardware) ─ + # Use the xacro from panda_moveit_config/config/ which includes + # ros2_control definitions and accepts ros2_control_hardware_type. + # NOTE: The panda xacro only supports "mock_components" and "isaac" + # hardware types. We generate with mock_components and then replace + # the plugin name with gz_ros2_control/GazeboSimSystem. + xacro_file = os.path.join( + panda_config_dir, "config", "panda.urdf.xacro" + ) + robot_description_raw = xacro.process_file( + xacro_file, + mappings={ + "ros2_control_hardware_type": "mock_components", + }, + ).toxml() + + # Swap mock_components hardware plugin for Gazebo simulation plugin + robot_description_raw = robot_description_raw.replace( + "mock_components/GenericSystem", + "gz_ros2_control/GazeboSimSystem", + ) + + # Inject gz_ros2_control Gazebo plugin if not already in the URDF + if "GazeboSimROS2ControlPlugin" not in robot_description_raw: + gz_plugin = ( + " \n" + ' \n' + f" {controllers_file}\n" + " \n" + " \n" + ) + robot_description_raw = robot_description_raw.replace( + "", gz_plugin + "" + ) + + # Anchor the robot base to the ground plane so it doesn't fall over. + # This also provides the world→panda_link0 TF that MoveIt's + # virtual_joint requires (published by robot_state_publisher). + if '\n' + ' \n' + ' \n' + ' \n' + ' \n' + ) + robot_description_raw = robot_description_raw.replace( + "", world_joint + "" + ) + + robot_description = {"robot_description": robot_description_raw} + + # ── SRDF ───────────────────────────────────────────────────────── + srdf_file = os.path.join(panda_config_dir, "config", "panda.srdf") + with open(srdf_file, "r") as f: + robot_description_semantic = { + "robot_description_semantic": f.read() + } + + # ── Kinematics ─────────────────────────────────────────────────── + kinematics_file = os.path.join( + panda_config_dir, "config", "kinematics.yaml" + ) + with open(kinematics_file, "r") as f: + kinematics_config = yaml.safe_load(f) + + # ── OMPL planning pipeline ─────────────────────────────────────── + ompl_file = os.path.join( + panda_config_dir, "config", "ompl_planning.yaml" + ) + with open(ompl_file, "r") as f: + ompl_config = yaml.safe_load(f) + + # ── Joint limits ───────────────────────────────────────────────── + joint_limits_file = os.path.join( + panda_config_dir, "config", "joint_limits.yaml" + ) + with open(joint_limits_file, "r") as f: + joint_limits_config = yaml.safe_load(f) + + # ── MoveIt trajectory execution / controller manager config ────── + moveit_ctrl_file = os.path.join( + panda_config_dir, "config", "gripper_moveit_controllers.yaml" + ) + with open(moveit_ctrl_file, "r") as f: + moveit_controllers_config = yaml.safe_load(f) + + # ── Assemble move_group parameters (MoveIt-namespaced) ─────────── + move_group_params = {} + move_group_params.update(robot_description) + move_group_params.update(robot_description_semantic) + # Kinematics under robot_description_kinematics namespace + move_group_params["robot_description_kinematics"] = kinematics_config + # Joint limits under robot_description_planning namespace + move_group_params["robot_description_planning"] = joint_limits_config + # Pipeline names under planning_pipelines; OMPL config at root level + # (MoveIt looks for "ompl.planning_plugins", not + # "planning_pipelines.ompl.planning_plugins") + move_group_params["planning_pipelines"] = { + "pipeline_names": ["ompl"], + } + move_group_params["ompl"] = ompl_config + # Trajectory execution & controller manager config (top-level keys) + move_group_params.update(moveit_controllers_config) + move_group_params.update( + { + "use_sim_time": True, + "planning_scene_monitor.publish_planning_scene": True, + "planning_scene_monitor.publish_geometry_updates": True, + "planning_scene_monitor.publish_state_updates": True, + } + ) + + # ═════════════════════════════════════════════════════════════════ + return LaunchDescription( + [ + # ── Arguments ──────────────────────────────────────────── + DeclareLaunchArgument( + "headless", + default_value="False", + description="Run without GUI (True for Docker/CI)", + ), + # Suppress Qt when headless + SetEnvironmentVariable( + name="QT_QPA_PLATFORM", + value="offscreen", + condition=IfCondition(headless), + ), + # ── Gazebo Harmonic (GUI) ──────────────────────────────── + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ] + ), + launch_arguments={"gz_args": "-r empty.sdf"}.items(), + condition=UnlessCondition(headless), + ), + # ── Gazebo Harmonic (headless — server only) ───────────── + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ] + ), + launch_arguments={"gz_args": "-r -s empty.sdf"}.items(), + condition=IfCondition(headless), + ), + # ── Clock bridge (Gazebo → ROS 2) ──────────────────────── + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock" + ], + output="screen", + ), + # ── Robot state publisher ──────────────────────────────── + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description, {"use_sim_time": True}], + ), + # ── Spawn robot in Gazebo ──────────────────────────────── + TimerAction( + period=3.0, + actions=[ + Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-topic", + "robot_description", + "-name", + "panda", + "-allow_renaming", + "true", + ], + output="screen", + ), + ], + ), + # ── Spawn controllers ──────────────────────────────────── + TimerAction( + period=8.0, + actions=[ + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "-c", + "/controller_manager", + ], + ), + ], + ), + TimerAction( + period=10.0, + actions=[ + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "panda_arm_controller", + "-c", + "/controller_manager", + ], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "panda_hand_controller", + "-c", + "/controller_manager", + ], + ), + ], + ), + # ── MoveGroup ──────────────────────────────────────────── + TimerAction( + period=14.0, + actions=[ + Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[move_group_params], + ), + ], + ), + # ═════════════════════════════════════════════════════════ + # ros2_medkit stack + # ═════════════════════════════════════════════════════════ + Node( + package="ros2_medkit_fault_manager", + executable="fault_manager_node", + name="fault_manager", + namespace="", + output="screen", + parameters=[{"use_sim_time": True}], + ), + Node( + package="ros2_medkit_diagnostic_bridge", + executable="diagnostic_bridge_node", + name="diagnostic_bridge", + namespace="bridge", + output="screen", + parameters=[ + { + "use_sim_time": True, + "auto_generate_codes": False, + } + ], + ), + Node( + package="ros2_medkit_gateway", + executable="gateway_node", + name="ros2_medkit_gateway", + namespace="diagnostics", + output="screen", + parameters=[ + medkit_params_file, + { + "use_sim_time": True, + "manifest_path": manifest_file, + }, + ], + ), + # ═════════════════════════════════════════════════════════ + # Demo scripts + # ═════════════════════════════════════════════════════════ + TimerAction( + period=18.0, + actions=[ + Node( + package="moveit_medkit_demo", + executable="pick_place_loop.py", + name="pick_place_loop", + namespace="", + output="screen", + parameters=[{"use_sim_time": True}], + ), + ], + ), + Node( + package="moveit_medkit_demo", + executable="manipulation_monitor.py", + name="manipulation_monitor", + namespace="bridge", + output="screen", + parameters=[{"use_sim_time": True}], + ), + ] + ) diff --git a/demos/moveit_pick_place/move-arm.sh b/demos/moveit_pick_place/move-arm.sh new file mode 100755 index 0000000..120f684 --- /dev/null +++ b/demos/moveit_pick_place/move-arm.sh @@ -0,0 +1,152 @@ +#!/bin/bash +# Move the Panda arm to preset positions via ros2_control action interface. +# Works with fake hardware (mock controllers) — no MoveIt planning needed. +# +# Usage: +# ./move-arm.sh # Interactive menu +# ./move-arm.sh ready # Go to ready pose +# ./move-arm.sh extended # Extend arm forward +# ./move-arm.sh pick # Go to pick pose +# ./move-arm.sh place # Go to place pose +# ./move-arm.sh home # All joints to zero + +set -eu + +CONTAINER="${CONTAINER_NAME:-moveit_medkit_demo}" +ACTION="/panda_arm_controller/follow_joint_trajectory" +JOINT_NAMES='["panda_joint1","panda_joint2","panda_joint3","panda_joint4","panda_joint5","panda_joint6","panda_joint7"]' + +# Duration in seconds for trajectory execution +DURATION_SEC=3 + +# --- Preset joint positions (radians) --- +# Ready: default MoveIt pose (from SRDF) +READY="[0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]" + +# Home: all joints at zero +HOME="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" + +# Extended: arm stretched forward +EXTENDED="[0.0, -0.3, 0.0, -1.5, 0.0, 1.2, 0.785]" + +# Pick: reaching down to pick position +PICK="[0.0, -0.5, 0.0, -2.0, 0.0, 1.5, 0.785]" + +# Place: rotated to place position +PLACE="[1.2, -0.5, 0.0, -2.0, 0.0, 1.5, 0.785]" + +# Left: arm rotated left +LEFT="[-1.5, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]" + +# Right: arm rotated right +RIGHT="[1.5, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]" + +# Wave: arm raised for waving +WAVE="[0.0, -1.0, 0.0, -0.5, 0.0, 2.5, 0.785]" + + +send_trajectory() { + local positions="$1" + local label="$2" + + echo "🤖 Moving to: ${label}" + echo " Joints: ${positions}" + echo "" + + # Build FollowJointTrajectory goal message + local goal_msg="{ + trajectory: { + joint_names: ${JOINT_NAMES}, + points: [{ + positions: ${positions}, + time_from_start: {sec: ${DURATION_SEC}, nanosec: 0} + }] + } + }" + + # Check if we're inside the container or outside + if command -v ros2 &> /dev/null && ros2 node list &> /dev/null 2>&1; then + # Inside the container (or ROS 2 env is set up) + ros2 action send_goal "${ACTION}" \ + control_msgs/action/FollowJointTrajectory \ + "${goal_msg}" \ + --feedback + else + # Outside — exec into container + docker exec -it "${CONTAINER}" bash -c " + source /opt/ros/jazzy/setup.bash && \ + source /root/demo_ws/install/setup.bash && \ + ros2 action send_goal ${ACTION} \ + control_msgs/action/FollowJointTrajectory \ + \"${goal_msg}\" \ + --feedback + " + fi + + echo "" + echo "✅ Done: ${label}" +} + +show_menu() { + echo "" + echo "🤖 Panda Arm Controller" + echo "========================" + echo "" + echo "Preset positions:" + echo " 1) ready — Default MoveIt pose (relaxed)" + echo " 2) home — All joints at zero" + echo " 3) extended — Arm stretched forward" + echo " 4) pick — Reaching down to pick" + echo " 5) place — Rotated to place position" + echo " 6) left — Arm rotated left" + echo " 7) right — Arm rotated right" + echo " 8) wave — Arm raised high" + echo "" + echo " d) demo — Run full pick-place-home cycle" + echo " q) quit" + echo "" +} + +run_demo_cycle() { + echo "🔄 Running pick → place → home cycle..." + echo "" + send_trajectory "${PICK}" "pick" + sleep 2 + send_trajectory "${PLACE}" "place" + sleep 2 + send_trajectory "${READY}" "ready (home)" + echo "" + echo "🔄 Cycle complete!" +} + +handle_choice() { + local choice="$1" + case "$choice" in + 1|ready) send_trajectory "${READY}" "ready" ;; + 2|home) send_trajectory "${HOME}" "home" ;; + 3|extended) send_trajectory "${EXTENDED}" "extended" ;; + 4|pick) send_trajectory "${PICK}" "pick" ;; + 5|place) send_trajectory "${PLACE}" "place" ;; + 6|left) send_trajectory "${LEFT}" "left" ;; + 7|right) send_trajectory "${RIGHT}" "right" ;; + 8|wave) send_trajectory "${WAVE}" "wave" ;; + d|demo) run_demo_cycle ;; + q|quit|exit) echo "Bye!"; exit 0 ;; + *) echo "Unknown option: ${choice}" ;; + esac +} + +# --- Main --- + +# If argument provided, run directly +if [[ $# -gt 0 ]]; then + handle_choice "$1" + exit 0 +fi + +# Interactive mode +while true; do + show_menu + read -rp "Choose position (1-8, d, q): " choice + handle_choice "${choice}" +done diff --git a/demos/moveit_pick_place/package.xml b/demos/moveit_pick_place/package.xml new file mode 100644 index 0000000..d4e805d --- /dev/null +++ b/demos/moveit_pick_place/package.xml @@ -0,0 +1,29 @@ + + + + moveit_medkit_demo + 0.1.0 + MoveIt 2 Panda + ros2_medkit integration demo with pick-and-place manipulation + bburda + Apache-2.0 + + ament_cmake + + ros2launch + moveit_ros_planning_interface + moveit_resources_panda_moveit_config + moveit_resources_panda_description + ros2_control + ros2_controllers + joint_state_publisher + ros_gz_sim + ros_gz_bridge + gz_ros2_control + ros2_medkit_gateway + ros2_medkit_fault_manager + ros2_medkit_diagnostic_bridge + + + ament_cmake + + diff --git a/demos/moveit_pick_place/restore-normal.sh b/demos/moveit_pick_place/restore-normal.sh new file mode 100755 index 0000000..6924673 --- /dev/null +++ b/demos/moveit_pick_place/restore-normal.sh @@ -0,0 +1,80 @@ +#!/bin/bash +# Restore Normal Operation - Reset all injected faults +# Use this after running any inject-*.sh script + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "🔄 Restoring NORMAL operation..." +echo "" + +# Check for jq dependency +if ! command -v jq >/dev/null 2>&1; then + echo "❌ 'jq' is required but not installed." + echo " Please install jq (e.g., 'sudo apt-get install jq') and retry." + exit 1 +fi + +# Check gateway +if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then + echo "❌ Gateway not available at ${GATEWAY_URL}" + exit 1 +fi + +# 1. Remove injected collision objects +echo "Removing injected collision objects..." +docker exec moveit_medkit_demo bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ + world: { + collision_objects: [ + {id: \"injected_wall\", operation: 1}, + {id: \"surprise_obstacle\", operation: 1} + ] + }, + is_diff: true +}'" + +# 2. Restore target object to original position +echo "Restoring target object to pick position..." +docker exec moveit_medkit_demo bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ + world: { + collision_objects: [ + { + id: \"target_cylinder\", + header: {frame_id: \"panda_link0\"}, + primitives: [{type: 3, dimensions: [0.1, 0.02]}], + primitive_poses: [{ + position: {x: 0.5, y: 0.0, z: 0.1}, + orientation: {w: 1.0} + }], + operation: 0 + } + ] + }, + is_diff: true +}'" + +# 3. Restore controller parameters +echo "Restoring controller parameters..." +docker exec moveit_medkit_demo bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 param set /panda_arm_controller constraints.goal_time 0.0 && \ +ros2 param set /panda_arm_controller constraints.stopped_velocity_tolerance 0.01" 2>/dev/null || true + +# 4. Clear all faults +echo "Clearing all faults..." +curl -s -X DELETE "${API_BASE}/faults" > /dev/null + +echo "" +echo "✓ Normal operation restored!" +echo "" +echo "Current fault status:" +curl -s "${API_BASE}/faults" | jq '.items | length' | xargs -I {} echo " Active faults: {}" +echo "" +echo "Robot is ready for normal operation." diff --git a/demos/moveit_pick_place/run-demo.sh b/demos/moveit_pick_place/run-demo.sh new file mode 100755 index 0000000..573bec2 --- /dev/null +++ b/demos/moveit_pick_place/run-demo.sh @@ -0,0 +1,177 @@ +#!/bin/bash +# MoveIt 2 Panda + ros2_medkit Demo Runner + +set -eu + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "$SCRIPT_DIR" + +echo "🤖 MoveIt 2 Panda + ros2_medkit Demo" +echo "======================================" + +# Check for Docker +if ! command -v docker &> /dev/null; then + echo "Error: Docker is not installed" + exit 1 +fi + +# Setup X11 forwarding for RViz GUI +echo "Setting up X11 forwarding..." +xhost +local:docker 2>/dev/null || { + echo " Warning: xhost failed. GUI may not work." + echo " Install with: sudo apt install x11-xserver-utils" +} + +# Cleanup function +cleanup() { + echo "" + echo "Cleaning up..." + xhost -local:docker 2>/dev/null || true + echo "Done!" +} +trap cleanup EXIT + +# Parse arguments +COMPOSE_ARGS="" +BUILD_ARGS="" +HEADLESS_MODE="false" +UPDATE_IMAGES="false" +DETACH_MODE="true" +GAZEBO_MODE="true" + +usage() { + echo "Usage: $0 [OPTIONS]" + echo "" + echo "Options:" + echo " --nvidia Use NVIDIA GPU acceleration" + echo " --fake Use fake hardware (mock controllers, no physics)" + echo " --no-cache Build Docker images without cache" + echo " --headless Run without GUI (default: GUI enabled)" + echo " --update Pull latest images before running" + echo " --attached Run in foreground (default: daemon mode)" + echo " -h, --help Show this help message" + echo "" + echo "Examples:" + echo " $0 # Daemon mode with Gazebo (default)" + echo " $0 --attached # Foreground with logs" + echo " $0 --headless # Headless mode (no GUI)" + echo " $0 --fake # Fake hardware (no physics sim)" + echo " $0 --nvidia # GPU acceleration" + echo " $0 --no-cache # Rebuild without cache" + echo " $0 --update # Pull and run latest version" + echo "" + echo "Environment variables:" + echo " HEADLESS=true|false Control GUI mode (default: false)" +} + +while [[ $# -gt 0 ]]; do + case "$1" in + --nvidia) + echo "Using NVIDIA GPU acceleration" + COMPOSE_ARGS="--profile nvidia" + ;; + --no-cache) + echo "Building without cache" + BUILD_ARGS="--no-cache" + ;; + --headless) + echo "Running in headless mode (no GUI)" + HEADLESS_MODE="true" + ;; + --gazebo) + # Kept for backward compatibility (Gazebo is now default) + GAZEBO_MODE="true" + ;; + --fake) + echo "Using fake hardware (mock controllers)" + GAZEBO_MODE="false" + ;; + --update) + echo "Will pull latest images" + UPDATE_IMAGES="true" + ;; + --attached) + echo "Running in foreground mode" + DETACH_MODE="false" + ;; + -h|--help) + usage + exit 0 + ;; + *) + echo "Unknown option: $1" + usage + exit 1 + ;; + esac + shift +done + +if [[ -z "$COMPOSE_ARGS" ]]; then + echo "Using CPU-only mode (use --nvidia flag for GPU acceleration)" + COMPOSE_ARGS="--profile cpu" +fi + +# Export for docker-compose +export HEADLESS=$HEADLESS_MODE +if [[ "$GAZEBO_MODE" == "true" ]]; then + export LAUNCH_FILE="demo_gazebo.launch.py" + echo "Simulation: Gazebo Harmonic (physics simulation)" +else + export LAUNCH_FILE="demo.launch.py" + echo "Simulation: fake hardware (mock controllers, RViz only)" +fi +echo "Display mode: $([ "$HEADLESS_MODE" = "true" ] && echo "headless (no GUI)" || echo "GUI enabled")" +echo "Run mode: $([ "$DETACH_MODE" = "true" ] && echo "daemon (background)" || echo "attached (foreground)")" + +# Build and run +echo " Building and starting demo..." +echo " (First run takes ~15-20 min, downloading ~5-6GB image)" +echo "" +echo "🌐 REST API available at: http://localhost:8080/api/v1/" +echo "🌐 Web UI available at: http://localhost:3000/" +echo "" + +# Pull images if --update flag is set +if [[ "$UPDATE_IMAGES" == "true" ]]; then + echo "📥 Pulling latest images..." + if docker compose version &> /dev/null; then + # shellcheck disable=SC2086 + docker compose ${COMPOSE_ARGS} pull + else + # shellcheck disable=SC2086 + docker-compose ${COMPOSE_ARGS} pull + fi + echo "" +fi + +# Set detach flag +DETACH_FLAG="" +if [[ "$DETACH_MODE" == "true" ]]; then + DETACH_FLAG="-d" +fi + +if docker compose version &> /dev/null; then + # shellcheck disable=SC2086 + docker compose ${COMPOSE_ARGS} build ${BUILD_ARGS} && \ + docker compose ${COMPOSE_ARGS} up ${DETACH_FLAG} +else + # shellcheck disable=SC2086 + docker-compose ${COMPOSE_ARGS} build ${BUILD_ARGS} && \ + docker-compose ${COMPOSE_ARGS} up ${DETACH_FLAG} +fi + +if [[ "$DETACH_MODE" == "true" ]]; then + echo "" + echo "✅ Demo started in background!" + echo "" + echo "📊 To view logs:" + echo " docker compose --profile cpu logs -f # CPU version" + echo " docker compose --profile nvidia logs -f # NVIDIA version" + echo "" + echo "🔧 To interact with ROS 2:" + echo " docker exec -it moveit_medkit_demo bash # CPU" + echo " docker exec -it moveit_medkit_demo_nvidia bash # NVIDIA" + echo "" + echo "🛑 To stop: ./stop-demo.sh" +fi diff --git a/demos/moveit_pick_place/scripts/manipulation_monitor.py b/demos/moveit_pick_place/scripts/manipulation_monitor.py new file mode 100755 index 0000000..e1847ed --- /dev/null +++ b/demos/moveit_pick_place/scripts/manipulation_monitor.py @@ -0,0 +1,352 @@ +#!/usr/bin/env python3 +# Copyright 2026 selfpatch +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# 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 governing permissions and +# limitations under the License. + +""" +Manipulation Monitor Node for MoveIt 2 Panda Demo. + +Monitors robot manipulation for faults and reports to FaultManager: +- Planning failure (MoveGroup action aborted) -> MOTION_PLANNING_FAILED (ERROR) +- Trajectory execution failure -> TRAJECTORY_EXECUTION_FAILED (ERROR) +- Controller timeout -> CONTROLLER_TIMEOUT (ERROR) +- Joint limit approaching/violation -> JOINT_LIMIT_APPROACHING (WARN) + -> JOINT_LIMIT_VIOLATED (ERROR) + +Reports faults via /fault_manager/report_fault service. +""" + +from typing import Dict, Optional + +import rclpy +from rclpy.node import Node +from rclpy.time import Time +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy + +from action_msgs.msg import GoalStatusArray, GoalStatus +from sensor_msgs.msg import JointState +from ros2_medkit_msgs.srv import ReportFault + + +# Severity levels (from ReportFault.srv) +SEVERITY_INFO = 0 +SEVERITY_WARN = 1 +SEVERITY_ERROR = 2 +SEVERITY_CRITICAL = 3 + +# Event types +EVENT_FAILED = 0 +EVENT_PASSED = 1 + +# Panda arm joint limits (from URDF) +PANDA_JOINT_LIMITS = { + 'panda_joint1': (-2.8973, 2.8973), + 'panda_joint2': (-1.7628, 1.7628), + 'panda_joint3': (-2.8973, 2.8973), + 'panda_joint4': (-3.0718, -0.0698), + 'panda_joint5': (-2.8973, 2.8973), + 'panda_joint6': (-0.0175, 3.7525), + 'panda_joint7': (-2.8973, 2.8973), +} + +# How close to the limit before warning (radians) +JOINT_LIMIT_WARN_MARGIN = 0.1 # ~5.7 degrees + + +class ManipulationMonitor(Node): + """Monitors MoveIt 2 manipulation for faults.""" + + def __init__(self): + super().__init__('manipulation_monitor') + + # Parameters + self.declare_parameter('joint_limit_warn_margin', JOINT_LIMIT_WARN_MARGIN) + self.declare_parameter('check_interval_sec', 1.0) + self.declare_parameter('report_throttle_sec', 5.0) + + self.joint_limit_warn_margin = self.get_parameter( + 'joint_limit_warn_margin' + ).value + check_interval = self.get_parameter('check_interval_sec').value + self.report_throttle_sec = self.get_parameter('report_throttle_sec').value + + # --- Subscribers --- + + # QoS for action status (transient local) + status_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + depth=10, + ) + + # MoveGroup action status — for planning failures + self.move_group_status_sub = self.create_subscription( + GoalStatusArray, + '/move_group/_action/status', + self.move_group_status_callback, + status_qos, + ) + + # Controller action status — for trajectory execution failures + self.controller_status_sub = self.create_subscription( + GoalStatusArray, + '/panda_arm_controller/follow_joint_trajectory/_action/status', + self.controller_status_callback, + status_qos, + ) + + # Joint states — for joint limit monitoring + self.joint_state_sub = self.create_subscription( + JointState, + '/joint_states', + self.joint_state_callback, + 10, + ) + + # --- Fault reporting service client --- + self.fault_client = self.create_client( + ReportFault, '/fault_manager/report_fault' + ) + self.get_logger().info( + 'Waiting for /fault_manager/report_fault service...' + ) + while not self.fault_client.wait_for_service(timeout_sec=5.0): + self.get_logger().warn('Service not available, waiting...') + self.get_logger().info('Connected to fault_manager service') + + # --- State tracking --- + self.move_group_goal_status: Dict[str, int] = {} + self.controller_goal_status: Dict[str, int] = {} + self.active_faults: set = set() + # Track which specific joints are in violation/approaching limits + self.joints_violating: set = set() + self.joints_approaching: set = set() + + # Throttling + self.last_report_times: Dict[str, Time] = {} + + # Timer for periodic checks + self.check_timer = self.create_timer(check_interval, self.periodic_check) + + self.get_logger().info('ManipulationMonitor started') + + # ----------------------------------------------------------------- + # Subscriber callbacks + # ----------------------------------------------------------------- + + def move_group_status_callback(self, msg: GoalStatusArray): + """Monitor MoveGroup action for planning/execution failures.""" + for status in msg.status_list: + goal_id = self._goal_id_str(status) + last = self.move_group_goal_status.get(goal_id) + + if last != status.status: + self.move_group_goal_status[goal_id] = status.status + + if status.status == GoalStatus.STATUS_ABORTED: + self.report_fault( + 'MOTION_PLANNING_FAILED', + SEVERITY_ERROR, + f'MoveGroup goal ABORTED — planning or execution ' + f'failed (goal: {goal_id[:8]})', + EVENT_FAILED, + ) + elif status.status == GoalStatus.STATUS_SUCCEEDED: + self.report_fault( + 'MOTION_PLANNING_FAILED', + SEVERITY_INFO, + 'MoveGroup goal succeeded', + EVENT_PASSED, + ) + + def controller_status_callback(self, msg: GoalStatusArray): + """Monitor arm controller for trajectory execution failures.""" + for status in msg.status_list: + goal_id = self._goal_id_str(status) + last = self.controller_goal_status.get(goal_id) + + if last != status.status: + self.controller_goal_status[goal_id] = status.status + + if status.status == GoalStatus.STATUS_ABORTED: + self.report_fault( + 'TRAJECTORY_EXECUTION_FAILED', + SEVERITY_ERROR, + f'Arm controller ABORTED trajectory ' + f'(goal: {goal_id[:8]})', + EVENT_FAILED, + ) + self.report_fault( + 'CONTROLLER_TIMEOUT', + SEVERITY_ERROR, + 'Joint trajectory controller timed out or failed', + EVENT_FAILED, + ) + elif status.status == GoalStatus.STATUS_SUCCEEDED: + self.report_fault( + 'TRAJECTORY_EXECUTION_FAILED', + SEVERITY_INFO, + 'Trajectory execution succeeded', + EVENT_PASSED, + ) + self.report_fault( + 'CONTROLLER_TIMEOUT', + SEVERITY_INFO, + 'Controller operating normally', + EVENT_PASSED, + ) + + def joint_state_callback(self, msg: JointState): + """Check joint positions against limits.""" + for i, name in enumerate(msg.name): + if name not in PANDA_JOINT_LIMITS: + continue + + position = msg.position[i] + lower, upper = PANDA_JOINT_LIMITS[name] + + dist_to_lower = position - lower + dist_to_upper = upper - position + + if dist_to_lower < 0 or dist_to_upper < 0: + # Joint beyond limit + self.joints_violating.add(name) + self.joints_approaching.discard(name) + self.report_fault_throttled( + 'JOINT_LIMIT_VIOLATED', + SEVERITY_ERROR, + f'Joint {name} at {position:.3f} rad — ' + f'BEYOND limits [{lower:.3f}, {upper:.3f}]', + EVENT_FAILED, + ) + elif ( + dist_to_lower < self.joint_limit_warn_margin + or dist_to_upper < self.joint_limit_warn_margin + ): + # Joint approaching limit + self.joints_approaching.add(name) + self.joints_violating.discard(name) + margin = min(dist_to_lower, dist_to_upper) + self.report_fault_throttled( + 'JOINT_LIMIT_APPROACHING', + SEVERITY_WARN, + f'Joint {name} at {position:.3f} rad — ' + f'approaching limit (margin: {margin:.3f} rad)', + EVENT_FAILED, + ) + else: + # Joint is within safe range — remove from tracking sets + self.joints_violating.discard(name) + self.joints_approaching.discard(name) + + # Only clear faults when ALL joints are within safe range + if ( + not self.joints_violating + and 'JOINT_LIMIT_VIOLATED' in self.active_faults + ): + self.report_fault( + 'JOINT_LIMIT_VIOLATED', + SEVERITY_INFO, + 'All joint positions within limits', + EVENT_PASSED, + ) + if ( + not self.joints_approaching + and 'JOINT_LIMIT_APPROACHING' in self.active_faults + ): + self.report_fault( + 'JOINT_LIMIT_APPROACHING', + SEVERITY_INFO, + 'All joint positions within safe range', + EVENT_PASSED, + ) + + # ----------------------------------------------------------------- + # Periodic check + # ----------------------------------------------------------------- + + def periodic_check(self): + """Periodic health checks (placeholder for future checks).""" + pass # Could check for stale joint states, etc. + + # ----------------------------------------------------------------- + # Fault reporting helpers + # ----------------------------------------------------------------- + + def report_fault_throttled( + self, fault_code, severity, description, event_type + ): + """Report fault with throttling to avoid flooding.""" + now = self.get_clock().now() + last = self.last_report_times.get(fault_code) + if last is not None: + elapsed = (now - last).nanoseconds / 1e9 + if elapsed < self.report_throttle_sec: + return + self.last_report_times[fault_code] = now + self.report_fault(fault_code, severity, description, event_type) + + def report_fault(self, fault_code, severity, description, event_type): + """Report a fault to FaultManager via service call.""" + request = ReportFault.Request() + request.fault_code = fault_code + request.event_type = event_type + request.severity = severity + request.description = description + request.source_id = self.get_fully_qualified_name() + + if event_type == EVENT_FAILED: + self.active_faults.add(fault_code) + else: + self.active_faults.discard(fault_code) + + future = self.fault_client.call_async(request) + future.add_done_callback( + lambda f: self._handle_response(f, fault_code, event_type) + ) + + def _handle_response(self, future, fault_code, event_type): + """Handle response from fault_manager service.""" + try: + response = future.result() + if not response.accepted: + self.get_logger().warn( + f'Fault report rejected: {fault_code}' + ) + except Exception as e: + self.get_logger().error( + f'Failed to report fault {fault_code}: {e}' + ) + + @staticmethod + def _goal_id_str(status) -> str: + """Convert goal UUID to hex string.""" + return ''.join(f'{b:02x}' for b in status.goal_info.goal_id.uuid) + + +def main(args=None): + rclpy.init(args=args) + node = ManipulationMonitor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info( + 'ManipulationMonitor interrupted, shutting down.' + ) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/demos/moveit_pick_place/scripts/pick_place_loop.py b/demos/moveit_pick_place/scripts/pick_place_loop.py new file mode 100755 index 0000000..78abb48 --- /dev/null +++ b/demos/moveit_pick_place/scripts/pick_place_loop.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python3 +# Copyright 2026 selfpatch +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# 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 governing permissions and +# limitations under the License. + +"""Continuous pick-and-place loop for MoveIt 2 Panda demo. + +Performs repeating pick-and-place cycles so faults can be injected mid-operation. +Uses MoveGroupInterface via the MoveGroup action for motion planning and execution. + +All targets are joint-space goals for guaranteed reachability in both +fake-hardware and Gazebo simulation modes. +""" + +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from action_msgs.msg import GoalStatus +from moveit_msgs.action import MoveGroup +from moveit_msgs.msg import Constraints, JointConstraint + +# Joint-space targets (all within Panda limits, collision-free) +PANDA_TARGETS = { + # "ready" / home — standard upright pose from SRDF + "ready": { + "panda_joint1": 0.0, + "panda_joint2": -0.785, # -π/4 + "panda_joint3": 0.0, + "panda_joint4": -2.356, # -3π/4 + "panda_joint5": 0.0, + "panda_joint6": 1.571, # π/2 + "panda_joint7": 0.785, # π/4 + }, + # "pick" — arm reaching forward and down + "pick": { + "panda_joint1": 0.0, + "panda_joint2": 0.3, + "panda_joint3": 0.0, + "panda_joint4": -1.5, + "panda_joint5": 0.0, + "panda_joint6": 1.9, + "panda_joint7": 0.785, + }, + # "place" — arm reaching to the left + "place": { + "panda_joint1": 1.2, + "panda_joint2": 0.0, + "panda_joint3": 0.3, + "panda_joint4": -1.8, + "panda_joint5": 0.0, + "panda_joint6": 1.9, + "panda_joint7": 0.785, + }, +} + + +class PickPlaceLoop(Node): + """Repeatedly executes pick-and-place cycles using MoveGroup action.""" + + def __init__(self): + super().__init__("pick_place_loop") + + # Parameters + self.declare_parameter("cycle_delay_sec", 5.0) + self.cycle_delay = self.get_parameter("cycle_delay_sec").value + + # MoveGroup action client + # In MoveIt 2 Jazzy, MoveGroupMoveAction serves at /move_action + self.move_group_client = ActionClient(self, MoveGroup, "/move_action") + + # State machine: pick → place → home → pick ... + self.cycle_count = 0 + self.phases = ["pick", "place", "home"] + self.phase_idx = 0 + + # State flags + self.move_group_ready = False + + # Wait for MoveGroup with periodic retries (non-blocking startup) + self.get_logger().info("PickPlaceLoop initialized, waiting for MoveGroup...") + self.startup_timer = self.create_timer(5.0, self.check_move_group_ready) + + def check_move_group_ready(self): + """Periodically check if MoveGroup action server is available.""" + if self.move_group_client.server_is_ready(): + self.get_logger().info("MoveGroup available, starting pick-and-place loop") + self.move_group_ready = True + self.startup_timer.cancel() + + # Start the cycle timer + self.timer = self.create_timer(self.cycle_delay, self.execute_cycle) + else: + self.get_logger().info( + "MoveGroup not yet available, retrying in 5 seconds..." + ) + + def execute_cycle(self): + """One pick-and-place cycle step.""" + if not self.move_group_ready: + return + + phase = self.phases[self.phase_idx] + self.cycle_count += 1 + self.get_logger().info(f"=== Cycle {self.cycle_count} ({phase}) ===") + + # Map phase to joint target + target_name = "ready" if phase == "home" else phase + self.send_joint_goal(target_name, phase) + + def send_joint_goal(self, target_name, label): + """Send a MoveGroup goal using joint-space target values.""" + joint_values = PANDA_TARGETS.get(target_name) + if not joint_values: + self.get_logger().error(f"Unknown target: {target_name}") + return + + goal = MoveGroup.Goal() + goal.request.group_name = "panda_arm" + goal.request.num_planning_attempts = 5 + goal.request.allowed_planning_time = 5.0 + goal.planning_options.plan_only = False + goal.planning_options.replan = True + goal.planning_options.replan_attempts = 3 + + # Build joint constraints + constraints = Constraints() + for joint_name, position in joint_values.items(): + jc = JointConstraint() + jc.joint_name = joint_name + jc.position = position + jc.tolerance_above = 0.01 + jc.tolerance_below = 0.01 + jc.weight = 1.0 + constraints.joint_constraints.append(jc) + goal.request.goal_constraints.append(constraints) + + self.get_logger().info(f"Sending {label} goal (target: {target_name})...") + future = self.move_group_client.send_goal_async(goal) + future.add_done_callback(lambda f: self.goal_response_callback(f, label)) + + def goal_response_callback(self, future, label): + """Handle MoveGroup goal acceptance/rejection.""" + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().error(f"{label} goal REJECTED") + return + + self.get_logger().info(f"{label} goal accepted, waiting for result...") + result_future = goal_handle.get_result_async() + result_future.add_done_callback(lambda f: self.result_callback(f, label)) + + def result_callback(self, future, label): + """Handle MoveGroup result and advance state machine.""" + result = future.result().result + status = future.result().status + + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info(f"{label} SUCCEEDED") + # Advance state machine + self.phase_idx = (self.phase_idx + 1) % len(self.phases) + else: + error_code = result.error_code.val if result else "unknown" + self.get_logger().warn( + f"{label} FAILED with status {status}, error_code={error_code}" + ) + # On failure, retry same phase next cycle + + +def main(args=None): + rclpy.init(args=args) + node = PickPlaceLoop() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/demos/moveit_pick_place/stop-demo.sh b/demos/moveit_pick_place/stop-demo.sh new file mode 100755 index 0000000..950c499 --- /dev/null +++ b/demos/moveit_pick_place/stop-demo.sh @@ -0,0 +1,74 @@ +#!/bin/bash +# Stop MoveIt 2 Panda + ros2_medkit Demo + +set -eu + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "$SCRIPT_DIR" + +echo "🛑 Stopping MoveIt 2 Panda + ros2_medkit Demo" +echo "===============================================" + +# Check for Docker +if ! command -v docker &> /dev/null; then + echo "Error: Docker is not installed" + exit 1 +fi + +# Parse arguments +REMOVE_VOLUMES="" +REMOVE_IMAGES="" + +usage() { + echo "Usage: $0 [OPTIONS]" + echo "" + echo "Options:" + echo " -v, --volumes Remove named volumes" + echo " --images Remove images" + echo " -h, --help Show this help message" + echo "" + echo "Examples:" + echo " $0 # Stop containers" + echo " $0 --volumes # Stop and remove volumes" + echo " $0 --images # Stop and remove images" +} + +while [[ $# -gt 0 ]]; do + case "$1" in + -v|--volumes) + echo "Will remove named volumes" + REMOVE_VOLUMES="-v" + ;; + --images) + echo "Will remove images" + REMOVE_IMAGES="--rmi all" + ;; + -h|--help) + usage + exit 0 + ;; + *) + echo "Unknown option: $1" + usage + exit 1 + ;; + esac + shift +done + +# Stop containers +echo "Stopping containers..." +if docker compose version &> /dev/null; then + # shellcheck disable=SC2086 + docker compose --profile cpu --profile nvidia down ${REMOVE_VOLUMES} ${REMOVE_IMAGES} +else + # shellcheck disable=SC2086 + docker-compose --profile cpu --profile nvidia down ${REMOVE_VOLUMES} ${REMOVE_IMAGES} +fi + +# Cleanup X11 +echo "Cleaning up X11 permissions..." +xhost -local:docker 2>/dev/null || true + +echo "" +echo "✅ Demo stopped successfully!" From c466098fc27ad6fbe0db7f3c9980ac8d9afe5c81 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 8 Feb 2026 19:12:50 +0000 Subject: [PATCH 2/6] fix(moveit-demo): fix 9 bugs found during headless verification MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Scripts: - Fix monitor topic /move_group/_action/status → /move_action/_action/status - Fix injection scripts: use /apply_planning_scene service instead of unreliable ros2 topic pub --once - Rewrite controller-timeout and joint-limit scripts to use gateway REST API instead of docker exec rclpy (DDS discovery issues) - Fix restore script: use service call + REST API, add delay for PREFAILED timing - Fix check-entities.sh: joint states jq path .name → .data.name - Fix check-faults.sh: correct jq field names to match API response Config: - Add fault_manager config to medkit_params.yaml (snapshots, storage) - Pass medkit_params_file to fault_manager in demo.launch.py Docs: - Fix monitored topics table and architecture diagram - Fix verification jq example field names - Add snapshot documentation and REST API configuration section - Add injection limitation notes (fake hardware, single-goal controller) - Add DDS/docker exec troubleshooting entries - Update Docker image size estimate (~5-6 GB → ~7 GB) --- demos/moveit_pick_place/README.md | 54 +++++++++-- demos/moveit_pick_place/check-entities.sh | 6 +- demos/moveit_pick_place/check-faults.sh | 15 +-- .../config/medkit_params.yaml | 26 +++++ demos/moveit_pick_place/inject-collision.sh | 61 ++++++++---- .../inject-controller-timeout.sh | 35 ++++--- .../moveit_pick_place/inject-grasp-failure.sh | 65 +++++++++---- demos/moveit_pick_place/inject-joint-limit.sh | 41 +++++--- .../inject-planning-failure.sh | 61 ++++++++---- demos/moveit_pick_place/launch/demo.launch.py | 6 +- demos/moveit_pick_place/restore-normal.sh | 96 +++++++++++-------- .../scripts/manipulation_monitor.py | 3 +- 12 files changed, 325 insertions(+), 144 deletions(-) diff --git a/demos/moveit_pick_place/README.md b/demos/moveit_pick_place/README.md index 4345cd1..61177d9 100644 --- a/demos/moveit_pick_place/README.md +++ b/demos/moveit_pick_place/README.md @@ -23,7 +23,7 @@ This demo demonstrates: - Docker and docker-compose - X11 display server (for RViz GUI) or `--headless` mode - (Optional) NVIDIA GPU + [nvidia-container-toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html) -- ~5-6 GB disk space for Docker image +- ~7 GB disk space for Docker image ## Quick Start @@ -35,7 +35,7 @@ cd demos/moveit_pick_place ``` That's it! The script will: -1. Build the Docker image (first run: ~15-20 min, ~5-6 GB) +1. Build the Docker image (first run: ~15-20 min, ~7 GB) 2. Set up X11 forwarding for RViz GUI 3. Launch Panda robot + MoveIt 2 + ros2_medkit gateway 4. Launch sovd_web_ui at http://localhost:3000 @@ -113,7 +113,7 @@ docker exec -it moveit_medkit_demo bash # Shell into container │ │ manipulation_monitor.py ──► fault_manager ◄── diag_bridge│ │ │ │ │ │ │ │ │ │ │ monitors: │ stores faults │ │ -│ │ │ • /move_group status │ │ │ +│ │ │ • /move_action status │ │ │ │ │ │ • /controller status ▼ │ │ │ │ │ • /joint_states gateway_node ──► REST API │ │ │ └─────────┴────────────────────────┬───────────────────────┘ │ @@ -185,7 +185,7 @@ curl http://localhost:8080/api/v1/functions | jq ### Read Joint States ```bash -curl http://localhost:8080/api/v1/apps/joint-state-broadcaster/data/joint_states | jq +curl http://localhost:8080/api/v1/apps/joint-state-broadcaster/data/%2Fjoint_states | jq ``` ### List MoveIt Configurations @@ -214,6 +214,38 @@ curl http://localhost:8080/api/v1/faults | jq curl -X DELETE http://localhost:8080/api/v1/faults ``` +### View Fault Snapshots + +When a fault is detected, the fault manager captures environment snapshots (freeze frames) from configured ROS 2 topics. Snapshots are embedded in the fault detail response: + +```bash +# Get fault detail with snapshots +curl http://localhost:8080/api/v1/apps/manipulation-monitor/faults/MOTION_PLANNING_FAILED | jq '.environment_data.snapshots' +``` + +Captured topics (on-demand, 2s timeout): +- `/joint_states` — Current joint positions at fault time +- `/diagnostics` — Active diagnostics messages + +> **Note:** Action status topics (`/move_action/_action/status`, `/panda_arm_controller/follow_joint_trajectory/_action/status`) may timeout during snapshot capture since they only publish on state transitions. + +### Modify Configurations via REST API + +You can read and write ROS 2 node parameters through the gateway: + +```bash +# List all parameters for an app +curl http://localhost:8080/api/v1/apps/panda-arm-controller/configurations | jq + +# Read a specific parameter +curl http://localhost:8080/api/v1/apps/panda-arm-controller/configurations/gains.panda_joint1.p | jq + +# Set a parameter value +curl -X PUT http://localhost:8080/api/v1/apps/panda-arm-controller/configurations/constraints.goal_time \ + -H 'Content-Type: application/json' \ + -d '{"data": {"value": 0.5}}' +``` + ## Fault Injection Scenarios ### 1. Planning Failure @@ -232,6 +264,8 @@ Blocks the robot's path with a large collision wall. Moves the target object far outside the arm's reachable workspace. +> **Note:** This injection only works if the pick-place loop uses the `target_cylinder` collision object as its grasp target. With the default hardcoded joint/cartesian targets, this injection may have no visible effect. + ```bash ./inject-grasp-failure.sh ``` @@ -244,6 +278,8 @@ Moves the target object far outside the arm's reachable workspace. Sets extremely tight goal time tolerance on the arm controller. +> **Note:** This injection has no effect with fake/mock hardware because the simulated controller reports instant success. Use with Gazebo physics (`--gazebo`) or a physical robot for visible faults. + ```bash ./inject-controller-timeout.sh ``` @@ -257,6 +293,8 @@ Sets extremely tight goal time tolerance on the arm controller. Commands the arm to reach extreme joint positions near/beyond URDF limits. +> **Note:** While the pick-place loop is running, the controller accepts only one goal at a time and may reject external trajectory commands. This injection works best when the pick-place loop is paused. + ```bash ./inject-joint-limit.sh ``` @@ -293,7 +331,7 @@ After any injection, verify faults: ```bash ./check-faults.sh # OR -curl http://localhost:8080/api/v1/faults | jq '.items[] | {code, severity, message}' +curl http://localhost:8080/api/v1/faults | jq '.items[] | {fault_code, severity_label, description}' ``` ## Web UI @@ -331,6 +369,8 @@ Connect it to the gateway at `http://localhost:8080` to browse: | "MoveGroup not available" | Slow startup | Wait 60-90 seconds after container starts | | Controller not loading | Missing config | Verify `moveit_controllers.yaml` is correct | | Joint states empty | Controllers not loaded | Check `ros2 control list_controllers` inside container | +| `ros2` CLI hangs in `docker exec` | DDS discovery across container boundaries | Use gateway REST API instead of `ros2` CLI for parameter/service operations | +| Injection script has no output | DDS multicast not reachable from host | Run injection scripts inside the demo container or use REST API equivalents | ## Comparison with Other Demos @@ -342,7 +382,7 @@ Connect it to the gateway at `http://localhost:8080` to browse: | Fault types | Sensor drift, noise | Nav failures, localization | Planning, controller, joint limits | | Entity complexity | Simple (flat) | Medium (3 areas) | High (4 areas, 7 components) | | SOVD manifest | No | Yes (hybrid) | Yes (hybrid) | -| Docker image | ~2 GB | ~4 GB | ~5-6 GB | +| Docker image | ~2 GB | ~4 GB | ~7 GB | | GPU recommended | No | Optional | Optional | ## Technical Details @@ -351,7 +391,7 @@ Connect it to the gateway at `http://localhost:8080` to browse: | Topic | What it tells us | Fault codes | |-------|------------------|-------------| -| `/move_group/_action/status` | Planning success/failure | `MOTION_PLANNING_FAILED` | +| `/move_action/_action/status` | Planning success/failure | `MOTION_PLANNING_FAILED` | | `/panda_arm_controller/follow_joint_trajectory/_action/status` | Trajectory execution | `TRAJECTORY_EXECUTION_FAILED`, `CONTROLLER_TIMEOUT` | | `/joint_states` | Current joint positions | `JOINT_LIMIT_APPROACHING`, `JOINT_LIMIT_VIOLATED` | diff --git a/demos/moveit_pick_place/check-entities.sh b/demos/moveit_pick_place/check-entities.sh index 66dfa3a..3a32ef0 100755 --- a/demos/moveit_pick_place/check-entities.sh +++ b/demos/moveit_pick_place/check-entities.sh @@ -51,9 +51,9 @@ curl -s "${API_BASE}/functions" | jq '.items[] | {id: .id, name: .name, category echo_step "5. Sample Data (Joint States)" echo "Getting latest joint states from Panda arm..." curl -s "${API_BASE}/apps/joint-state-broadcaster/data/joint_states" 2>/dev/null | jq '{ - joint_names: .name, - positions: .position, - velocities: .velocity + joint_names: .data.name, + positions: .data.position, + velocities: .data.velocity }' || echo " (Joint state data not available — robot may still be starting)" echo_step "6. Faults" diff --git a/demos/moveit_pick_place/check-faults.sh b/demos/moveit_pick_place/check-faults.sh index 0374c85..51ac30b 100755 --- a/demos/moveit_pick_place/check-faults.sh +++ b/demos/moveit_pick_place/check-faults.sh @@ -36,11 +36,14 @@ if [ "$FAULT_COUNT" = "0" ]; then echo " No active faults — system is healthy! ✅" else echo "$FAULTS" | jq '.items[] | { - code: .code, - severity: .severity, - reporter: .reporter_id, - message: .message, - timestamp: .timestamp + code: .fault_code, + severity: .severity_label, + status: .status, + description: .description, + sources: .reporting_sources, + occurrences: .occurrence_count, + first_occurred: .first_occurred, + last_occurred: .last_occurred }' fi @@ -52,7 +55,7 @@ echo " Total active faults: $FAULT_COUNT" if [ "$FAULT_COUNT" != "0" ]; then echo "" echo " By severity:" - echo "$FAULTS" | jq -r '.items | group_by(.severity) | .[] | " \(.[0].severity): \(length)"' + echo "$FAULTS" | jq -r '.items | group_by(.severity_label) | .[] | " \(.[0].severity_label): \(length)"' fi echo "" diff --git a/demos/moveit_pick_place/config/medkit_params.yaml b/demos/moveit_pick_place/config/medkit_params.yaml index 2ae4d5b..107fe0b 100644 --- a/demos/moveit_pick_place/config/medkit_params.yaml +++ b/demos/moveit_pick_place/config/medkit_params.yaml @@ -27,3 +27,29 @@ diagnostics: discovery: runtime: create_synthetic_components: false # Manifest defines components + +# Fault Manager configuration (runs in root namespace) +fault_manager: + ros__parameters: + # Storage configuration + storage_type: "sqlite" + database_path: "/tmp/ros2_medkit/faults.db" + + # Debounce configuration + confirmation_threshold: 0 # Immediate confirmation for demo + healing_enabled: false + auto_confirm_after_sec: 0.0 + + # Snapshot configuration (freeze frames) + snapshots: + enabled: true + background_capture: false # On-demand capture when fault occurs + timeout_sec: 2.0 + max_message_size: 131072 # 128KB max per message + + # Topics to capture for all faults + default_topics: + - /joint_states + - /move_action/_action/status + - /panda_arm_controller/follow_joint_trajectory/_action/status + - /diagnostics diff --git a/demos/moveit_pick_place/inject-collision.sh b/demos/moveit_pick_place/inject-collision.sh index 2d5d464..f7b6d5a 100755 --- a/demos/moveit_pick_place/inject-collision.sh +++ b/demos/moveit_pick_place/inject-collision.sh @@ -22,28 +22,53 @@ if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then exit 1 fi -# Add a sphere right in the arm's working area +# Add a sphere right in the arm's working area via /apply_planning_scene service echo "Adding surprise obstacle (sphere) to planning scene..." docker exec moveit_medkit_demo bash -c " source /opt/ros/jazzy/setup.bash && \ source /root/demo_ws/install/setup.bash && \ -ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ - world: { - collision_objects: [ - { - id: \"surprise_obstacle\", - header: {frame_id: \"panda_link0\"}, - primitives: [{type: 2, dimensions: [0.15]}], - primitive_poses: [{ - position: {x: 0.4, y: 0.0, z: 0.4}, - orientation: {w: 1.0} - }], - operation: 0 - } - ] - }, - is_diff: true -}'" +python3 -c \" +import rclpy +from rclpy.node import Node +from moveit_msgs.srv import ApplyPlanningScene +from moveit_msgs.msg import PlanningScene, CollisionObject +from shape_msgs.msg import SolidPrimitive +from geometry_msgs.msg import Pose +from std_msgs.msg import Header + +rclpy.init() +node = Node('inject_obstacle') +client = node.create_client(ApplyPlanningScene, '/apply_planning_scene') +client.wait_for_service(timeout_sec=5.0) + +scene = PlanningScene() +scene.is_diff = True + +obs = CollisionObject() +obs.id = 'surprise_obstacle' +obs.header.frame_id = 'panda_link0' +obs.operation = CollisionObject.ADD +prim = SolidPrimitive() +prim.type = SolidPrimitive.SPHERE +prim.dimensions = [0.15] +obs.primitives.append(prim) +pose = Pose() +pose.position.x = 0.4 +pose.position.y = 0.0 +pose.position.z = 0.4 +pose.orientation.w = 1.0 +obs.primitive_poses.append(pose) +scene.world.collision_objects.append(obs) + +req = ApplyPlanningScene.Request() +req.scene = scene +future = client.call_async(req) +rclpy.spin_until_future_complete(node, future, timeout_sec=5.0) +result = future.result() +print(f'Obstacle added: {result.success}' if result else 'Service call failed') +node.destroy_node() +rclpy.shutdown() +\"" echo "" echo "✓ Collision fault injected!" diff --git a/demos/moveit_pick_place/inject-controller-timeout.sh b/demos/moveit_pick_place/inject-controller-timeout.sh index a8d2782..af5f90c 100755 --- a/demos/moveit_pick_place/inject-controller-timeout.sh +++ b/demos/moveit_pick_place/inject-controller-timeout.sh @@ -1,6 +1,10 @@ #!/bin/bash -# Inject Controller Timeout - Set extremely low velocity scaling +# Inject Controller Timeout - Set extremely tight goal time tolerance # This will cause the arm controller to timeout during trajectory execution +# +# NOTE: This injection has no effect with fake/mock hardware (ros2_control +# FakeSystem) because the simulated controller reports instant success. +# It produces real faults only with physical robots or Gazebo physics. GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" API_BASE="${GATEWAY_URL}/api/v1" @@ -23,28 +27,23 @@ if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then exit 1 fi -# Set an extremely tight goal_time constraint on the controller -# This causes FollowJointTrajectory to abort because the robot can't -# reach the target within the allowed time window -echo "Setting controller goal_time constraint via ROS 2 parameter..." -docker exec moveit_medkit_demo bash -c " -source /opt/ros/jazzy/setup.bash && \ -source /root/demo_ws/install/setup.bash && \ -ros2 param set /panda_arm_controller constraints.goal_time 0.001" 2>/dev/null || \ - echo " Warning: Could not set goal_time parameter" +# Set controller parameters via gateway REST API (reliable, no DDS issues) +echo "Setting controller goal_time constraint via gateway API..." +curl -s -X PUT "${API_BASE}/apps/panda-arm-controller/configurations/constraints.goal_time" \ + -H "Content-Type: application/json" \ + -d '{"data": 0.001}' > /dev/null -# Also try setting stopped_velocity_tolerance to near-zero to make it -# harder for the controller to consider the trajectory complete -docker exec moveit_medkit_demo bash -c " -source /opt/ros/jazzy/setup.bash && \ -source /root/demo_ws/install/setup.bash && \ -ros2 param set /panda_arm_controller constraints.stopped_velocity_tolerance 0.0001" 2>/dev/null || \ - echo " Warning: Could not set stopped_velocity_tolerance parameter" +curl -s -X PUT "${API_BASE}/apps/panda-arm-controller/configurations/constraints.stopped_velocity_tolerance" \ + -H "Content-Type: application/json" \ + -d '{"data": 0.0001}' > /dev/null echo "" echo "✓ Controller timeout injected!" echo "" -echo "Expected faults (via manipulation_monitor → FaultManager):" +echo "⚠️ NOTE: With fake hardware, the controller succeeds instantly and this" +echo " injection will NOT produce faults. Use with real robot or Gazebo." +echo "" +echo "Expected faults (with real hardware):" echo " - CONTROLLER_TIMEOUT: Joint trajectory controller timed out" echo " - TRAJECTORY_EXECUTION_FAILED: Arm controller ABORTED trajectory" echo "" diff --git a/demos/moveit_pick_place/inject-grasp-failure.sh b/demos/moveit_pick_place/inject-grasp-failure.sh index 469b0d7..c3b9716 100755 --- a/demos/moveit_pick_place/inject-grasp-failure.sh +++ b/demos/moveit_pick_place/inject-grasp-failure.sh @@ -1,6 +1,10 @@ #!/bin/bash # Inject Grasp Failure - Move target object out of robot's workspace # This will cause MoveGroup to fail planning to an unreachable target +# +# NOTE: This injection only works if the pick-place loop uses the +# target_cylinder object as its grasp target. If the loop uses hardcoded +# joint/cartesian targets, moving the collision object has no effect. GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" API_BASE="${GATEWAY_URL}/api/v1" @@ -22,28 +26,53 @@ if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then exit 1 fi -# Move the target cylinder far away from the robot +# Move the target cylinder far away from the robot via /apply_planning_scene service echo "Moving target object to unreachable position (5.0, 5.0, 0.1)..." docker exec moveit_medkit_demo bash -c " source /opt/ros/jazzy/setup.bash && \ source /root/demo_ws/install/setup.bash && \ -ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ - world: { - collision_objects: [ - { - id: \"target_cylinder\", - header: {frame_id: \"panda_link0\"}, - primitives: [{type: 3, dimensions: [0.1, 0.02]}], - primitive_poses: [{ - position: {x: 5.0, y: 5.0, z: 0.1}, - orientation: {w: 1.0} - }], - operation: 0 - } - ] - }, - is_diff: true -}'" +python3 -c \" +import rclpy +from rclpy.node import Node +from moveit_msgs.srv import ApplyPlanningScene +from moveit_msgs.msg import PlanningScene, CollisionObject +from shape_msgs.msg import SolidPrimitive +from geometry_msgs.msg import Pose +from std_msgs.msg import Header + +rclpy.init() +node = Node('inject_grasp') +client = node.create_client(ApplyPlanningScene, '/apply_planning_scene') +client.wait_for_service(timeout_sec=5.0) + +scene = PlanningScene() +scene.is_diff = True + +cyl = CollisionObject() +cyl.id = 'target_cylinder' +cyl.header.frame_id = 'panda_link0' +cyl.operation = CollisionObject.ADD +prim = SolidPrimitive() +prim.type = SolidPrimitive.CYLINDER +prim.dimensions = [0.1, 0.02] +cyl.primitives.append(prim) +pose = Pose() +pose.position.x = 5.0 +pose.position.y = 5.0 +pose.position.z = 0.1 +pose.orientation.w = 1.0 +cyl.primitive_poses.append(pose) +scene.world.collision_objects.append(cyl) + +req = ApplyPlanningScene.Request() +req.scene = scene +future = client.call_async(req) +rclpy.spin_until_future_complete(node, future, timeout_sec=5.0) +result = future.result() +print(f'Target moved: {result.success}' if result else 'Service call failed') +node.destroy_node() +rclpy.shutdown() +\"" echo "" echo "✓ Grasp failure injected!" diff --git a/demos/moveit_pick_place/inject-joint-limit.sh b/demos/moveit_pick_place/inject-joint-limit.sh index cb44005..9741b35 100755 --- a/demos/moveit_pick_place/inject-joint-limit.sh +++ b/demos/moveit_pick_place/inject-joint-limit.sh @@ -1,6 +1,11 @@ #!/bin/bash # Inject Joint Limit Violation - Command extreme joint positions # This will trigger joint limit approaching/violation faults +# +# NOTE: While the pick-place loop is running, the controller will reject +# external trajectory goals (only one goal at a time). This injection +# works best when the pick-place loop is not active, or after stopping +# the mtc-pick-place node. GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" API_BASE="${GATEWAY_URL}/api/v1" @@ -22,26 +27,32 @@ if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then exit 1 fi -# Send a goal with extreme joint positions via ROS 2 CLI -echo "Sending joint trajectory goal near limits..." -docker exec moveit_medkit_demo bash -c " -source /opt/ros/jazzy/setup.bash && \ -source /root/demo_ws/install/setup.bash && \ -ros2 action send_goal /panda_arm_controller/follow_joint_trajectory \ - control_msgs/action/FollowJointTrajectory '{ - trajectory: { - joint_names: [panda_joint1, panda_joint2, panda_joint3, - panda_joint4, panda_joint5, panda_joint6, panda_joint7], - points: [{ - positions: [2.85, 1.70, 2.85, -0.10, 2.85, 3.70, 2.85], - time_from_start: {sec: 5, nanosec: 0} - }] +# Send a goal with extreme joint positions via gateway operations API +# These positions are near/beyond joint limits (see panda URDF) +echo "Sending joint trajectory goal near limits via gateway API..." +RESULT=$(curl -s -X POST "${API_BASE}/apps/panda-arm-controller/operations/follow_joint_trajectory/executions" \ + -H "Content-Type: application/json" \ + -d '{ + "input_data": { + "trajectory": { + "joint_names": ["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7"], + "points": [{ + "positions": [2.85, 1.70, 2.85, -0.10, 2.85, 3.70, 2.85], + "time_from_start": {"sec": 5, "nanosec": 0} + }] + } } - }'" 2>/dev/null & + }') + +echo " Result: ${RESULT}" | head -c 200 +echo "" echo "" echo "✓ Joint limit fault injected!" echo "" +echo "⚠️ NOTE: If the pick-place loop is running, the goal may be rejected" +echo " (controller accepts only one goal at a time)." +echo "" echo "Expected faults (via manipulation_monitor → FaultManager):" echo " - JOINT_LIMIT_APPROACHING: Joint near limit (WARN)" echo " - JOINT_LIMIT_VIOLATED: Joint beyond limit (ERROR)" diff --git a/demos/moveit_pick_place/inject-planning-failure.sh b/demos/moveit_pick_place/inject-planning-failure.sh index b3794cf..a12e70c 100755 --- a/demos/moveit_pick_place/inject-planning-failure.sh +++ b/demos/moveit_pick_place/inject-planning-failure.sh @@ -22,28 +22,53 @@ if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then exit 1 fi -# Add a large wall via PlanningScene topic inside the container +# Add a large wall via /apply_planning_scene service (reliable) echo "Adding collision wall to planning scene..." docker exec moveit_medkit_demo bash -c " source /opt/ros/jazzy/setup.bash && \ source /root/demo_ws/install/setup.bash && \ -ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ - world: { - collision_objects: [ - { - id: \"injected_wall\", - header: {frame_id: \"panda_link0\"}, - primitives: [{type: 1, dimensions: [2.0, 0.05, 1.0]}], - primitive_poses: [{ - position: {x: 0.3, y: 0.25, z: 0.5}, - orientation: {w: 1.0} - }], - operation: 0 - } - ] - }, - is_diff: true -}'" +python3 -c \" +import rclpy +from rclpy.node import Node +from moveit_msgs.srv import ApplyPlanningScene +from moveit_msgs.msg import PlanningScene, CollisionObject +from shape_msgs.msg import SolidPrimitive +from geometry_msgs.msg import Pose +from std_msgs.msg import Header + +rclpy.init() +node = Node('inject_wall') +client = node.create_client(ApplyPlanningScene, '/apply_planning_scene') +client.wait_for_service(timeout_sec=5.0) + +scene = PlanningScene() +scene.is_diff = True + +wall = CollisionObject() +wall.id = 'injected_wall' +wall.header.frame_id = 'panda_link0' +wall.operation = CollisionObject.ADD +prim = SolidPrimitive() +prim.type = SolidPrimitive.BOX +prim.dimensions = [2.0, 0.05, 1.0] +wall.primitives.append(prim) +pose = Pose() +pose.position.x = 0.3 +pose.position.y = 0.25 +pose.position.z = 0.5 +pose.orientation.w = 1.0 +wall.primitive_poses.append(pose) +scene.world.collision_objects.append(wall) + +req = ApplyPlanningScene.Request() +req.scene = scene +future = client.call_async(req) +rclpy.spin_until_future_complete(node, future, timeout_sec=5.0) +result = future.result() +print(f'Wall added: {result.success}' if result else 'Service call failed') +node.destroy_node() +rclpy.shutdown() +\"" echo "" echo "✓ Planning failure injected!" diff --git a/demos/moveit_pick_place/launch/demo.launch.py b/demos/moveit_pick_place/launch/demo.launch.py index 06bf7f0..a1093c0 100644 --- a/demos/moveit_pick_place/launch/demo.launch.py +++ b/demos/moveit_pick_place/launch/demo.launch.py @@ -86,13 +86,17 @@ def generate_launch_description(): ), # === ros2_medkit stack === # Fault manager — root namespace + # Also handles snapshot capture when faults are confirmed Node( package="ros2_medkit_fault_manager", executable="fault_manager_node", name="fault_manager", namespace="", output="screen", - parameters=[{"use_sim_time": use_sim_time}], + parameters=[ + medkit_params_file, + {"use_sim_time": use_sim_time}, + ], ), # Diagnostic bridge — /bridge namespace # auto_generate_codes=False prevents noisy controller_manager diff --git a/demos/moveit_pick_place/restore-normal.sh b/demos/moveit_pick_place/restore-normal.sh index 6924673..f98f6a5 100755 --- a/demos/moveit_pick_place/restore-normal.sh +++ b/demos/moveit_pick_place/restore-normal.sh @@ -21,55 +21,73 @@ if ! curl -sf "${API_BASE}/health" > /dev/null 2>&1; then exit 1 fi -# 1. Remove injected collision objects +# 1. Remove injected collision objects via /apply_planning_scene service +# Using service call (not topic pub) for reliable planning scene updates echo "Removing injected collision objects..." docker exec moveit_medkit_demo bash -c " source /opt/ros/jazzy/setup.bash && \ source /root/demo_ws/install/setup.bash && \ -ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ - world: { - collision_objects: [ - {id: \"injected_wall\", operation: 1}, - {id: \"surprise_obstacle\", operation: 1} - ] - }, - is_diff: true -}'" +python3 -c \" +import rclpy +from rclpy.node import Node +from moveit_msgs.srv import ApplyPlanningScene +from moveit_msgs.msg import PlanningScene, CollisionObject +from std_msgs.msg import Header -# 2. Restore target object to original position -echo "Restoring target object to pick position..." -docker exec moveit_medkit_demo bash -c " -source /opt/ros/jazzy/setup.bash && \ -source /root/demo_ws/install/setup.bash && \ -ros2 topic pub --once /planning_scene moveit_msgs/msg/PlanningScene '{ - world: { - collision_objects: [ - { - id: \"target_cylinder\", - header: {frame_id: \"panda_link0\"}, - primitives: [{type: 3, dimensions: [0.1, 0.02]}], - primitive_poses: [{ - position: {x: 0.5, y: 0.0, z: 0.1}, - orientation: {w: 1.0} - }], - operation: 0 - } - ] - }, - is_diff: true -}'" +rclpy.init() +node = Node('restore_helper') + +# Remove injected objects +client = node.create_client(ApplyPlanningScene, '/apply_planning_scene') +client.wait_for_service(timeout_sec=5.0) + +scene = PlanningScene() +scene.is_diff = True + +# Remove injected_wall +wall = CollisionObject() +wall.id = 'injected_wall' +wall.operation = CollisionObject.REMOVE +scene.world.collision_objects.append(wall) + +# Remove surprise_obstacle +obstacle = CollisionObject() +obstacle.id = 'surprise_obstacle' +obstacle.operation = CollisionObject.REMOVE +scene.world.collision_objects.append(obstacle) -# 3. Restore controller parameters +# Remove target_cylinder (moved by grasp failure) +cylinder = CollisionObject() +cylinder.id = 'target_cylinder' +cylinder.operation = CollisionObject.REMOVE +scene.world.collision_objects.append(cylinder) + +req = ApplyPlanningScene.Request() +req.scene = scene +future = client.call_async(req) +rclpy.spin_until_future_complete(node, future, timeout_sec=5.0) +result = future.result() +print(f'Scene update accepted: {result.success}' if result else 'Service call failed') +node.destroy_node() +rclpy.shutdown() +\" +" + +# 3. Restore controller parameters via gateway REST API echo "Restoring controller parameters..." -docker exec moveit_medkit_demo bash -c " -source /opt/ros/jazzy/setup.bash && \ -source /root/demo_ws/install/setup.bash && \ -ros2 param set /panda_arm_controller constraints.goal_time 0.0 && \ -ros2 param set /panda_arm_controller constraints.stopped_velocity_tolerance 0.01" 2>/dev/null || true +curl -s -X PUT "${API_BASE}/apps/panda-arm-controller/configurations/constraints.goal_time" \ + -H "Content-Type: application/json" -d '{"data": 0.0}' > /dev/null 2>&1 || true +curl -s -X PUT "${API_BASE}/apps/panda-arm-controller/configurations/constraints.stopped_velocity_tolerance" \ + -H "Content-Type: application/json" -d '{"data": 0.01}' > /dev/null 2>&1 || true -# 4. Clear all faults +# 4. Wait for operations to stabilize, then clear faults +echo "Waiting for operations to stabilize..." +sleep 10 echo "Clearing all faults..." curl -s -X DELETE "${API_BASE}/faults" > /dev/null +# Wait for any straggling reports to land, then clear again +sleep 5 +curl -s -X DELETE "${API_BASE}/faults" > /dev/null echo "" echo "✓ Normal operation restored!" diff --git a/demos/moveit_pick_place/scripts/manipulation_monitor.py b/demos/moveit_pick_place/scripts/manipulation_monitor.py index e1847ed..c64420c 100755 --- a/demos/moveit_pick_place/scripts/manipulation_monitor.py +++ b/demos/moveit_pick_place/scripts/manipulation_monitor.py @@ -90,9 +90,10 @@ def __init__(self): ) # MoveGroup action status — for planning failures + # In MoveIt 2 Jazzy, MoveGroupMoveAction serves at /move_action self.move_group_status_sub = self.create_subscription( GoalStatusArray, - '/move_group/_action/status', + '/move_action/_action/status', self.move_group_status_callback, status_qos, ) From c8c499b575d34db479f8e0891032fec555941cc1 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 8 Feb 2026 19:17:06 +0000 Subject: [PATCH 3/6] ci: add MoveIt pick-and-place Docker build to CI --- .github/workflows/ci.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f38ab8b..a320138 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -48,3 +48,8 @@ jobs: run: | cd demos/turtlebot3_integration docker build -t turtlebot3-medkit-demo:test -f Dockerfile . + + - name: Build MoveIt Pick-and-Place demo image + run: | + cd demos/moveit_pick_place + docker build -t moveit-pick-place-demo:test -f Dockerfile . From d79bfa466be075da4fd3984dfe0563d7836cd3c4 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 8 Feb 2026 19:54:56 +0000 Subject: [PATCH 4/6] fix(moveit-demo): address PR review comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Scripts: - Add container auto-detection (CONTAINER_NAME env or grep for moveit_medkit_demo / moveit_medkit_demo_nvidia) in all docker exec scripts (inject-*, restore-normal) - Guard pick_place_loop against overlapping goals (goal_in_flight flag) - Use zip() instead of index-based iteration in joint_state_callback - Remove unused Optional import from manipulation_monitor - Add comment to bare except in pick_place_loop Config: - Rename task-constructor → pick-place-loop, mtc-pick-place → pick-place-node in panda_manifest.yaml to match actual demo node - Restrict CORS origins from wildcard to localhost:8080/3000 Packaging: - Add missing exec_depend entries in package.xml (rclpy, action_msgs, sensor_msgs, moveit_msgs, ros2_medkit_msgs) - Pin ros2_medkit clone to configurable ARG ROS2_MEDKIT_REF in Dockerfile - Remove rosdep || true fallback to fail on missing dependencies run-demo.sh: - Skip X11/xhost setup in headless mode or when DISPLAY is unset - Add --gazebo flag to usage help text - Fix image size estimate in output message Docs: - Update README to reflect Gazebo as default simulation mode - Rename entity hierarchy entries to match updated manifest --- demos/moveit_pick_place/Dockerfile | 7 +++--- demos/moveit_pick_place/README.md | 16 +++++++------- .../config/medkit_params.yaml | 4 +++- .../config/panda_manifest.yaml | 12 +++++----- demos/moveit_pick_place/inject-collision.sh | 11 +++++++++- .../moveit_pick_place/inject-grasp-failure.sh | 11 +++++++++- .../inject-planning-failure.sh | 11 +++++++++- demos/moveit_pick_place/package.xml | 5 +++++ demos/moveit_pick_place/restore-normal.sh | 11 +++++++++- demos/moveit_pick_place/run-demo.sh | 22 ++++++++++++------- .../scripts/manipulation_monitor.py | 6 ++--- .../scripts/pick_place_loop.py | 8 +++++++ 12 files changed, 90 insertions(+), 34 deletions(-) diff --git a/demos/moveit_pick_place/Dockerfile b/demos/moveit_pick_place/Dockerfile index ffcdfb6..5975e8d 100644 --- a/demos/moveit_pick_place/Dockerfile +++ b/demos/moveit_pick_place/Dockerfile @@ -32,9 +32,10 @@ RUN apt-get update && apt-get install -y \ sqlite3 libsqlite3-dev git curl \ && rm -rf /var/lib/apt/lists/* -# Clone ros2_medkit from GitHub +# Clone ros2_medkit from GitHub (pinned to a specific ref for reproducibility) +ARG ROS2_MEDKIT_REF=main WORKDIR ${COLCON_WS}/src -RUN git clone --depth 1 https://github.com/selfpatch/ros2_medkit.git && \ +RUN git clone --depth 1 --branch ${ROS2_MEDKIT_REF} https://github.com/selfpatch/ros2_medkit.git && \ mv ros2_medkit/src/ros2_medkit_gateway \ ros2_medkit/src/ros2_medkit_msgs \ ros2_medkit/src/ros2_medkit_serialization \ @@ -53,7 +54,7 @@ COPY scripts/ ${COLCON_WS}/src/moveit_medkit_demo/scripts/ WORKDIR ${COLCON_WS} RUN bash -c "source /opt/ros/jazzy/setup.bash && \ rosdep update && \ - rosdep install --from-paths src --ignore-src -r -y || true" && \ + rosdep install --from-paths src --ignore-src -r -y" && \ bash -c "source /opt/ros/jazzy/setup.bash && \ colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF" diff --git a/demos/moveit_pick_place/README.md b/demos/moveit_pick_place/README.md index 61177d9..d91e707 100644 --- a/demos/moveit_pick_place/README.md +++ b/demos/moveit_pick_place/README.md @@ -46,10 +46,10 @@ That's it! The script will: ### 2. Available Options ```bash -./run-demo.sh # Default: CPU, fake hardware, daemon mode -./run-demo.sh --gazebo # Gazebo Harmonic physics simulation -./run-demo.sh --gazebo --nvidia # Gazebo + GPU acceleration -./run-demo.sh --nvidia # GPU acceleration (fake hardware) +./run-demo.sh # Default: Gazebo simulation, daemon mode +./run-demo.sh --fake # Fake hardware (mock controllers, no physics) +./run-demo.sh --nvidia # Gazebo + GPU acceleration +./run-demo.sh --fake --nvidia # Fake hardware + GPU acceleration ./run-demo.sh --headless # No GUI (CI/server) ./run-demo.sh --attached # Foreground with logs ./run-demo.sh --no-cache # Rebuild without cache @@ -57,8 +57,8 @@ That's it! The script will: ``` **Simulation modes:** -- **Default (fake hardware)** — Mock controllers echo commanded positions instantly. Fast startup (~10s), works headless, no physics. Good for diagnostics testing. -- **Gazebo (`--gazebo`)** — Gazebo Harmonic physics simulation with `gz_ros2_control`. Realistic dynamics, 3D world view. Slower startup (~30s), needs X11 or `--headless`. Recommended with `--nvidia` for GPU acceleration. +- **Default (Gazebo)** — Gazebo Harmonic physics simulation with `gz_ros2_control`. Realistic dynamics, 3D world view. Slower startup (~30s), needs X11 or `--headless`. Recommended with `--nvidia` for GPU acceleration. +- **Fake hardware (`--fake`)** — Mock controllers echo commanded positions instantly. Fast startup (~10s), works headless, no physics. Good for diagnostics testing. ### 3. Moving the Arm @@ -141,8 +141,8 @@ Areas │ Components │ ├── moveit-planning — OMPL planning pipeline │ │ Apps: move-group -│ └── task-constructor — Pick-and-place coordinator -│ Apps: mtc-pick-place +│ └── pick-place-loop — Pick-and-place demo node +│ Apps: pick-place-node │ ├── diagnostics/ — ros2_medkit gateway and fault management │ Components diff --git a/demos/moveit_pick_place/config/medkit_params.yaml b/demos/moveit_pick_place/config/medkit_params.yaml index 107fe0b..10d4cc8 100644 --- a/demos/moveit_pick_place/config/medkit_params.yaml +++ b/demos/moveit_pick_place/config/medkit_params.yaml @@ -11,7 +11,9 @@ diagnostics: refresh_interval_ms: 10000 # 10 seconds (default), reduces log spam cors: - allowed_origins: ["*"] + # Restrict origins to localhost by default; add explicit origins + # (e.g., "http://your-ui-host:3000") for cross-host access. + allowed_origins: ["http://localhost:8080", "http://localhost:3000"] allowed_methods: ["GET", "PUT", "POST", "DELETE", "OPTIONS"] allowed_headers: ["Content-Type", "Accept"] allow_credentials: false diff --git a/demos/moveit_pick_place/config/panda_manifest.yaml b/demos/moveit_pick_place/config/panda_manifest.yaml index 5954e17..f5f57e4 100644 --- a/demos/moveit_pick_place/config/panda_manifest.yaml +++ b/demos/moveit_pick_place/config/panda_manifest.yaml @@ -57,10 +57,10 @@ components: description: "MoveIt 2 motion planning pipeline (OMPL)" area: planning - - id: task-constructor - name: "Task Constructor" + - id: pick-place-loop + name: "Pick-and-Place Loop" type: "controller" - description: "MoveIt Task Constructor pick-and-place coordinator" + description: "Custom pick_place_loop.py pick-and-place demo node" area: planning - id: gateway @@ -135,10 +135,10 @@ apps: node_name: move_group namespace: / - - id: mtc-pick-place + - id: pick-place-node name: "Pick-and-Place Loop" category: "application" - is_located_on: task-constructor + is_located_on: pick-place-loop description: "Continuous pick-and-place task executor" depends_on: - move-group @@ -196,7 +196,7 @@ functions: category: "manipulation" description: "Pick objects and place them at target positions" hosted_by: - - mtc-pick-place + - pick-place-node - move-group - panda-arm-controller - panda-hand-controller diff --git a/demos/moveit_pick_place/inject-collision.sh b/demos/moveit_pick_place/inject-collision.sh index f7b6d5a..ad6ad9d 100755 --- a/demos/moveit_pick_place/inject-collision.sh +++ b/demos/moveit_pick_place/inject-collision.sh @@ -24,7 +24,16 @@ fi # Add a sphere right in the arm's working area via /apply_planning_scene service echo "Adding surprise obstacle (sphere) to planning scene..." -docker exec moveit_medkit_demo bash -c " + +# Auto-detect running demo container (supports CPU and NVIDIA profiles) +CONTAINER="${CONTAINER_NAME:-$(docker ps --format '{{.Names}}' | grep -E '^moveit_medkit_demo(_nvidia)?$' | head -n1)}" +if [ -z "${CONTAINER}" ]; then + echo "❌ Could not find a running MoveIt demo container." + echo " Start the demo first, or set CONTAINER_NAME explicitly." + exit 1 +fi + +docker exec "${CONTAINER}" bash -c " source /opt/ros/jazzy/setup.bash && \ source /root/demo_ws/install/setup.bash && \ python3 -c \" diff --git a/demos/moveit_pick_place/inject-grasp-failure.sh b/demos/moveit_pick_place/inject-grasp-failure.sh index c3b9716..047c84a 100755 --- a/demos/moveit_pick_place/inject-grasp-failure.sh +++ b/demos/moveit_pick_place/inject-grasp-failure.sh @@ -28,7 +28,16 @@ fi # Move the target cylinder far away from the robot via /apply_planning_scene service echo "Moving target object to unreachable position (5.0, 5.0, 0.1)..." -docker exec moveit_medkit_demo bash -c " + +# Auto-detect running demo container (supports CPU and NVIDIA profiles) +CONTAINER="${CONTAINER_NAME:-$(docker ps --format '{{.Names}}' | grep -E '^moveit_medkit_demo(_nvidia)?$' | head -n1)}" +if [ -z "${CONTAINER}" ]; then + echo "❌ Could not find a running MoveIt demo container." + echo " Start the demo first, or set CONTAINER_NAME explicitly." + exit 1 +fi + +docker exec "${CONTAINER}" bash -c " source /opt/ros/jazzy/setup.bash && \ source /root/demo_ws/install/setup.bash && \ python3 -c \" diff --git a/demos/moveit_pick_place/inject-planning-failure.sh b/demos/moveit_pick_place/inject-planning-failure.sh index a12e70c..4fc77b7 100755 --- a/demos/moveit_pick_place/inject-planning-failure.sh +++ b/demos/moveit_pick_place/inject-planning-failure.sh @@ -24,7 +24,16 @@ fi # Add a large wall via /apply_planning_scene service (reliable) echo "Adding collision wall to planning scene..." -docker exec moveit_medkit_demo bash -c " + +# Auto-detect running demo container (supports CPU and NVIDIA profiles) +CONTAINER="${CONTAINER_NAME:-$(docker ps --format '{{.Names}}' | grep -E '^moveit_medkit_demo(_nvidia)?$' | head -n1)}" +if [ -z "${CONTAINER}" ]; then + echo "❌ Could not find a running MoveIt demo container." + echo " Start the demo first, or set CONTAINER_NAME explicitly." + exit 1 +fi + +docker exec "${CONTAINER}" bash -c " source /opt/ros/jazzy/setup.bash && \ source /root/demo_ws/install/setup.bash && \ python3 -c \" diff --git a/demos/moveit_pick_place/package.xml b/demos/moveit_pick_place/package.xml index d4e805d..f1f3e50 100644 --- a/demos/moveit_pick_place/package.xml +++ b/demos/moveit_pick_place/package.xml @@ -10,6 +10,11 @@ ament_cmake ros2launch + rclpy + action_msgs + sensor_msgs + moveit_msgs + ros2_medkit_msgs moveit_ros_planning_interface moveit_resources_panda_moveit_config moveit_resources_panda_description diff --git a/demos/moveit_pick_place/restore-normal.sh b/demos/moveit_pick_place/restore-normal.sh index f98f6a5..c1f625e 100755 --- a/demos/moveit_pick_place/restore-normal.sh +++ b/demos/moveit_pick_place/restore-normal.sh @@ -24,7 +24,16 @@ fi # 1. Remove injected collision objects via /apply_planning_scene service # Using service call (not topic pub) for reliable planning scene updates echo "Removing injected collision objects..." -docker exec moveit_medkit_demo bash -c " + +# Auto-detect running demo container (supports CPU and NVIDIA profiles) +CONTAINER="${CONTAINER_NAME:-$(docker ps --format '{{.Names}}' | grep -E '^moveit_medkit_demo(_nvidia)?$' | head -n1)}" +if [ -z "${CONTAINER}" ]; then + echo "❌ Could not find a running MoveIt demo container." + echo " Start the demo first, or set CONTAINER_NAME explicitly." + exit 1 +fi + +docker exec "${CONTAINER}" bash -c " source /opt/ros/jazzy/setup.bash && \ source /root/demo_ws/install/setup.bash && \ python3 -c \" diff --git a/demos/moveit_pick_place/run-demo.sh b/demos/moveit_pick_place/run-demo.sh index 573bec2..1acdd63 100755 --- a/demos/moveit_pick_place/run-demo.sh +++ b/demos/moveit_pick_place/run-demo.sh @@ -15,18 +15,22 @@ if ! command -v docker &> /dev/null; then exit 1 fi -# Setup X11 forwarding for RViz GUI -echo "Setting up X11 forwarding..." -xhost +local:docker 2>/dev/null || { - echo " Warning: xhost failed. GUI may not work." - echo " Install with: sudo apt install x11-xserver-utils" -} +# Setup X11 forwarding for RViz GUI (skip in headless mode or when no display) +if [[ "${HEADLESS:-false}" != "true" ]] && [[ -n "${DISPLAY:-}" ]]; then + echo "Setting up X11 forwarding..." + xhost +local:docker 2>/dev/null || { + echo " Warning: xhost failed. GUI may not work." + echo " Install with: sudo apt install x11-xserver-utils" + } +fi # Cleanup function cleanup() { echo "" echo "Cleaning up..." - xhost -local:docker 2>/dev/null || true + if [[ "${HEADLESS:-false}" != "true" ]] && [[ -n "${DISPLAY:-}" ]]; then + xhost -local:docker 2>/dev/null || true + fi echo "Done!" } trap cleanup EXIT @@ -44,6 +48,7 @@ usage() { echo "" echo "Options:" echo " --nvidia Use NVIDIA GPU acceleration" + echo " --gazebo Run in Gazebo simulation mode (default)" echo " --fake Use fake hardware (mock controllers, no physics)" echo " --no-cache Build Docker images without cache" echo " --headless Run without GUI (default: GUI enabled)" @@ -53,6 +58,7 @@ usage() { echo "" echo "Examples:" echo " $0 # Daemon mode with Gazebo (default)" + echo " $0 --gazebo # Gazebo simulation mode (default)" echo " $0 --attached # Foreground with logs" echo " $0 --headless # Headless mode (no GUI)" echo " $0 --fake # Fake hardware (no physics sim)" @@ -126,7 +132,7 @@ echo "Run mode: $([ "$DETACH_MODE" = "true" ] && echo "daemon (background)" || e # Build and run echo " Building and starting demo..." -echo " (First run takes ~15-20 min, downloading ~5-6GB image)" +echo " (First run takes ~15-20 min, downloading ~7GB image)" echo "" echo "🌐 REST API available at: http://localhost:8080/api/v1/" echo "🌐 Web UI available at: http://localhost:3000/" diff --git a/demos/moveit_pick_place/scripts/manipulation_monitor.py b/demos/moveit_pick_place/scripts/manipulation_monitor.py index c64420c..463fbf8 100755 --- a/demos/moveit_pick_place/scripts/manipulation_monitor.py +++ b/demos/moveit_pick_place/scripts/manipulation_monitor.py @@ -26,7 +26,7 @@ Reports faults via /fault_manager/report_fault service. """ -from typing import Dict, Optional +from typing import Dict import rclpy from rclpy.node import Node @@ -209,11 +209,9 @@ def controller_status_callback(self, msg: GoalStatusArray): def joint_state_callback(self, msg: JointState): """Check joint positions against limits.""" - for i, name in enumerate(msg.name): + for name, position in zip(msg.name, msg.position): if name not in PANDA_JOINT_LIMITS: continue - - position = msg.position[i] lower, upper = PANDA_JOINT_LIMITS[name] dist_to_lower = position - lower diff --git a/demos/moveit_pick_place/scripts/pick_place_loop.py b/demos/moveit_pick_place/scripts/pick_place_loop.py index 78abb48..0aa63c0 100755 --- a/demos/moveit_pick_place/scripts/pick_place_loop.py +++ b/demos/moveit_pick_place/scripts/pick_place_loop.py @@ -85,6 +85,7 @@ def __init__(self): # State flags self.move_group_ready = False + self.goal_in_flight = False # Guard against overlapping goals # Wait for MoveGroup with periodic retries (non-blocking startup) self.get_logger().info("PickPlaceLoop initialized, waiting for MoveGroup...") @@ -108,6 +109,8 @@ def execute_cycle(self): """One pick-and-place cycle step.""" if not self.move_group_ready: return + if self.goal_in_flight: + return # Skip — previous goal still executing phase = self.phases[self.phase_idx] self.cycle_count += 1 @@ -145,6 +148,7 @@ def send_joint_goal(self, target_name, label): goal.request.goal_constraints.append(constraints) self.get_logger().info(f"Sending {label} goal (target: {target_name})...") + self.goal_in_flight = True future = self.move_group_client.send_goal_async(goal) future.add_done_callback(lambda f: self.goal_response_callback(f, label)) @@ -153,6 +157,7 @@ def goal_response_callback(self, future, label): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().error(f"{label} goal REJECTED") + self.goal_in_flight = False return self.get_logger().info(f"{label} goal accepted, waiting for result...") @@ -164,6 +169,8 @@ def result_callback(self, future, label): result = future.result().result status = future.result().status + self.goal_in_flight = False + if status == GoalStatus.STATUS_SUCCEEDED: self.get_logger().info(f"{label} SUCCEEDED") # Advance state machine @@ -182,6 +189,7 @@ def main(args=None): try: rclpy.spin(node) except KeyboardInterrupt: + # Allow clean shutdown on Ctrl+C without printing a traceback. pass finally: node.destroy_node() From 8e6932279e748f2c281dbe2213fd3d5127d05385 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 8 Feb 2026 20:11:24 +0000 Subject: [PATCH 5/6] fix(moveit-demo): restore rosdep || true with explanatory comment ros2_medkit packages are not in rosdep indices (built from source), so rosdep install always exits non-zero. The || true is necessary but now has a comment explaining why. --- demos/moveit_pick_place/Dockerfile | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/demos/moveit_pick_place/Dockerfile b/demos/moveit_pick_place/Dockerfile index 5975e8d..428888c 100644 --- a/demos/moveit_pick_place/Dockerfile +++ b/demos/moveit_pick_place/Dockerfile @@ -51,10 +51,13 @@ COPY launch/ ${COLCON_WS}/src/moveit_medkit_demo/launch/ COPY scripts/ ${COLCON_WS}/src/moveit_medkit_demo/scripts/ # Build ros2_medkit and demo package +# Note: rosdep install uses || true because ros2_medkit packages are not in +# rosdep indices (they're built from source). All system deps are already +# installed via apt above; rosdep handles any transitive deps it can resolve. WORKDIR ${COLCON_WS} RUN bash -c "source /opt/ros/jazzy/setup.bash && \ rosdep update && \ - rosdep install --from-paths src --ignore-src -r -y" && \ + rosdep install --from-paths src --ignore-src -r -y || true" && \ bash -c "source /opt/ros/jazzy/setup.bash && \ colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF" From 64bb3eea62335ebdaeeddc6a82e3006f55b62312 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Mon, 9 Feb 2026 17:26:23 +0000 Subject: [PATCH 6/6] feat(moveit_pick_place): Add factory world --- demos/moveit_pick_place/CMakeLists.txt | 5 + demos/moveit_pick_place/Dockerfile | 1 + .../launch/demo_gazebo.launch.py | 135 ++- demos/moveit_pick_place/worlds/factory.sdf | 997 ++++++++++++++++++ 4 files changed, 1136 insertions(+), 2 deletions(-) create mode 100644 demos/moveit_pick_place/worlds/factory.sdf diff --git a/demos/moveit_pick_place/CMakeLists.txt b/demos/moveit_pick_place/CMakeLists.txt index 421c1b5..8e3fb5a 100644 --- a/demos/moveit_pick_place/CMakeLists.txt +++ b/demos/moveit_pick_place/CMakeLists.txt @@ -13,6 +13,11 @@ install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config ) +# Install world files +install(DIRECTORY worlds/ + DESTINATION share/${PROJECT_NAME}/worlds +) + # Install scripts install(PROGRAMS scripts/manipulation_monitor.py diff --git a/demos/moveit_pick_place/Dockerfile b/demos/moveit_pick_place/Dockerfile index 428888c..1693d17 100644 --- a/demos/moveit_pick_place/Dockerfile +++ b/demos/moveit_pick_place/Dockerfile @@ -49,6 +49,7 @@ COPY package.xml CMakeLists.txt ${COLCON_WS}/src/moveit_medkit_demo/ COPY config/ ${COLCON_WS}/src/moveit_medkit_demo/config/ COPY launch/ ${COLCON_WS}/src/moveit_medkit_demo/launch/ COPY scripts/ ${COLCON_WS}/src/moveit_medkit_demo/scripts/ +COPY worlds/ ${COLCON_WS}/src/moveit_medkit_demo/worlds/ # Build ros2_medkit and demo package # Note: rosdep install uses || true because ros2_medkit packages are not in diff --git a/demos/moveit_pick_place/launch/demo_gazebo.launch.py b/demos/moveit_pick_place/launch/demo_gazebo.launch.py index 34c777e..2910893 100644 --- a/demos/moveit_pick_place/launch/demo_gazebo.launch.py +++ b/demos/moveit_pick_place/launch/demo_gazebo.launch.py @@ -54,6 +54,11 @@ def generate_launch_description(): demo_pkg_dir, "config", "panda_manifest.yaml" ) + # Factory world file path + factory_world = os.path.join( + demo_pkg_dir, "worlds", "factory.sdf" + ) + headless = LaunchConfiguration("headless", default="False") # ── Robot description (URDF with ros2_control + Gazebo hardware) ─ @@ -99,6 +104,7 @@ def generate_launch_description(): world_joint = ( ' \n' ' \n' + ' \n' ' \n' ' \n' ' \n' @@ -107,6 +113,122 @@ def generate_launch_description(): "", world_joint + "" ) + # ── Inject Gazebo visual materials for robot links ─────────────── + # Gives the Panda arm a realistic industrial appearance in Gazebo + # with white body links, dark joints, and orange accents (Franka + # Emika brand colors). + gazebo_materials = ( + # Base and main body links — white with subtle metallic sheen + ' \n' + " \n" + " \n" + " 0.85 0.85 0.85 1\n" + " 0.92 0.92 0.92 1\n" + " 0.6 0.6 0.6 1\n" + " \n" + " \n" + " \n" + ' \n' + " \n" + " \n" + " 0.85 0.85 0.85 1\n" + " 0.92 0.92 0.92 1\n" + " 0.6 0.6 0.6 1\n" + " \n" + " \n" + " \n" + # Joints — dark anthracite grey + ' \n' + " \n" + " \n" + " 0.25 0.25 0.28 1\n" + " 0.35 0.35 0.38 1\n" + " 0.4 0.4 0.4 1\n" + " \n" + " \n" + " \n" + ' \n' + " \n" + " \n" + " 0.85 0.85 0.85 1\n" + " 0.92 0.92 0.92 1\n" + " 0.6 0.6 0.6 1\n" + " \n" + " \n" + " \n" + ' \n' + " \n" + " \n" + " 0.25 0.25 0.28 1\n" + " 0.35 0.35 0.38 1\n" + " 0.4 0.4 0.4 1\n" + " \n" + " \n" + " \n" + # Upper arm — white + ' \n' + " \n" + " \n" + " 0.85 0.85 0.85 1\n" + " 0.92 0.92 0.92 1\n" + " 0.6 0.6 0.6 1\n" + " \n" + " \n" + " \n" + # Wrist joint — dark + ' \n' + " \n" + " \n" + " 0.25 0.25 0.28 1\n" + " 0.35 0.35 0.38 1\n" + " 0.4 0.4 0.4 1\n" + " \n" + " \n" + " \n" + # End-effector flange — white + ' \n' + " \n" + " \n" + " 0.85 0.85 0.85 1\n" + " 0.92 0.92 0.92 1\n" + " 0.6 0.6 0.6 1\n" + " \n" + " \n" + " \n" + # Hand / gripper — dark grey with slight blue tint + ' \n' + " \n" + " \n" + " 0.2 0.22 0.28 1\n" + " 0.3 0.32 0.38 1\n" + " 0.45 0.45 0.5 1\n" + " \n" + " \n" + " \n" + # Finger tips — dark rubber-like + ' \n' + " \n" + " \n" + " 0.15 0.15 0.15 1\n" + " 0.22 0.22 0.22 1\n" + " 0.1 0.1 0.1 1\n" + " \n" + " \n" + " \n" + ' \n' + " \n" + " \n" + " 0.15 0.15 0.15 1\n" + " 0.22 0.22 0.22 1\n" + " 0.1 0.1 0.1 1\n" + " \n" + " \n" + " \n" + ) + robot_description_raw = robot_description_raw.replace( + "", gazebo_materials + "" + ) + robot_description = {"robot_description": robot_description_raw} # ── SRDF ───────────────────────────────────────────────────────── @@ -185,6 +307,11 @@ def generate_launch_description(): value="offscreen", condition=IfCondition(headless), ), + # ── Add world directory to Gazebo resource path ──────── + SetEnvironmentVariable( + name="GZ_SIM_RESOURCE_PATH", + value=os.path.join(demo_pkg_dir, "worlds"), + ), # ── Gazebo Harmonic (GUI) ──────────────────────────────── IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -198,7 +325,9 @@ def generate_launch_description(): ) ] ), - launch_arguments={"gz_args": "-r empty.sdf"}.items(), + launch_arguments={ + "gz_args": f"-r {factory_world}" + }.items(), condition=UnlessCondition(headless), ), # ── Gazebo Harmonic (headless — server only) ───────────── @@ -214,7 +343,9 @@ def generate_launch_description(): ) ] ), - launch_arguments={"gz_args": "-r -s empty.sdf"}.items(), + launch_arguments={ + "gz_args": f"-r -s {factory_world}" + }.items(), condition=IfCondition(headless), ), # ── Clock bridge (Gazebo → ROS 2) ──────────────────────── diff --git a/demos/moveit_pick_place/worlds/factory.sdf b/demos/moveit_pick_place/worlds/factory.sdf new file mode 100644 index 0000000..25f0187 --- /dev/null +++ b/demos/moveit_pick_place/worlds/factory.sdf @@ -0,0 +1,997 @@ + + + + + + + 0.001 + 1.0 + 1000 + + + + 0.6 0.6 0.6 1.0 + 0.2 0.22 0.25 1.0 + true + false + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.9 0.88 0.82 1 + 0.3 0.3 0.3 1 + -0.3 0.2 -1.0 + + 100 + 0.9 + 0.01 + 0.001 + + + + + + 0 0 3.5 0 0 0 + 0.95 0.92 0.85 1 + 0.2 0.2 0.2 1 + + 15 + 0.5 + 0.05 + 0.01 + + false + + + + + 1.5 0 3.5 0 0 0 + 0.9 0.9 0.95 1 + 0.15 0.15 0.15 1 + + 12 + 0.5 + 0.05 + 0.01 + + false + + + + + -2 -2 3 0 0 0 + 0.4 0.42 0.48 1 + 0.05 0.05 0.05 1 + + 20 + 0.5 + 0.02 + 0.005 + + false + + + + + true + 0 0 0 0 0 0 + + + + + 0 0 1 + 20 20 + + + + + + + 0 0 1 + 20 20 + + + + 0.55 0.55 0.52 1 + 0.65 0.64 0.60 1 + 0.15 0.15 0.15 1 + + + + + + + + + true + 0 1.2 0.001 0 0 0 + + + + 3.0 0.08 0.002 + + + 0.85 0.75 0.0 1 + 0.95 0.85 0.0 1 + 0.3 0.3 0.0 1 + 0.1 0.09 0.0 1 + + + + + + + + true + 0 -1.2 0.001 0 0 0 + + + + 3.0 0.08 0.002 + + + 0.85 0.75 0.0 1 + 0.95 0.85 0.0 1 + 0.3 0.3 0.0 1 + 0.1 0.09 0.0 1 + + + + + + + + true + -1.5 0 0.001 0 0 0 + + + + 0.08 2.48 0.002 + + + 0.85 0.75 0.0 1 + 0.95 0.85 0.0 1 + 0.3 0.3 0.0 1 + 0.1 0.09 0.0 1 + + + + + + + + true + 1.5 0 0.001 0 0 0 + + + + 0.08 2.48 0.002 + + + 0.85 0.75 0.0 1 + 0.95 0.85 0.0 1 + 0.3 0.3 0.0 1 + 0.1 0.09 0.0 1 + + + + + + + + true + 0 0 0 0 0 0 + + + 0 0 0.725 0 0 0 + + + 0.9 0.7 0.05 + + + + + 0.9 0.7 0.05 + + + 0.25 0.27 0.30 1 + 0.35 0.37 0.40 1 + 0.6 0.6 0.6 1 + + + + + + 0.40 0.30 0.35 0 0 0 + 0.05 0.05 0.70 + + 0.05 0.05 0.70 + 0.3 0.3 0.32 10.4 0.4 0.42 10.5 0.5 0.5 1 + + + + + 0.40 -0.30 0.35 0 0 0 + 0.05 0.05 0.70 + + 0.05 0.05 0.70 + 0.3 0.3 0.32 10.4 0.4 0.42 10.5 0.5 0.5 1 + + + + + -0.40 0.30 0.35 0 0 0 + 0.05 0.05 0.70 + + 0.05 0.05 0.70 + 0.3 0.3 0.32 10.4 0.4 0.42 10.5 0.5 0.5 1 + + + + + -0.40 -0.30 0.35 0 0 0 + 0.05 0.05 0.70 + + 0.05 0.05 0.70 + 0.3 0.3 0.32 10.4 0.4 0.42 10.5 0.5 0.5 1 + + + + + 0.40 0 0.15 0 0 0 + + 0.03 0.56 0.03 + 0.3 0.3 0.32 10.4 0.4 0.42 10.5 0.5 0.5 1 + + + + -0.40 0 0.15 0 0 0 + + 0.03 0.56 0.03 + 0.3 0.3 0.32 10.4 0.4 0.42 10.5 0.5 0.5 1 + + + + + + + true + 0.0 0.0 0 0 0 0 + + + + 1.8 0 0.72 0 0 0 + + + 2.4 0.6 0.02 + + + + 1.01.0 + + + + + + 2.4 0.6 0.02 + + + 0.08 0.08 0.08 1 + 0.15 0.15 0.15 1 + 0.05 0.05 0.05 1 + + + + + + + 1.8 0 0.36 0 0 0 + + + 2.5 0.65 0.7 + + + 0.28 0.30 0.33 1 + 0.38 0.40 0.43 1 + 0.4 0.4 0.4 1 + + + + + 2.5 0.65 0.7 + + + + + + + 1.8 -0.34 0.76 0 0 0 + + + 2.5 0.03 0.06 + + + 0.7 0.6 0.0 1 + 0.85 0.75 0.0 1 + 0.4 0.4 0.1 1 + + + + + + + 1.8 0.34 0.76 0 0 0 + + + 2.5 0.03 0.06 + + + 0.7 0.6 0.0 1 + 0.85 0.75 0.0 1 + 0.4 0.4 0.1 1 + + + + + + + 0.58 0 0.72 1.5708 0 0 + + + 0.0350.62 + + + 0.5 0.5 0.5 1 + 0.65 0.65 0.65 1 + 0.7 0.7 0.7 1 + + + + + + + 3.02 0 0.72 1.5708 0 0 + + + 0.0350.62 + + + 0.5 0.5 0.5 1 + 0.65 0.65 0.65 1 + 0.7 0.7 0.7 1 + + + + + + + 1.4 0 0.72 1.5708 0 0 + + + 0.0250.58 + + + 0.45 0.45 0.45 1 + 0.6 0.6 0.6 1 + 0.65 0.65 0.65 1 + + + + + + 2.2 0 0.72 1.5708 0 0 + + + 0.0250.58 + + + 0.45 0.45 0.45 1 + 0.6 0.6 0.6 1 + 0.65 0.65 0.65 1 + + + + + + + + + false + 1.2 0 0.77 0 0 0.1 + + + 0.3 + + 0.000080.000080.00005 + + + + + 0.06 0.06 0.06 + + + + + 0.06 0.06 0.06 + + + 0.7 0.1 0.1 1 + 0.85 0.15 0.15 1 + 0.3 0.1 0.1 1 + + + + + + + + false + 1.6 -0.05 0.77 0 0 -0.05 + + + 0.25 + + 0.000060.000060.00004 + + + + + 0.05 0.05 0.05 + + + + + 0.05 0.05 0.05 + + + 0.1 0.15 0.7 1 + 0.15 0.25 0.85 1 + 0.1 0.1 0.3 1 + + + + + + + + false + 2.0 0.08 0.77 0 0 0.15 + + + 0.35 + + 0.00010.00010.00007 + + + + + 0.07 0.05 0.04 + + + + + 0.07 0.05 0.04 + + + 0.1 0.6 0.15 1 + 0.15 0.75 0.2 1 + 0.1 0.25 0.1 1 + + + + + + + + false + 2.4 -0.03 0.76 0 0 0 + + + 0.2 + + 0.000040.000040.00003 + + + + + 0.0250.05 + + + + + 0.0250.05 + + + 0.8 0.7 0.05 1 + 0.9 0.85 0.1 1 + 0.3 0.3 0.1 1 + + + + + + + + true + -0.5 0.6 0 0 0 0 + + + 0.17 0.12 0.305 0 0 0 + 0.03 0.03 0.61 + + 0.03 0.03 0.61 + 0.3 0.3 0.32 10.42 0.42 0.44 10.4 0.4 0.4 1 + + + + 0.17 -0.12 0.305 0 0 0 + 0.03 0.03 0.61 + + 0.03 0.03 0.61 + 0.3 0.3 0.32 10.42 0.42 0.44 10.4 0.4 0.4 1 + + + + -0.17 0.12 0.305 0 0 0 + 0.03 0.03 0.61 + + 0.03 0.03 0.61 + 0.3 0.3 0.32 10.42 0.42 0.44 10.4 0.4 0.4 1 + + + + -0.17 -0.12 0.305 0 0 0 + 0.03 0.03 0.61 + + 0.03 0.03 0.61 + 0.3 0.3 0.32 10.42 0.42 0.44 10.4 0.4 0.4 1 + + + + + 0 0 0.615 0 0 0 + 0.42 0.32 0.02 + + 0.42 0.32 0.02 + 0.3 0.3 0.32 10.42 0.42 0.44 10.4 0.4 0.4 1 + + + + + 0 0 0.2 0 0 0 + + 0.30 0.02 0.02 + 0.3 0.3 0.32 10.42 0.42 0.44 10.4 0.4 0.4 1 + + + + + 0 0 0.635 0 0 0 + + 0.4 0.3 0.02 + + + 0.4 0.3 0.02 + + 0.1 0.2 0.55 1 + 0.15 0.3 0.7 1 + 0.2 0.2 0.3 1 + + + + + + 0 0.155 0.74 0 0 0 + + 0.4 0.01 0.19 + + 0.1 0.2 0.55 1 + 0.15 0.3 0.7 1 + 0.2 0.2 0.3 1 + + + + + + 0 -0.155 0.74 0 0 0 + + 0.4 0.01 0.19 + + 0.1 0.2 0.55 1 + 0.15 0.3 0.7 1 + 0.2 0.2 0.3 1 + + + + + + -0.205 0 0.74 0 0 0 + + 0.01 0.3 0.19 + + 0.1 0.2 0.55 1 + 0.15 0.3 0.7 1 + 0.2 0.2 0.3 1 + + + + + + 0.205 0 0.74 0 0 0 + + 0.01 0.3 0.19 + + 0.1 0.2 0.55 1 + 0.15 0.3 0.7 1 + 0.2 0.2 0.3 1 + + + + + + + + + true + -3 0 1.5 0 0 0 + + + + 0.15 12 3.0 + + + 0.45 0.48 0.50 1 + 0.55 0.58 0.60 1 + 0.08 0.08 0.08 1 + + + + + 0.15 12 3.0 + + + + + + + + true + 2.5 -4 1.5 0 0 0 + + + + 11 0.15 3.0 + + + 0.42 0.45 0.48 1 + 0.52 0.55 0.58 1 + 0.08 0.08 0.08 1 + + + + + 11 0.15 3.0 + + + + + + + + true + 2.5 4 1.5 0 0 0 + + + + 11 0.15 3.0 + + + 0.42 0.45 0.48 1 + 0.52 0.55 0.58 1 + 0.08 0.08 0.08 1 + + + + + 11 0.15 3.0 + + + + + + + + true + -2.92 0 0.1 0 0 0 + + + 0.02 12 0.2 + + 0.3 0.3 0.3 1 + 0.4 0.4 0.4 1 + 0.15 0.15 0.15 1 + + + + + + + + true + -2.90 -1.0 1.2 0 0 0 + + + + 0.08 0.6 0.8 + + 0.5 0.5 0.48 1 + 0.65 0.65 0.62 1 + 0.2 0.2 0.2 1 + + + + + + 0.045 0.15 0.25 0 0 0 + + 0.020.01 + + 0.0 0.8 0.0 1 + 0.1 0.95 0.1 1 + 0.1 0.3 0.1 1 + 0.0 0.5 0.0 1 + + + + + + 0.045 0.15 0.15 0 0 0 + + 0.020.01 + + 0.8 0.5 0.0 1 + 0.95 0.65 0.1 1 + 0.3 0.2 0.05 1 + 0.3 0.2 0.0 1 + + + + + + + + true + -2.5 2.0 0 0 0 0 + + + 0.3 0.2 0.6 0 0 0 + 0.04 0.04 1.2 + 0.35 0.35 0.35 10.5 0.5 0.5 10.4 0.4 0.4 1 + + + + 0.3 -0.2 0.6 0 0 0 + 0.04 0.04 1.2 + 0.35 0.35 0.35 10.5 0.5 0.5 10.4 0.4 0.4 1 + + + + -0.3 0.2 0.6 0 0 0 + 0.04 0.04 1.2 + 0.35 0.35 0.35 10.5 0.5 0.5 10.4 0.4 0.4 1 + + + + -0.3 -0.2 0.6 0 0 0 + 0.04 0.04 1.2 + 0.35 0.35 0.35 10.5 0.5 0.5 10.4 0.4 0.4 1 + + + + + 0 0 0.3 0 0 0 + 0.7 0.5 0.02 + 0.4 0.4 0.4 10.55 0.55 0.55 10.3 0.3 0.3 1 + + + + 0 0 0.7 0 0 0 + 0.7 0.5 0.02 + 0.4 0.4 0.4 10.55 0.55 0.55 10.3 0.3 0.3 1 + + + + 0 0 1.1 0 0 0 + 0.7 0.5 0.02 + 0.4 0.4 0.4 10.55 0.55 0.55 10.3 0.3 0.3 1 + + + + + + + true + -2.4 2.05 0.34 0 0 0.3 + + 0.08 0.06 0.06 + 0.7 0.3 0.0 10.85 0.4 0.05 10.2 0.1 0.0 1 + + + + + true + -2.6 1.9 0.74 0 0 -0.1 + + 0.030.08 + 0.6 0.6 0.6 10.75 0.75 0.75 10.5 0.5 0.5 1 + + + + + + + true + -1.2 -0.8 0 0 0 0 + + + 0 0 0.45 0 0 0 + 0.030.9 + 0.3 0.3 0.3 10.45 0.45 0.45 10.4 0.4 0.4 1 + + + + + 0 0 0.92 0 0 0 + 0.08 0.08 0.04 + 0.6 0.6 0.0 10.8 0.8 0.0 10.3 0.3 0.0 1 + + + + + 0 0 0.955 0 0 0 + 0.0250.03 + + 0.8 0.0 0.0 1 + 0.95 0.05 0.05 1 + 0.4 0.1 0.1 1 + 0.15 0.0 0.0 1 + + + + + + + + true + 0 0 2.8 0 0 0 + + + 5 0.15 0.03 + 0.3 0.3 0.3 10.45 0.45 0.45 10.3 0.3 0.3 1 + + + + + -1.5 0 0.1 0 0 0 + 0.04 0.12 0.2 + 0.25 0.25 0.25 10.4 0.4 0.4 10.3 0.3 0.3 1 + + + + + 1.5 0 0.1 0 0 0 + 0.04 0.12 0.2 + 0.25 0.25 0.25 10.4 0.4 0.4 10.3 0.3 0.3 1 + + + + + + + true + 0.4 -0.6 0 0 0 0 + + + 0 0 0.65 0 0 0 + 0.5 0.4 0.03 + + 0.5 0.4 0.03 + + 0.6 0.58 0.52 1 + 0.75 0.72 0.65 1 + 0.2 0.2 0.2 1 + + + + + 0.22 0.17 0.325 0 0 0 + 0.04 0.04 0.65 + 0.35 0.35 0.35 10.5 0.5 0.5 10.4 0.4 0.4 1 + + + -0.22 0.17 0.325 0 0 0 + 0.04 0.04 0.65 + 0.35 0.35 0.35 10.5 0.5 0.5 10.4 0.4 0.4 1 + + + 0.22 -0.17 0.325 0 0 0 + 0.04 0.04 0.65 + 0.35 0.35 0.35 10.5 0.5 0.5 10.4 0.4 0.4 1 + + + -0.22 -0.17 0.325 0 0 0 + 0.04 0.04 0.65 + 0.35 0.35 0.35 10.5 0.5 0.5 10.4 0.4 0.4 1 + + + + + + + true + 0 -2.5 0.002 0 0 0 + + + 1.2 0.3 0.002 + + 0.0 0.45 0.7 1 + 0.05 0.55 0.85 1 + 0.1 0.1 0.1 1 + + + + + + +