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 . diff --git a/.gitignore b/.gitignore index 01e5dcc..7cdd647 100644 --- a/.gitignore +++ b/.gitignore @@ -30,6 +30,10 @@ COLCON_IGNORE .coverage htmlcov/ +# Local dev Docker files (not for CI/CD) +Dockerfile.local +docker-compose.local.yml + # Environment .env .venv/ 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..534f8a5 --- /dev/null +++ b/demos/moveit_pick_place/CMakeLists.txt @@ -0,0 +1,31 @@ +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 world files +install(DIRECTORY worlds/ + DESTINATION share/${PROJECT_NAME}/worlds +) + +# Install scripts +install(PROGRAMS + scripts/manipulation_monitor.py + scripts/pick_place_loop.py + scripts/inject-collision.sh + scripts/inject-planning-failure.sh + scripts/restore-normal.sh + 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..a072e96 --- /dev/null +++ b/demos/moveit_pick_place/Dockerfile @@ -0,0 +1,84 @@ +# 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 \ + ros-jazzy-rosbag2-storage-mcap \ + ros-jazzy-foxglove-bridge \ + sqlite3 libsqlite3-dev git curl \ + && rm -rf /var/lib/apt/lists/* + +# Create persistent directories for fault storage and rosbag recordings +RUN mkdir -p /var/lib/ros2_medkit/rosbags + +# 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 --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 \ + 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/ +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 +# 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 || 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 + +# Make inject/restore scripts available at a well-known path +ENV DEMO_SCRIPTS=${COLCON_WS}/scripts +RUN mkdir -p ${DEMO_SCRIPTS} && \ + ln -sf ${COLCON_WS}/install/moveit_medkit_demo/lib/moveit_medkit_demo/inject-collision.sh ${DEMO_SCRIPTS}/inject-collision.sh && \ + ln -sf ${COLCON_WS}/install/moveit_medkit_demo/lib/moveit_medkit_demo/inject-planning-failure.sh ${DEMO_SCRIPTS}/inject-planning-failure.sh && \ + ln -sf ${COLCON_WS}/install/moveit_medkit_demo/lib/moveit_medkit_demo/restore-normal.sh ${DEMO_SCRIPTS}/restore-normal.sh +ENV PATH="${DEMO_SCRIPTS}:${PATH}" + +EXPOSE 8080 8765 + +CMD ["bash"] diff --git a/demos/moveit_pick_place/README.md b/demos/moveit_pick_place/README.md new file mode 100644 index 0000000..1d7f759 --- /dev/null +++ b/demos/moveit_pick_place/README.md @@ -0,0 +1,393 @@ +# 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 in a **Gazebo Harmonic factory scene** while a manipulation monitor detects faults — planning failures, collisions — and reports them through the SOVD REST API with environment snapshots. + +## Status + +✅ **Demo Ready** — Docker-based deployment with MoveIt 2, Gazebo Harmonic physics simulation, factory environment, and full ros2_medkit stack. + +## Overview + +This demo demonstrates: + +- **MoveIt 2 motion planning** with the Panda 7-DOF arm and gripper +- **Gazebo Harmonic simulation** with a realistic factory scene (conveyor belt, work table, storage, lighting) +- **Continuous pick-and-place** loop as a realistic manipulation workload +- **Manipulation fault monitoring** (planning failures, collision detection) +- **Fault snapshots** — environment state captured at fault time (joint states, diagnostics) +- **SOVD-compliant REST API** with Areas → Components → Apps → Functions hierarchy +- **Manifest-based entity discovery** (hybrid mode with runtime enrichment) +- **2 fault injection scenarios** with visible Gazebo models and one-click scripts +- **Web UI** for visual entity browsing and fault monitoring + +## Prerequisites + +- Docker and docker-compose +- X11 display server (for Gazebo GUI) or `--headless` mode +- (Optional) NVIDIA GPU + [nvidia-container-toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html) — recommended for smooth Gazebo rendering +- ~7 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, ~7 GB) +2. Set up X11 forwarding for Gazebo GUI +3. Launch Panda robot in factory world + 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: Gazebo simulation, daemon mode +./run-demo.sh --nvidia # GPU acceleration (recommended) +./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 +``` + +The demo always uses **Gazebo Harmonic** physics simulation with `gz_ros2_control`. The factory scene includes a work table, conveyor belt, storage bin, and industrial lighting. Use `--nvidia` for GPU-accelerated rendering. + +### 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 │ +│ │ +│ ┌──────────┐ ┌──────────────┐ ┌──────────────────────────┐ │ +│ │ Gazebo │ │ ros2_control │ │ MoveIt 2 │ │ +│ │ Harmonic │──│ Controllers │──│ move_group (OMPL) │ │ +│ │ (physics)│ │ arm+gripper │ │ │ │ +│ └──────────┘ └──────────────┘ └──────────┬───────────────┘ │ +│ │ │ +│ ┌──────────────────────────────┐ │ │ +│ │ pick_place_loop.py │◄───────────┘ │ +│ │ (continuous manipulation) │ MoveGroup action │ +│ └──────────────────────────────┘ │ +│ │ +│ ┌──────────────────────────────────────────────────────────┐ │ +│ │ ros2_medkit Stack │ │ +│ │ │ │ +│ │ manipulation_monitor.py ──► fault_manager ◄── diag_bridge│ │ +│ │ │ │ │ │ +│ │ │ monitors: │ stores faults + │ │ +│ │ │ • /move_action status │ captures snapshots │ │ +│ │ │ • /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 +│ └── pick-place-loop — Pick-and-place demo node +│ Apps: pick-place-node +│ +├── 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/%2Fjoint_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 +``` + +### 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 (background capture, always available): +- `/joint_states` — Current joint positions at fault time +- `/diagnostics` — Active diagnostics messages + +### 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 + +The fault injection scripts are **baked into the Docker image** under `$DEMO_SCRIPTS/` (on `PATH`). The host-side `./inject-*.sh` and `./restore-normal.sh` wrappers auto-detect the running container and delegate via `docker exec`. + +You can also run them directly inside the container: + +```bash +docker exec -it moveit_medkit_demo inject-collision.sh +docker exec -it moveit_medkit_demo inject-planning-failure.sh +docker exec -it moveit_medkit_demo restore-normal.sh +``` + +> **Future:** When SOVD Scripts endpoints are available, these will be callable via `curl` against the gateway REST API. + +### 1. Planning Failure + +Blocks the robot's path with a large collision wall (visible as orange wall in Gazebo). + +```bash +./inject-planning-failure.sh +``` + +| Code | Severity | Description | +|------|----------|-------------| +| `MOTION_PLANNING_FAILED` | ERROR | MoveGroup goal ABORTED — no collision-free path | + +### 2. Collision Detection + +Spawns a surprise obstacle in the robot's active workspace (visible as red sphere in Gazebo). + +```bash +./inject-collision.sh +``` + +| Code | Severity | Description | +|------|----------|-------------| +| `MOTION_PLANNING_FAILED` | ERROR | Cannot find collision-free path around obstacle | + +### 3. Restore Normal + +Removes all injected objects (from both Gazebo and MoveIt planning scene) 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[] | {fault_code, severity_label, description}' +``` + +## 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 + +### Host-side (run from your machine) + +| Script | Description | +|--------|-------------| +| `run-demo.sh` | **Start the demo** — build and launch the Docker container | +| `stop-demo.sh` | Stop demo containers | +| `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` | Thin wrapper → `docker exec` the in-container script | +| `inject-collision.sh` | Thin wrapper → `docker exec` the in-container script | +| `restore-normal.sh` | Thin wrapper → `docker exec` the in-container script | + +### In-container (baked into Docker image, on `PATH`) + +| Script | Description | +|--------|-------------| +| `inject-planning-failure.sh` | Spawn visible wall + MoveIt collision object | +| `inject-collision.sh` | Spawn visible sphere + MoveIt collision object | +| `restore-normal.sh` | Remove Gazebo models + MoveIt objects, clear faults | +| `manipulation_monitor.py` | ROS 2 node: monitors topics and reports faults | +| `pick_place_loop.py` | ROS 2 node: continuous pick-and-place cycle | + +## 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 | +| `ros2` CLI hangs in `docker exec` | DDS discovery across container boundaries | Use gateway REST API instead of `ros2` CLI for parameter/service operations | + +## 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 | Gazebo Harmonic | +| Task | Sensor monitoring | Autonomous navigation | Pick-and-place manipulation | +| Fault types | Sensor drift, noise | Nav failures, localization | Planning failures, collisions | +| 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 | ~7 GB | +| GPU recommended | No | Optional | Recommended | + +## Technical Details + +### Monitored Topics + +| Topic | What it tells us | Fault codes | +|-------|------------------|-------------| +| `/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` | + +### Fault Codes Reference + +| Code | Severity | Trigger | +|------|----------|---------| +| `MOTION_PLANNING_FAILED` | ERROR | MoveGroup goal ABORTED (collision wall or obstacle) | +| `TRAJECTORY_EXECUTION_FAILED` | ERROR | Controller action ABORTED | +| `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 +- Factory world scene (SDF) +- 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..3a32ef0 --- /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: .data.name, + positions: .data.position, + velocities: .data.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..51ac30b --- /dev/null +++ b/demos/moveit_pick_place/check-faults.sh @@ -0,0 +1,65 @@ +#!/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: .fault_code, + severity: .severity_label, + status: .status, + description: .description, + sources: .reporting_sources, + occurrences: .occurrence_count, + first_occurred: .first_occurred, + last_occurred: .last_occurred + }' +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_label) | .[] | " \(.[0].severity_label): \(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..5874a79 --- /dev/null +++ b/demos/moveit_pick_place/config/medkit_params.yaml @@ -0,0 +1,81 @@ +# 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: + # Allow any origin for demo use + 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 + +# Fault Manager configuration (runs in root namespace) +fault_manager: + ros__parameters: + # Storage configuration + storage_type: "sqlite" + database_path: "/var/lib/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: true # Continuously cache latest messages + timeout_sec: 2.0 + max_message_size: 131072 # 128KB max per message + + # Topics to capture for all faults + default_topics: + - /joint_states + - /diagnostics + + # Rosbag recording configuration + rosbag: + enabled: true + duration_sec: 10.0 # Record 10 seconds before fault confirmation + duration_after_sec: 2.0 # Record 2 seconds after confirmation + lazy_start: false # Always recording (ring buffer) + format: "mcap" + storage_path: "/var/lib/ros2_medkit/rosbags" + max_bag_size_mb: 100 + max_total_storage_mb: 1000 + auto_cleanup: true + + topics: "config" + include_topics: + - /joint_states + - /diagnostics + - /tf + - /tf_static + - /move_action/_action/status + - /move_action/_action/feedback + - /panda_arm_controller/follow_joint_trajectory/_action/status + - /panda_arm_controller/follow_joint_trajectory/_action/feedback + - /planning_scene + exclude_topics: + - /rosout + - /parameter_events 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..f5f57e4 --- /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: pick-place-loop + name: "Pick-and-Place Loop" + type: "controller" + description: "Custom pick_place_loop.py pick-and-place demo node" + 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: pick-place-node + name: "Pick-and-Place Loop" + category: "application" + is_located_on: pick-place-loop + 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: + - pick-place-node + - 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..69d56c2 --- /dev/null +++ b/demos/moveit_pick_place/docker-compose.yml @@ -0,0 +1,70 @@ +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_gazebo.launch.py} + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix:rw + ports: + - "8080:8080" + - "8765:8765" + stdin_open: true + tty: true + command: > + bash -c "mkdir -p /var/lib/ros2_medkit/rosbags && + 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_gazebo.launch.py} + - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix:rw + ports: + - "8080:8080" + - "8765:8765" + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all + capabilities: [gpu] + stdin_open: true + tty: true + command: > + bash -c "mkdir -p /var/lib/ros2_medkit/rosbags && + 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..fca1bfe --- /dev/null +++ b/demos/moveit_pick_place/inject-collision.sh @@ -0,0 +1,11 @@ +#!/bin/bash +# Inject Collision - Spawn a surprise obstacle in the robot's workspace +set -eu + +CONTAINER="${CONTAINER_NAME:-$(docker ps --format '{{.Names}}' | grep -E '^moveit_medkit_demo(_nvidia)?(_local)?$' | head -n1)}" +if [ -z "${CONTAINER}" ]; then + echo "❌ Demo container not running. Start it first: ./run-demo.sh" + exit 1 +fi + +exec docker exec -it "${CONTAINER}" inject-collision.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..ff4784d --- /dev/null +++ b/demos/moveit_pick_place/inject-planning-failure.sh @@ -0,0 +1,11 @@ +#!/bin/bash +# Inject Planning Failure - Block the robot's path with a collision wall +set -eu + +CONTAINER="${CONTAINER_NAME:-$(docker ps --format '{{.Names}}' | grep -E '^moveit_medkit_demo(_nvidia)?(_local)?$' | head -n1)}" +if [ -z "${CONTAINER}" ]; then + echo "❌ Demo container not running. Start it first: ./run-demo.sh" + exit 1 +fi + +exec docker exec -it "${CONTAINER}" inject-planning-failure.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..36a66c3 --- /dev/null +++ b/demos/moveit_pick_place/launch/demo.launch.py @@ -0,0 +1,169 @@ +"""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 + # 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=[ + medkit_params_file, + {"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, + }, + ], + ), + # === Foxglove Bridge (WebSocket on port 8765) === + Node( + package="foxglove_bridge", + executable="foxglove_bridge", + name="foxglove_bridge", + output="screen", + parameters=[ + { + "use_sim_time": use_sim_time, + "port": 8765, + "address": "0.0.0.0", + "send_buffer_limit": 10000000, + "num_threads": 0, + } + ], + ), + # === 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..3a700f6 --- /dev/null +++ b/demos/moveit_pick_place/launch/demo_gazebo.launch.py @@ -0,0 +1,520 @@ +"""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" + ) + + # 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) ─ + # 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' + ' \n' + ) + robot_description_raw = robot_description_raw.replace( + "", 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 ───────────────────────────────────────────────────────── + 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), + ), + # ── 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( + [ + PathJoinSubstitution( + [ + FindPackageShare("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ] + ), + launch_arguments={ + "gz_args": f"-r {factory_world}" + }.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": f"-r -s {factory_world}" + }.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=[ + medkit_params_file, + {"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, + }, + ], + ), + # ═════════════════════════════════════════════════════════ + # Foxglove Bridge (WebSocket on port 8765) + # ═════════════════════════════════════════════════════════ + Node( + package="foxglove_bridge", + executable="foxglove_bridge", + name="foxglove_bridge", + output="screen", + parameters=[ + { + "use_sim_time": True, + "port": 8765, + "address": "0.0.0.0", + "send_buffer_limit": 10000000, + "num_threads": 0, + } + ], + ), + # ═════════════════════════════════════════════════════════ + # 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..91af056 --- /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:-$(docker ps --format '{{.Names}}' | grep -E '^moveit_medkit_demo(_nvidia)?(_local)?$' | head -n1)}" +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..feb316d --- /dev/null +++ b/demos/moveit_pick_place/package.xml @@ -0,0 +1,35 @@ + + + + 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 + rclpy + action_msgs + sensor_msgs + moveit_msgs + ros2_medkit_msgs + 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 + foxglove_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..68c7f89 --- /dev/null +++ b/demos/moveit_pick_place/restore-normal.sh @@ -0,0 +1,11 @@ +#!/bin/bash +# Restore Normal Operation - Remove all injected faults +set -eu + +CONTAINER="${CONTAINER_NAME:-$(docker ps --format '{{.Names}}' | grep -E '^moveit_medkit_demo(_nvidia)?(_local)?$' | head -n1)}" +if [ -z "${CONTAINER}" ]; then + echo "❌ Demo container not running. Start it first: ./run-demo.sh" + exit 1 +fi + +exec docker exec -it "${CONTAINER}" restore-normal.sh diff --git a/demos/moveit_pick_place/run-demo.sh b/demos/moveit_pick_place/run-demo.sh new file mode 100755 index 0000000..351b01d --- /dev/null +++ b/demos/moveit_pick_place/run-demo.sh @@ -0,0 +1,162 @@ +#!/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 (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..." + if [[ "${HEADLESS:-false}" != "true" ]] && [[ -n "${DISPLAY:-}" ]]; then + xhost -local:docker 2>/dev/null || true + fi + echo "Done!" +} +trap cleanup EXIT + +# Parse arguments +COMPOSE_ARGS="" +BUILD_ARGS="" +HEADLESS_MODE="false" +UPDATE_IMAGES="false" +DETACH_MODE="true" + +usage() { + echo "Usage: $0 [OPTIONS]" + echo "" + echo "Options:" + echo " --nvidia Use NVIDIA GPU acceleration" + 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 --nvidia # GPU acceleration" + echo " $0 --no-cache # Rebuild without cache" + echo " $0 --update # Pull and run latest version" +} + +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" + ;; + --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 +export LAUNCH_FILE="demo_gazebo.launch.py" +echo "Simulation: Gazebo Harmonic (physics simulation)" +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 ~7GB 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/inject-collision.sh b/demos/moveit_pick_place/scripts/inject-collision.sh new file mode 100755 index 0000000..3f799c7 --- /dev/null +++ b/demos/moveit_pick_place/scripts/inject-collision.sh @@ -0,0 +1,100 @@ +#!/bin/bash +# Inject Collision - Spawn a surprise obstacle in the robot's workspace +# Adds the object to both Gazebo (visible) and MoveIt planning scene (causes faults) +# Runs INSIDE the Docker container. + +set -e + +# shellcheck source=/dev/null +source /opt/ros/jazzy/setup.bash +# shellcheck source=/dev/null +source /root/demo_ws/install/setup.bash + +echo "🚫 Injecting COLLISION fault..." +echo " Spawning surprise obstacle in robot workspace" +echo "" + +# 1. Spawn visible model in Gazebo +# Robot base (panda_link0) is at z=0.75 in the world frame. +# Obstacle at panda_link0 frame (0.4, 0, 0.4) → world frame (0.4, 0, 1.15) +echo "Spawning visible red sphere in Gazebo..." +cat > /tmp/surprise_obstacle.sdf << 'EOSDF' + + + + true + + + 0.15 + + 0.9 0.1 0.1 1 + 0.95 0.15 0.15 1 + 0.4 0.1 0.1 1 + + + + + +EOSDF + +if ros2 run ros_gz_sim create \ + -file /tmp/surprise_obstacle.sdf \ + -name surprise_obstacle \ + -x 0.4 -y 0.0 -z 1.15 2>&1 | tail -1; then + echo " ✓ Gazebo model spawned" +else + echo " ⚠ Gazebo spawn failed (visual only — fault injection still works)" +fi + +# 2. Add to MoveIt planning scene (so planner detects the collision) +echo "Adding obstacle to MoveIt planning scene..." +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 + +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'Planning scene updated: {result.success}' if result else 'Service call failed') +node.destroy_node() +rclpy.shutdown() +" + +echo "" +echo "✓ Collision fault injected!" +echo " A red sphere is now visible in Gazebo and registered in MoveIt planning scene." +echo "" +echo "Expected faults (via manipulation_monitor → FaultManager):" +echo " - MOTION_PLANNING_FAILED: Cannot find collision-free path" +echo "" +echo "Restore with: /root/demo_ws/scripts/restore-normal.sh" diff --git a/demos/moveit_pick_place/scripts/inject-planning-failure.sh b/demos/moveit_pick_place/scripts/inject-planning-failure.sh new file mode 100755 index 0000000..35af337 --- /dev/null +++ b/demos/moveit_pick_place/scripts/inject-planning-failure.sh @@ -0,0 +1,101 @@ +#!/bin/bash +# Inject Planning Failure - Add collision wall blocking the pick-place path +# Adds the wall to both Gazebo (visible) and MoveIt planning scene (causes faults) +# Runs INSIDE the Docker container. + +set -e + +# shellcheck source=/dev/null +source /opt/ros/jazzy/setup.bash +# shellcheck source=/dev/null +source /root/demo_ws/install/setup.bash + +echo "🚫 Injecting PLANNING FAILURE fault..." +echo " Adding collision wall between pick and place positions" +echo "" + +# 1. Spawn visible wall in Gazebo +# Robot base (panda_link0) is at z=0.75 in the world frame. +# Wall at panda_link0 frame (0.3, 0.25, 0.5) → world frame (0.3, 0.25, 1.25) +# Wall dimensions: 2.0 x 0.05 x 1.0 (wide, thin, tall) +echo "Spawning visible orange wall in Gazebo..." +cat > /tmp/injected_wall.sdf << 'EOSDF' + + + + true + + + 2.0 0.05 1.0 + + 0.9 0.6 0.1 1 + 0.95 0.65 0.15 1 + 0.4 0.3 0.1 1 + + + + + +EOSDF + +if ros2 run ros_gz_sim create \ + -file /tmp/injected_wall.sdf \ + -name injected_wall \ + -x 0.3 -y 0.25 -z 1.25 2>&1 | tail -1; then + echo " ✓ Gazebo model spawned" +else + echo " ⚠ Gazebo spawn failed (visual only — fault injection still works)" +fi + +# 2. Add to MoveIt planning scene (so planner cannot find a path) +echo "Adding collision wall to MoveIt planning scene..." +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 + +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'Planning scene updated: {result.success}' if result else 'Service call failed') +node.destroy_node() +rclpy.shutdown() +" + +echo "" +echo "✓ Planning failure injected!" +echo " An orange wall is now visible in Gazebo and registered in MoveIt planning scene." +echo "" +echo "Expected faults (via manipulation_monitor → FaultManager):" +echo " - MOTION_PLANNING_FAILED: MoveGroup goal ABORTED — no collision-free path" +echo "" +echo "Restore with: /root/demo_ws/scripts/restore-normal.sh" 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..463fbf8 --- /dev/null +++ b/demos/moveit_pick_place/scripts/manipulation_monitor.py @@ -0,0 +1,351 @@ +#!/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 + +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 + # In MoveIt 2 Jazzy, MoveGroupMoveAction serves at /move_action + self.move_group_status_sub = self.create_subscription( + GoalStatusArray, + '/move_action/_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 name, position in zip(msg.name, msg.position): + if name not in PANDA_JOINT_LIMITS: + continue + 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..0aa63c0 --- /dev/null +++ b/demos/moveit_pick_place/scripts/pick_place_loop.py @@ -0,0 +1,200 @@ +#!/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 + 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...") + 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 + if self.goal_in_flight: + return # Skip — previous goal still executing + + 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})...") + 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)) + + 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") + self.goal_in_flight = False + 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 + + self.goal_in_flight = False + + 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: + # Allow clean shutdown on Ctrl+C without printing a traceback. + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/demos/moveit_pick_place/scripts/restore-normal.sh b/demos/moveit_pick_place/scripts/restore-normal.sh new file mode 100755 index 0000000..82abac1 --- /dev/null +++ b/demos/moveit_pick_place/scripts/restore-normal.sh @@ -0,0 +1,82 @@ +#!/bin/bash +# Restore Normal Operation - Remove all injected faults +# Runs INSIDE the Docker container. + +set -e + +# shellcheck source=/dev/null +source /opt/ros/jazzy/setup.bash +# shellcheck source=/dev/null +source /root/demo_ws/install/setup.bash + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "🔄 Restoring NORMAL operation..." +echo "" + +# 1. Remove injected Gazebo models (visual objects) +echo "Removing injected Gazebo models..." +for MODEL_NAME in surprise_obstacle injected_wall; do + if gz service -s /world/panda_factory/remove \ + --reqtype gz.msgs.Entity \ + --reptype gz.msgs.Boolean \ + --timeout 3000 \ + --req "name: \"${MODEL_NAME}\" type: MODEL" 2>/dev/null; then + echo " ${MODEL_NAME}: removed" + else + echo " ${MODEL_NAME}: not found (ok)" + fi +done + +# 2. Remove injected collision objects from MoveIt planning scene +echo "Removing injected collision objects from planning scene..." +python3 -c " +import rclpy +from rclpy.node import Node +from moveit_msgs.srv import ApplyPlanningScene +from moveit_msgs.msg import PlanningScene, CollisionObject + +rclpy.init() +node = Node('restore_helper') + +client = node.create_client(ApplyPlanningScene, '/apply_planning_scene') +client.wait_for_service(timeout_sec=5.0) + +scene = PlanningScene() +scene.is_diff = True + +for obj_id in ['injected_wall', 'surprise_obstacle']: + obj = CollisionObject() + obj.id = obj_id + obj.operation = CollisionObject.REMOVE + scene.world.collision_objects.append(obj) + +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'Planning scene cleaned: {result.success}' if result else 'Service call failed') +node.destroy_node() +rclpy.shutdown() +" + +# 3. Wait for operations to stabilize, then clear faults +echo "Waiting for operations to stabilize..." +sleep 10 +echo "Clearing all faults..." +curl -sf -X DELETE "${API_BASE}/faults" > /dev/null 2>&1 || true +# Wait for any straggling reports to land, then clear again +sleep 5 +curl -sf -X DELETE "${API_BASE}/faults" > /dev/null 2>&1 || true + +echo "" +echo "✓ Normal operation restored!" +echo "" +if command -v jq >/dev/null 2>&1; then + echo "Current fault status:" + curl -sf "${API_BASE}/faults" 2>/dev/null | jq '.items | length' | xargs -I {} echo " Active faults: {}" || echo " (could not query faults)" +fi +echo "" +echo "Robot is ready for normal operation." 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!" 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 + + + + + + +