Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 0 additions & 3 deletions demos/moveit_pick_place/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,6 @@ install(DIRECTORY worlds/
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}
)

Expand Down
14 changes: 5 additions & 9 deletions demos/moveit_pick_place/Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ RUN apt-get update && apt-get install -y \
ros-jazzy-rosbag2-storage-mcap \
ros-jazzy-foxglove-bridge \
libsystemd-dev \
sqlite3 libsqlite3-dev git curl \
sqlite3 libsqlite3-dev git curl jq \
&& rm -rf /var/lib/apt/lists/*

# Create persistent directories for fault storage and rosbag recordings
Expand Down Expand Up @@ -63,6 +63,10 @@ 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/

# TODO(#49): Move to manifest-defined scripts once ros2_medkit#303 lands
COPY container_scripts/ /var/lib/ros2_medkit/scripts/
RUN find /var/lib/ros2_medkit/scripts -name "*.bash" -exec chmod +x {} \;

# 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
Expand All @@ -78,14 +82,6 @@ RUN bash -c "source /opt/ros/jazzy/setup.bash && \
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"]
75 changes: 60 additions & 15 deletions demos/moveit_pick_place/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ This demo demonstrates:
## Prerequisites

- Docker and docker-compose
- `curl` and `jq` installed on the host (required for host-side scripts)
- 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
Expand Down Expand Up @@ -239,22 +240,53 @@ curl http://localhost:8080/api/v1/apps/panda-arm-controller/configurations/gains
# 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}}'
-d '{"value": 0.5}'
```

## Fault Injection Scenarios
## Scripts API

The gateway exposes a Scripts API for the `moveit-planning` component. All fault injection and diagnostic scripts are available via REST without `docker exec`.

### Available Scripts

| Script ID | Name | Description |
|-----------|------|-------------|
| `inject-collision` | Inject Collision | Spawn a surprise obstacle in the robot workspace (Gazebo + MoveIt planning scene) |
| `inject-planning-failure` | Inject Planning Failure | Add collision wall blocking the pick-place path (Gazebo + MoveIt planning scene) |
| `restore-normal` | Restore Normal | Remove all injected obstacles and clear faults |
| `arm-self-test` | Arm Self-Test | Check joint states via REST API, verify values are reasonable |
| `planning-benchmark` | Planning Benchmark | Verify MoveIt planning is functional by checking key nodes and operations |

### List Available Scripts

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`.
```bash
curl http://localhost:8080/api/v1/components/moveit-planning/scripts | jq
```

You can also run them directly inside the container:
### Execute a Script via REST

```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
# Start execution
curl -X POST http://localhost:8080/api/v1/components/moveit-planning/scripts/inject-collision/executions \
-H "Content-Type: application/json" \
-d '{"execution_type": "now"}' | jq

# Poll status (use execution ID from above response)
curl http://localhost:8080/api/v1/components/moveit-planning/scripts/inject-collision/executions/<exec_id> | jq
```

> **Future:** When SOVD Scripts endpoints are available, these will be callable via `curl` against the gateway REST API.
### Override Gateway URL

```bash
# Point scripts at a non-default gateway
GATEWAY_URL=http://192.168.1.10:8080 ./inject-collision.sh
```

The host-side wrapper scripts (`./inject-collision.sh`, etc.) call the Scripts API automatically - no `docker exec` needed. Prerequisites: `curl` and `jq` must be installed on the host.

## Fault Injection Scenarios

The fault injection scripts run inside the container via the Scripts API. The host-side `./inject-*.sh` and `./restore-normal.sh` wrappers call the gateway REST endpoint - no `docker exec` required.

### 1. Planning Failure

Expand Down Expand Up @@ -319,17 +351,30 @@ Connect it to the gateway at `http://localhost:8080` to browse:
| `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 |
| `inject-planning-failure.sh` | Scripts API wrapper — inject planning failure |
| `inject-collision.sh` | Scripts API wrapper — inject collision obstacle |
| `restore-normal.sh` | Scripts API wrapper — restore normal operation |
| `arm-self-test.sh` | Scripts API wrapper — run arm self-test |
| `planning-benchmark.sh` | Scripts API wrapper — run planning benchmark |

Scripts API wrappers require `curl` and `jq` on the host and call the gateway REST endpoint directly - no `docker exec` needed.

### In-container scripts (auto-discovery via Scripts API)

Container scripts are stored under `/var/lib/ros2_medkit/scripts/moveit-planning/` and exposed via the gateway Scripts API.

| Script ID | Description |
|-----------|-------------|
| `inject-collision` | Spawn visible sphere + MoveIt collision object |
| `inject-planning-failure` | Spawn visible wall + MoveIt collision object |
| `restore-normal` | Remove Gazebo models + MoveIt objects, clear faults |
| `arm-self-test` | Check joint states via REST API |
| `planning-benchmark` | Verify MoveIt planning is functional |

### In-container (baked into Docker image, on `PATH`)
### ROS 2 runtime scripts (baked into colcon install)

| 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 |

Expand Down
8 changes: 8 additions & 0 deletions demos/moveit_pick_place/arm-self-test.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
#!/bin/bash
# Arm self-test via Scripts API
set -eu
SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)"
# shellcheck disable=SC1091
source "${SCRIPT_DIR}/../../lib/scripts-api.sh"

execute_script "components" "moveit-planning" "arm-self-test" "Arm self-test"
8 changes: 8 additions & 0 deletions demos/moveit_pick_place/config/medkit_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,14 @@ diagnostics:
# Plugin configuration (set by launch file when .so paths are resolved)
plugins: [""]

# Scripts configuration (filesystem auto-discovery)
# TODO(#49): Migrate to manifest-defined scripts once ros2_medkit#303 lands
scripts:
scripts_dir: "/var/lib/ros2_medkit/scripts"
allow_uploads: false
max_concurrent_executions: 3
default_timeout_sec: 60

# Fault Manager configuration (runs in root namespace)
fault_manager:
ros__parameters:
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
{
"name": "Arm Self-Test",
"description": "Check joint states via REST API, verify values are reasonable",
"format": "bash"
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
#!/bin/bash
# Arm self-test - verify joint states are within expected limits
set -eu
GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}"
API_BASE="${GATEWAY_URL}/api/v1"

echo "Running arm self-test..."
echo "Checking joint state broadcaster..."
if ! curl -sf "${API_BASE}/apps/joint-state-broadcaster" > /dev/null 2>&1; then
echo "FAIL: joint-state-broadcaster not responding"
exit 1
fi
echo "OK: joint-state-broadcaster responding"

echo "Checking move-group..."
if ! curl -sf "${API_BASE}/apps/move-group" > /dev/null 2>&1; then
echo "FAIL: move-group not responding"
exit 1
fi
echo "OK: move-group responding"

echo "Checking fault status..."
FAULT_COUNT=$(curl -sf "${API_BASE}/faults" | jq '.items | length' 2>/dev/null || echo "?")
echo "Active faults: $FAULT_COUNT"

echo "Arm self-test passed"
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
{
"name": "Inject Collision",
"description": "Spawn a surprise obstacle in the robot workspace (Gazebo + MoveIt planning scene)",
"format": "bash"
}
Original file line number Diff line number Diff line change
Expand Up @@ -3,20 +3,20 @@
# Adds the object to both Gazebo (visible) and MoveIt planning scene (causes faults)
# Runs INSIDE the Docker container.

set -e
set -eu

# 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 "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)
# 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'
<?xml version="1.0" ?>
Expand All @@ -41,9 +41,9 @@ 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"
echo " Gazebo model spawned"
else
echo " Gazebo spawn failed (visual only fault injection still works)"
echo " Gazebo spawn failed (visual only - fault injection still works)"
fi

# 2. Add to MoveIt planning scene (so planner detects the collision)
Expand Down Expand Up @@ -91,10 +91,10 @@ rclpy.shutdown()
"

echo ""
echo "Collision fault injected!"
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 "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"
echo "Restore with: ./restore-normal.sh (or via Scripts API)"
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
{
"name": "Inject Planning Failure",
"description": "Add collision wall blocking the pick-place path (Gazebo + MoveIt planning scene)",
"format": "bash"
}
Original file line number Diff line number Diff line change
Expand Up @@ -3,20 +3,20 @@
# Adds the wall to both Gazebo (visible) and MoveIt planning scene (causes faults)
# Runs INSIDE the Docker container.

set -e
set -eu

# 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 "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 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'
Expand All @@ -42,9 +42,9 @@ 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"
echo " Gazebo model spawned"
else
echo " Gazebo spawn failed (visual only fault injection still works)"
echo " Gazebo spawn failed (visual only - fault injection still works)"
fi

# 2. Add to MoveIt planning scene (so planner cannot find a path)
Expand Down Expand Up @@ -92,10 +92,10 @@ rclpy.shutdown()
"

echo ""
echo "Planning failure injected!"
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 "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"
echo "Restore with: ./restore-normal.sh (or via Scripts API)"
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
{
"name": "Planning Benchmark",
"description": "Verify MoveIt planning is functional by checking key nodes and operations",
"format": "bash"
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
#!/bin/bash
# Planning benchmark - verify MoveIt planning is functional
set -eu
GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}"
API_BASE="${GATEWAY_URL}/api/v1"

echo "Running planning benchmark..."
echo "Checking move-group operations..."
if ! curl -sf "${API_BASE}/apps/move-group/operations" > /dev/null 2>&1; then
echo "FAIL: Cannot list move-group operations"
exit 1
fi
echo "OK: move-group operations available"

echo "Checking pick-place-node..."
if ! curl -sf "${API_BASE}/apps/pick-place-node" > /dev/null 2>&1; then
echo "FAIL: pick-place-node not responding"
exit 1
fi
echo "OK: pick-place-node responding"

echo "Checking manipulation monitor..."
if ! curl -sf "${API_BASE}/apps/manipulation-monitor" > /dev/null 2>&1; then
echo "FAIL: manipulation-monitor not responding"
exit 1
fi
echo "OK: manipulation-monitor responding"

echo "Planning benchmark passed"
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
{
"name": "Restore Normal",
"description": "Remove all injected obstacles from Gazebo and MoveIt planning scene, clear faults",
"format": "bash"
}
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
# Restore Normal Operation - Remove all injected faults
# Runs INSIDE the Docker container.

set -e
set -eu

# shellcheck source=/dev/null
source /opt/ros/jazzy/setup.bash
Expand All @@ -12,7 +12,7 @@ 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 "Restoring NORMAL operation..."
echo ""

# 1. Remove injected Gazebo models (visual objects)
Expand Down Expand Up @@ -72,7 +72,7 @@ sleep 5
curl -sf -X DELETE "${API_BASE}/faults" > /dev/null 2>&1 || true

echo ""
echo "Normal operation restored!"
echo "Normal operation restored!"
echo ""
if command -v jq >/dev/null 2>&1; then
echo "Current fault status:"
Expand Down
Loading
Loading