diff --git a/docs/model_import_runbook.md b/docs/model_import_runbook.md
index 37770c7..9f3fe1c 100644
--- a/docs/model_import_runbook.md
+++ b/docs/model_import_runbook.md
@@ -1,26 +1,220 @@
-# Waybionic Model Import Runbook
+# Waybionic Model Import & Validation Runbook
-This guide is for importing and testing real URDF and mechanical mesh exports (STLs) without breaking the clean ROS 2 foundation or editing Python launch files.
+How to import, run, and validate a robot model in this workspace without editing
+the launch files. Run every command from the **workspace root** — the folder
+containing `waybionic_bringup/` and `waybionic_description/`.
-## 1. Where to put the files
-- **Meshes (.stl, .dae):** Place all 3D mesh files into `waybionic_description/meshes/`.
-- **URDF/Xacro (.urdf, .xacro):** Place your exported robot description file into `waybionic_description/urdf/`.
+## Models in this package
-*Important: Inside the URDF, ensure the mesh paths use the standard ROS package syntax. Example:*
-``
+Both live in `waybionic_description/urdf/`:
-## 2. Rebuild the Workspace
-Any time new files are added, rebuild the foundation so CMake can install them to the ROS 2 share directory.
-From the root of your workspace (`~/waybionic_ws`):
-```
-colcon build --packages-select waybionic_description
+| File | Role | Meshes |
+|------|------|--------|
+| `full_arm_mar24.urdf` | **Default.** The real arm — a 5-link serial chain `base_link → shoulder → elbow → forearm → wrist` with articulated (revolute/continuous) joints. | 5 STLs in `meshes/` |
+| `waybionic_placeholder.urdf` | Fallback / test asset. A primitive box + cylinder on one revolute joint. | **None** — pure URDF primitives, always loads |
+
+The real arm's meshes are the only files kept in `waybionic_description/meshes/`:
+`base_link.STL`, `shoulder.STL`, `elbow.STL`, `forearm.STL`, `wrist.STL`.
+
+## 1. Import files
+
+- **URDF/Xacro** (`.urdf`, `.xacro`) → `waybionic_description/urdf/`
+- **Meshes** (`.stl`, `.dae`) → `waybionic_description/meshes/`
+
+Inside the URDF, reference meshes with the ROS package path, e.g.
+``.
+
+## 2. Build
+
+These are `ament_cmake` packages that *copy* files into `install/` at build
+time, so **rebuild after any change** to a URDF, mesh, or launch file — edits in
+the source tree are invisible to `ros2 launch` until you do.
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --packages-select waybionic_description waybionic_bringup
source install/setup.bash
```
-## 3. Test the model
-Don't edit `display.launch.py` to test the model. Instead, pass the path to the new URDF using the `model:=` argument.
-From the root of your workspace, run:
+If packages were renamed/removed (e.g. after a merge), do a clean rebuild so
+stale copies don't linger: `rm -rf build install log && colcon build`.
+
+## 3. Run
+
+`display.launch.py` defaults to the real arm and opens RViz (pre-configured with
+`waybionic.rviz`) plus the Joint State Publisher GUI for driving the joints.
+
+```bash
+# Real arm (default)
+ros2 launch waybionic_bringup display.launch.py
+
+# Placeholder (fallback / test) — needs no meshes
+ros2 launch waybionic_bringup display.launch.py \
+ model:=$(ros2 pkg prefix waybionic_description --share)/urdf/waybionic_placeholder.urdf
+
+# Any other model — no need to edit the launch file
+ros2 launch waybionic_bringup display.launch.py \
+ model:=$(ros2 pkg prefix waybionic_description --share)/urdf/YOUR_FILE.urdf
```
-ros2 launch waybionic_bringup display.launch.py model:=$(ros2 pkg prefix waybionic_description --share)/urdf/YOUR_NEW_FILE.urdf
+
+The `model` argument accepts a plain `.urdf` (read directly) or a `.xacro`
+(expanded via `xacro`). If a model doesn't appear, errors print in the terminal.
+
+## 4. Test & validate
+
+Run these from the workspace root after building. Steps 4.1–4.4 are automated
+(no GUI); 4.5 is the manual RViz/joint check. Expected results below are from the
+last verified run.
+
+### 4.1 Structural check — `check_urdf`
+
+Needs `liburdfdom-tools` (`sudo apt install liburdfdom-tools`).
+
+```bash
+check_urdf install/waybionic_description/share/waybionic_description/urdf/full_arm_mar24.urdf
+check_urdf install/waybionic_description/share/waybionic_description/urdf/waybionic_placeholder.urdf
+```
+
+**Expect:** `Successfully Parsed XML` and, for the arm, **`root Link: world`** with
+the chain `world → base_link → shoulder → elbow → forearm → wrist`. The `world`
+root is what stops KDL from ignoring `base_link`'s inertia — if the root prints as
+`base_link`, the massless `world` root link is missing.
+
+### 4.2 Build + unit tests
+
+```bash
+colcon build # or select the description, bringup, MoveIt, and RViz packages
+colcon test
+colcon test-result --all
```
-If parsed correctly, RViz will automatically open and display the model. If there are issues, errors will print in the terminal.
\ No newline at end of file
+
+**Expect:** build finishes with no errors; `colcon test-result` ends with
+`0 errors, 0 failures` (last run: **52 tests, 0 failures** across the workspace).
+
+### 4.3 KDL root-inertia check (headless)
+
+Confirms the "root link has inertia — KDL ignores it" warning is gone.
+
+```bash
+if ! rsp_prefix="$(ros2 pkg prefix robot_state_publisher 2>&1)"; then
+ printf '%s\n' "$rsp_prefix"
+ echo "ERROR — robot_state_publisher package is unavailable"
+ exit 1
+fi
+rsp_executable="$rsp_prefix/lib/robot_state_publisher/robot_state_publisher"
+if [ ! -x "$rsp_executable" ]; then
+ echo "ERROR — robot_state_publisher executable is missing"
+ exit 1
+fi
+
+kdl_log="$(mktemp)"
+"$rsp_executable" \
+ install/waybionic_description/share/waybionic_description/urdf/full_arm_mar24.urdf \
+ >"$kdl_log" 2>&1 &
+kdl_pid=$!
+sleep 5
+if kill -0 "$kdl_pid" 2>/dev/null; then
+ kdl_was_running=true
+ kill -INT "$kdl_pid" 2>/dev/null || kdl_was_running=false
+else
+ kdl_was_running=false
+fi
+if wait "$kdl_pid" 2>/dev/null; then
+ kdl_status=0
+else
+ kdl_status=$?
+fi
+kdl_output="$(cat "$kdl_log")"
+rm -f "$kdl_log"
+printf '%s\n' "$kdl_output"
+if [ "$kdl_was_running" != true ] \
+ || { [ "$kdl_status" -ne 0 ] && [ "$kdl_status" -ne 130 ]; }; then
+ echo "ERROR — robot_state_publisher exited unexpectedly (status $kdl_status)"
+ exit 1
+elif ! printf '%s\n' "$kdl_output" | grep -q 'Robot initialized'; then
+ echo "ERROR — robot_state_publisher did not initialize within 5 seconds"
+ exit 1
+elif printf '%s\n' "$kdl_output" | grep -qiE 'root link.*inertia|KDL.*inertia'; then
+ echo "ERROR — KDL root-inertia warning found"
+ exit 1
+else
+ echo "OK — no KDL root-inertia warning"
+fi
+```
+
+**Expect:** `OK — no KDL root-inertia warning` and `Robot initialized`.
+
+### 4.4 Part & mesh audit (simulation running in another terminal)
+
+Don't count parts by eye — they range from a ~30 cm housing to a few-mm screw.
+
+```bash
+if ! robot_description="$(
+ ros2 param get /robot_state_publisher robot_description 2>&1
+)"; then
+ printf '%s\n' "$robot_description"
+ echo "ERROR — could not read the live robot_description parameter"
+ exit 1
+fi
+
+# Robot links in the LIVE model loaded by RViz, excluding only the world frame
+printf '%s\n' "$robot_description" \
+ | grep -oE '`) or exceeds its true range **by exact joint name**.
+
+---
+
+*Model provenance:* `full_arm_mar24.urdf` was exported from the
+`full-arm-mar24.SLDASM` SolidWorks assembly via the `sw2urdf` exporter. Joint
+axes and limits are authored in the URDF (they can't be recovered from STLs).
diff --git a/docs/moveit_config.md b/docs/moveit_config.md
new file mode 100644
index 0000000..801ad04
--- /dev/null
+++ b/docs/moveit_config.md
@@ -0,0 +1,25 @@
+# WayBionic MoveIt demo
+
+The canonical setup and operating guide lives with the package:
+[`waybionic_moveit_config/README.md`](../waybionic_moveit_config/README.md).
+
+Use the MoveIt launch when you need planning, position-only IK, collision
+checking, or mock trajectory execution:
+
+```bash
+ros2 launch waybionic_moveit_config demo.launch.py
+```
+
+The lighter `waybionic_bringup/display.launch.py` only displays the robot and
+jogs individual joints. Do not run both launches together because they publish
+competing joint states.
+
+The MoveIt demo supplies the semantic robot description, mock ros2_control
+hardware, controllers, planner, RViz MotionPlanning UI, and an XYZ IK replay
+service. Click **Replay XYZ Demo** in RViz, or start the launch with
+`auto_demo:=true`. The arm has four degrees of freedom, so its IK intentionally
+solves position (XYZ) rather than an arbitrary six-degree-of-freedom pose.
+
+Visuals use the imported STL files. Collision checking uses conservative boxes
+and cylinders so both macOS and Ubuntu avoid loading high-resolution meshes into
+FCL. See the package README for limitations and test commands.
diff --git a/robostack.yaml b/robostack.yaml
index fac5030..692ea85 100644
--- a/robostack.yaml
+++ b/robostack.yaml
@@ -10,10 +10,14 @@ dependencies:
- ros-jazzy-rviz2
- ros-jazzy-xacro
- ros-jazzy-joint-state-publisher-gui
+ - ros-jazzy-moveit
+ - ros-jazzy-ros2-control
+ - ros-jazzy-ros2-controllers
- colcon-common-extensions
- compilers
- cmake
- pkg-config
- make
- ninja
- - pytest <9
\ No newline at end of file
+ - pytest <9
+ - setuptools <72
diff --git a/waybionic_bringup/launch/display.launch.py b/waybionic_bringup/launch/display.launch.py
index ceab624..3908ed3 100644
--- a/waybionic_bringup/launch/display.launch.py
+++ b/waybionic_bringup/launch/display.launch.py
@@ -39,7 +39,7 @@ def generate_launch_description():
waybionic_bringup_dir = get_package_share_directory('waybionic_bringup')
default_model_path = os.path.join(
- waybionic_desc_dir, 'urdf', 'waybionic_placeholder.urdf'
+ waybionic_desc_dir, 'urdf', 'full_arm_mar24.urdf'
)
default_rviz_config_path = os.path.join(
waybionic_bringup_dir, 'rviz', 'waybionic.rviz'
diff --git a/waybionic_description/CMakeLists.txt b/waybionic_description/CMakeLists.txt
index 0efa2ab..5902100 100644
--- a/waybionic_description/CMakeLists.txt
+++ b/waybionic_description/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(ament_cmake REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
+ find_package(ament_cmake_pytest REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
@@ -21,6 +22,12 @@ if(BUILD_TESTING)
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
+
+ ament_add_pytest_test(
+ test_full_arm_model
+ test/test_full_arm_model.py
+ TIMEOUT 60
+ )
endif()
install(DIRECTORY urdf meshes
diff --git a/waybionic_description/meshes/base_link.STL b/waybionic_description/meshes/base_link.STL
new file mode 100644
index 0000000..89431fe
Binary files /dev/null and b/waybionic_description/meshes/base_link.STL differ
diff --git a/waybionic_description/meshes/elbow.STL b/waybionic_description/meshes/elbow.STL
new file mode 100644
index 0000000..dda12b4
Binary files /dev/null and b/waybionic_description/meshes/elbow.STL differ
diff --git a/waybionic_description/meshes/forearm.STL b/waybionic_description/meshes/forearm.STL
new file mode 100644
index 0000000..55d605f
Binary files /dev/null and b/waybionic_description/meshes/forearm.STL differ
diff --git a/waybionic_description/meshes/shoulder.STL b/waybionic_description/meshes/shoulder.STL
new file mode 100644
index 0000000..6af81f0
Binary files /dev/null and b/waybionic_description/meshes/shoulder.STL differ
diff --git a/waybionic_description/meshes/wrist.STL b/waybionic_description/meshes/wrist.STL
new file mode 100644
index 0000000..23ebd22
Binary files /dev/null and b/waybionic_description/meshes/wrist.STL differ
diff --git a/waybionic_description/package.xml b/waybionic_description/package.xml
index 6ca6250..5d06400 100644
--- a/waybionic_description/package.xml
+++ b/waybionic_description/package.xml
@@ -14,6 +14,7 @@
ament_lint_auto
ament_lint_common
+ ament_cmake_pytest
ament_cmake
diff --git a/waybionic_description/test/test_full_arm_model.py b/waybionic_description/test/test_full_arm_model.py
new file mode 100644
index 0000000..67d5bdc
--- /dev/null
+++ b/waybionic_description/test/test_full_arm_model.py
@@ -0,0 +1,55 @@
+"""Regression checks for the imported full-arm model."""
+
+from pathlib import Path
+import struct
+import xml.etree.ElementTree as ET
+
+
+PACKAGE_ROOT = Path(__file__).resolve().parent.parent
+
+
+def _binary_stl_bounds(path):
+ """Return the axis-aligned bounds of a binary STL."""
+ data = path.read_bytes()
+ triangle_count = struct.unpack_from('
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/waybionic_moveit_config/CMakeLists.txt b/waybionic_moveit_config/CMakeLists.txt
new file mode 100644
index 0000000..588acbc
--- /dev/null
+++ b/waybionic_moveit_config/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(waybionic_moveit_config)
+
+find_package(ament_cmake REQUIRED)
+
+if(BUILD_TESTING)
+ find_package(ament_cmake_ros REQUIRED)
+ find_package(launch_testing_ament_cmake REQUIRED)
+
+ add_launch_test(
+ test/test_ik_demo_launch.py
+ TIMEOUT 120
+ RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py"
+ )
+ add_launch_test(
+ test/test_ik_demo_timeout_launch.py
+ TIMEOUT 20
+ RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py"
+ )
+endif()
+
+install(DIRECTORY config launch rviz srdf urdf
+ DESTINATION share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+ scripts/ik_xyz_demo.py
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/waybionic_moveit_config/README.md b/waybionic_moveit_config/README.md
new file mode 100644
index 0000000..151cf87
--- /dev/null
+++ b/waybionic_moveit_config/README.md
@@ -0,0 +1,124 @@
+# waybionic_moveit_config
+
+MoveIt 2 configuration for the WayBionic arm (`full_arm_mar24.urdf`).
+
+## Quickstart
+
+Clone the repository once and use that directory as the colcon workspace:
+
+```bash
+git clone https://github.com/Waybionic/waybionic_ground_station.git
+cd waybionic_ground_station
+```
+
+### Ubuntu 24.04 (ROS 2 Jazzy)
+
+From the repository root:
+
+```bash
+source /opt/ros/jazzy/setup.bash
+rosdep install --from-paths . --ignore-src -r -y
+colcon build --symlink-install
+source install/setup.bash
+ros2 launch waybionic_moveit_config demo.launch.py
+```
+
+### macOS (Apple Silicon)
+
+Use the repository helper so the RoboStack environment and Bash workspace
+overlay are applied correctly. Run `./scripts/macos.sh setup` once to create the
+environment, then use:
+
+```bash
+./scripts/macos.sh build
+./scripts/macos.sh run ros2 launch waybionic_moveit_config demo.launch.py
+```
+
+RViz opens with the MotionPlanning display already configured for the `arm`
+group. Use the **Planning** tab: pick a start/goal state (or a named pose),
+press **Plan**, then **Execute**.
+
+To run the automatic Cartesian demonstration at startup, append
+`auto_demo:=true` to the launch command. For example, on Ubuntu:
+
+```bash
+ros2 launch waybionic_moveit_config demo.launch.py auto_demo:=true
+```
+
+The arm first moves to its ready pose, then uses MoveIt's `/compute_ik`
+service to move the wrist along X, Y, and Z. RViz shows a red X axis, green Y
+axis, blue Z axis, and a yellow target. Click **Replay XYZ Demo** in the IK Demo
+panel to run it again. For manual IK, drag a colored goal-state arrow and click
+**Plan & Execute**. MotionPlanning uses 50% of the model's velocity and
+acceleration limits by default; those sliders can still be adjusted in RViz.
+
+Headless (no RViz), useful for testing:
+
+```bash
+ros2 launch waybionic_moveit_config demo.launch.py use_rviz:=false
+```
+
+On macOS, prefix the Ubuntu `ros2 launch` examples above with
+`./scripts/macos.sh run`.
+
+## Important: this arm has 4 DOF
+
+`joint1`, `joint2`, `joint4` are revolute and `joint3` is continuous — four
+degrees of freedom total. **A 4-DOF arm cannot reach an arbitrary 6-DOF pose.**
+
+Consequences you need to know about:
+
+- IK is configured **position-only** (`position_only_ik: true` in
+ `config/kinematics.yaml`). Goals are matched on XYZ; end-effector orientation
+ is whatever the arm happens to produce.
+- The RViz config sets `MoveIt_Allow_Approximate_IK: true`. Without it, dragging
+ the interactive marker almost never finds a solution.
+- Planning in **joint space** (the Joints tab, or named poses) is fully reliable
+ and is the recommended workflow for this arm.
+
+## What runs
+
+| Component | Purpose |
+|---|---|
+| `robot_state_publisher` | Publishes TF from the URDF |
+| `ros2_control_node` | Mock hardware (`mock_components/GenericSystem`) |
+| `joint_state_broadcaster` | Publishes `/joint_states` |
+| `arm_controller` | `JointTrajectoryController`, executes planned paths |
+| `move_group` | Planning, IK, collision checking |
+| `ik_xyz_demo` | Runs and replays the Cartesian XYZ demonstration |
+| `rviz2` | MotionPlanning UI |
+
+No physical hardware is needed. The mock system echoes commands back as state,
+so **Execute** animates the arm in RViz.
+
+## Layout
+
+```text
+srdf/waybionic.srdf # Planning group, named poses, collision matrix
+config/kinematics.yaml # KDL, position-only IK
+config/joint_limits.yaml # Velocity/acceleration limits
+config/ompl_planning.yaml # Focused OMPL RRTConnect configuration
+config/moveit_controllers.yaml # move_group -> ros2_control handoff
+config/ros2_controllers.yaml # controller_manager + JTC
+urdf/waybionic_moveit.urdf.xacro # Includes base URDF, adds ros2_control
+launch/demo.launch.py # Brings up everything
+rviz/moveit.rviz # MotionPlanning preconfigured for group "arm"
+```
+
+The xacro in `urdf/` includes the shared robot description and layers
+`` on top. High-resolution STL files are visual-only; lightweight
+boxes and cylinders provide portable, fast collision checking.
+
+## Known limitations
+
+- **The collision matrix only disables adjacent link pairs.** It was written by
+ hand, not sampled by the MoveIt Setup Assistant. The primitive collision
+ envelopes test clean at the demo poses, but production hardware should still
+ regenerate the matrix across the complete workspace with:
+ ```bash
+ ros2 launch moveit_setup_assistant setup_assistant.launch.py
+ ```
+ Load `urdf/waybionic_moveit.urdf.xacro`, then use the Self-Collisions pane.
+- **No end effector is defined** — there is no gripper in the URDF.
+- `No 3D sensor plugin(s) defined for octomap updates` is logged as an ERROR at
+ startup. It is harmless: there is no depth camera in this setup.
diff --git a/waybionic_moveit_config/config/joint_limits.yaml b/waybionic_moveit_config/config/joint_limits.yaml
new file mode 100644
index 0000000..3128f28
--- /dev/null
+++ b/waybionic_moveit_config/config/joint_limits.yaml
@@ -0,0 +1,27 @@
+# Velocity/effort values mirror the tags in full_arm_mar24.urdf.
+# The URDF declares no acceleration limits, so conservative values are set here
+# for time-optimal trajectory parameterization.
+default_velocity_scaling_factor: 0.5
+default_acceleration_scaling_factor: 0.5
+
+joint_limits:
+ joint1:
+ has_velocity_limits: true
+ max_velocity: 1.0
+ has_acceleration_limits: true
+ max_acceleration: 1.0
+ joint2:
+ has_velocity_limits: true
+ max_velocity: 1.0
+ has_acceleration_limits: true
+ max_acceleration: 1.0
+ joint3:
+ has_velocity_limits: true
+ max_velocity: 1.0
+ has_acceleration_limits: true
+ max_acceleration: 1.0
+ joint4:
+ has_velocity_limits: true
+ max_velocity: 1.0
+ has_acceleration_limits: true
+ max_acceleration: 1.0
diff --git a/waybionic_moveit_config/config/kinematics.yaml b/waybionic_moveit_config/config/kinematics.yaml
new file mode 100644
index 0000000..75eca4d
--- /dev/null
+++ b/waybionic_moveit_config/config/kinematics.yaml
@@ -0,0 +1,9 @@
+# The WayBionic arm has 4 DOF, so it cannot satisfy a full 6-DOF pose goal.
+# position_only_ik makes KDL solve for XYZ and ignore orientation, which is the
+# only way interactive-marker dragging converges on an arm of this size.
+arm:
+ kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+ kinematics_solver_attempts: 3
+ position_only_ik: true
diff --git a/waybionic_moveit_config/config/moveit_controllers.yaml b/waybionic_moveit_config/config/moveit_controllers.yaml
new file mode 100644
index 0000000..5e679a2
--- /dev/null
+++ b/waybionic_moveit_config/config/moveit_controllers.yaml
@@ -0,0 +1,16 @@
+# Tells move_group how to hand trajectories to ros2_control.
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+ controller_names:
+ - arm_controller
+
+ arm_controller:
+ type: FollowJointTrajectory
+ action_ns: follow_joint_trajectory
+ default: true
+ joints:
+ - joint1
+ - joint2
+ - joint3
+ - joint4
diff --git a/waybionic_moveit_config/config/ompl_planning.yaml b/waybionic_moveit_config/config/ompl_planning.yaml
new file mode 100644
index 0000000..ecaa329
--- /dev/null
+++ b/waybionic_moveit_config/config/ompl_planning.yaml
@@ -0,0 +1,23 @@
+planning_plugins:
+ - ompl_interface/OMPLPlanner
+request_adapters:
+ - default_planning_request_adapters/CheckStartStateBounds
+ - default_planning_request_adapters/CheckStartStateCollision
+ - default_planning_request_adapters/ResolveConstraintFrames
+ - default_planning_request_adapters/ValidateWorkspaceBounds
+response_adapters:
+ - default_planning_response_adapters/AddTimeOptimalParameterization
+ - default_planning_response_adapters/ValidateSolution
+
+planner_configs:
+ RRTConnectkConfigDefault:
+ type: geometric::RRTConnect
+ range: 0.0
+
+arm:
+ default_planner_config: RRTConnectkConfigDefault
+ planner_configs:
+ - RRTConnectkConfigDefault
+ longest_valid_segment_fraction: 0.01
+
+start_state_max_bounds_error: 0.1
diff --git a/waybionic_moveit_config/config/ros2_controllers.yaml b/waybionic_moveit_config/config/ros2_controllers.yaml
new file mode 100644
index 0000000..1646bd4
--- /dev/null
+++ b/waybionic_moveit_config/config/ros2_controllers.yaml
@@ -0,0 +1,26 @@
+# ros2_control controllers backing the mock hardware.
+controller_manager:
+ ros__parameters:
+ # 50 Hz is plenty for mock hardware and avoids "Overrun detected" spam on
+ # WSL2, which cannot grant the controller_manager realtime scheduling.
+ update_rate: 50
+
+ joint_state_broadcaster:
+ type: joint_state_broadcaster/JointStateBroadcaster
+
+ arm_controller:
+ type: joint_trajectory_controller/JointTrajectoryController
+
+arm_controller:
+ ros__parameters:
+ joints:
+ - joint1
+ - joint2
+ - joint3
+ - joint4
+ command_interfaces:
+ - position
+ state_interfaces:
+ - position
+ - velocity
+ allow_partial_joints_goal: false
diff --git a/waybionic_moveit_config/launch/demo.launch.py b/waybionic_moveit_config/launch/demo.launch.py
new file mode 100644
index 0000000..74bc929
--- /dev/null
+++ b/waybionic_moveit_config/launch/demo.launch.py
@@ -0,0 +1,210 @@
+"""Full MoveIt demo for the WayBionic arm.
+
+Brings up, in one shot:
+ robot_state_publisher, ros2_control (mock hardware), the joint_state_broadcaster
+ and arm_controller, move_group, and an RViz preloaded with MotionPlanning.
+
+No physical hardware is required.
+"""
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+import xacro
+import yaml
+
+PKG = "waybionic_moveit_config"
+
+
+def load_yaml(package_name, file_path):
+ absolute_path = os.path.join(get_package_share_directory(package_name), file_path)
+ with open(absolute_path, "r", encoding="utf-8") as handle:
+ return yaml.safe_load(handle)
+
+
+def load_robot_description():
+ """Expand the shared arm description, including primitive collisions."""
+ xacro_path = os.path.join(
+ get_package_share_directory(PKG),
+ "urdf",
+ "waybionic_moveit.urdf.xacro",
+ )
+ return xacro.process_file(xacro_path).toxml()
+
+
+def generate_launch_description():
+ use_rviz = LaunchConfiguration("use_rviz")
+ auto_demo = LaunchConfiguration("auto_demo")
+ rviz_config_file = LaunchConfiguration("rviz_config_file")
+
+ declared_arguments = [
+ DeclareLaunchArgument(
+ "use_rviz",
+ default_value="true",
+ choices=["true", "false"],
+ description="Start RViz with the MotionPlanning display",
+ ),
+ DeclareLaunchArgument(
+ "auto_demo",
+ default_value="false",
+ choices=["true", "false"],
+ description="Automatically demonstrate Cartesian X/Y/Z IK motion",
+ ),
+ DeclareLaunchArgument(
+ "rviz_config_file",
+ default_value=PathJoinSubstitution(
+ [FindPackageShare(PKG), "rviz", "moveit.rviz"]
+ ),
+ description="Full path to the RViz configuration file",
+ ),
+ ]
+
+ # --- robot_description (URDF + ros2_control) ---
+ robot_description_content = load_robot_description()
+ robot_description = {
+ "robot_description": ParameterValue(robot_description_content, value_type=str)
+ }
+
+ # --- robot_description_semantic (SRDF) ---
+ srdf_path = os.path.join(
+ get_package_share_directory(PKG), "srdf", "waybionic.srdf"
+ )
+ with open(srdf_path, "r", encoding="utf-8") as handle:
+ robot_description_semantic = {"robot_description_semantic": handle.read()}
+
+ robot_description_kinematics = {
+ "robot_description_kinematics": load_yaml(PKG, "config/kinematics.yaml")
+ }
+ joint_limits = {
+ "robot_description_planning": load_yaml(PKG, "config/joint_limits.yaml")
+ }
+
+ ompl_yaml = load_yaml(PKG, "config/ompl_planning.yaml")
+ planning_pipeline_config = {
+ "default_planning_pipeline": "ompl",
+ "planning_pipelines": ["ompl"],
+ "ompl": ompl_yaml,
+ }
+
+ moveit_controllers = load_yaml(PKG, "config/moveit_controllers.yaml")
+
+ trajectory_execution = {
+ "moveit_manage_controllers": True,
+ "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+ "trajectory_execution.allowed_goal_duration_margin": 0.5,
+ "trajectory_execution.allowed_start_tolerance": 0.01,
+ }
+
+ planning_scene_monitor_parameters = {
+ "publish_planning_scene": True,
+ "publish_geometry_updates": True,
+ "publish_state_updates": True,
+ "publish_transforms_updates": True,
+ # Makes move_group publish the SRDF on a topic, so RViz can pick it up
+ # even when a MotionPlanning display is added by hand after startup.
+ "publish_robot_description_semantic": True,
+ "publish_robot_description": True,
+ }
+
+ move_group_node = Node(
+ package="moveit_ros_move_group",
+ executable="move_group",
+ output="screen",
+ parameters=[
+ robot_description,
+ robot_description_semantic,
+ robot_description_kinematics,
+ joint_limits,
+ planning_pipeline_config,
+ trajectory_execution,
+ moveit_controllers,
+ planning_scene_monitor_parameters,
+ ],
+ )
+
+ robot_state_publisher_node = Node(
+ package="robot_state_publisher",
+ executable="robot_state_publisher",
+ output="both",
+ parameters=[robot_description],
+ )
+
+ ros2_controllers_path = os.path.join(
+ get_package_share_directory(PKG), "config", "ros2_controllers.yaml"
+ )
+ # On Jazzy the controller_manager picks robot_description up from the
+ # /robot_description topic published by robot_state_publisher. Passing it as
+ # a parameter as well makes it log a spurious "already loaded a urdf" warning.
+ ros2_control_node = Node(
+ package="controller_manager",
+ executable="ros2_control_node",
+ parameters=[ros2_controllers_path],
+ output="both",
+ )
+
+ joint_state_broadcaster_spawner = Node(
+ package="controller_manager",
+ executable="spawner",
+ arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
+ )
+
+ arm_controller_spawner = Node(
+ package="controller_manager",
+ executable="spawner",
+ arguments=["arm_controller", "-c", "/controller_manager"],
+ )
+
+ rviz_node = Node(
+ package="rviz2",
+ executable="rviz2",
+ name="rviz2",
+ output="log",
+ arguments=["-d", rviz_config_file],
+ condition=IfCondition(use_rviz),
+ parameters=[
+ robot_description,
+ robot_description_semantic,
+ robot_description_kinematics,
+ joint_limits,
+ planning_pipeline_config,
+ ],
+ )
+
+ ik_xyz_demo_node = Node(
+ package=PKG,
+ executable="ik_xyz_demo.py",
+ name="ik_xyz_demo",
+ output="screen",
+ parameters=[
+ {"run_on_start": ParameterValue(auto_demo, value_type=bool)},
+ ],
+ )
+
+ # Load arm_controller only once joint_state_broadcaster is up, so the
+ # controller_manager is guaranteed ready.
+ delayed_arm_controller = RegisterEventHandler(
+ OnProcessExit(
+ target_action=joint_state_broadcaster_spawner,
+ on_exit=[arm_controller_spawner],
+ )
+ )
+
+ return LaunchDescription(
+ declared_arguments
+ + [
+ robot_state_publisher_node,
+ ros2_control_node,
+ joint_state_broadcaster_spawner,
+ delayed_arm_controller,
+ move_group_node,
+ rviz_node,
+ ik_xyz_demo_node,
+ ]
+ )
diff --git a/waybionic_moveit_config/package.xml b/waybionic_moveit_config/package.xml
new file mode 100644
index 0000000..6a7686c
--- /dev/null
+++ b/waybionic_moveit_config/package.xml
@@ -0,0 +1,52 @@
+
+
+
+ waybionic_moveit_config
+ 0.0.0
+ MoveIt 2 configuration for the WayBionic 4-DOF arm.
+ Harold Kim
+ Apache-2.0
+
+ ament_cmake
+
+ waybionic_description
+
+ ament_index_python
+ launch
+ launch_ros
+ python3-yaml
+
+ moveit_ros_move_group
+ moveit_ros_visualization
+ moveit_planners_ompl
+ moveit_kinematics
+ moveit_simple_controller_manager
+ controller_manager
+ joint_state_broadcaster
+ joint_trajectory_controller
+ robot_state_publisher
+ hardware_interface
+
+ control_msgs
+ geometry_msgs
+ moveit_msgs
+ rclpy
+ sensor_msgs
+ std_srvs
+ tf2_ros
+ trajectory_msgs
+ visualization_msgs
+
+ rviz2
+ waybionic_rviz_plugins
+ xacro
+
+ action_msgs
+ ament_cmake_ros
+ controller_manager_msgs
+ launch_testing_ament_cmake
+
+
+ ament_cmake
+
+
diff --git a/waybionic_moveit_config/rviz/moveit.rviz b/waybionic_moveit_config/rviz/moveit.rviz
new file mode 100644
index 0000000..14a1714
--- /dev/null
+++ b/waybionic_moveit_config/rviz/moveit.rviz
@@ -0,0 +1,173 @@
+Panels:
+ - Class: waybionic_rviz_plugins/IkDemoPanel
+ Name: IK Demo
+ - Class: rviz_common/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /MotionPlanning1
+ Splitter Ratio: 0.5
+ Tree Height: 600
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 0.25
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.03
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 20
+ Reference Frame:
+ Value: true
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: IK XYZ Axes
+ Topic:
+ Depth: 5
+ Durability Policy: Transient Local
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /ik_demo/markers
+ Value: true
+ - Class: moveit_rviz_plugin/MotionPlanning
+ Enabled: true
+ Move Group Namespace: ""
+ MoveIt_Allow_Approximate_IK: true
+ MoveIt_Allow_External_Program: false
+ MoveIt_Allow_Replanning: false
+ MoveIt_Allow_Sensor_Positioning: false
+ MoveIt_Cartesian_Jump_Threshold: 0
+ MoveIt_Goal_Tolerance: 0
+ MoveIt_Planning_Attempts: 10
+ MoveIt_Planning_Time: 5
+ MoveIt_Use_Cartesian_Path: false
+ MoveIt_Use_Constraint_Aware_IK: false
+ Velocity_Scaling_Factor: 0.5
+ Acceleration_Scaling_Factor: 0.5
+ MoveIt_Workspace:
+ Center:
+ X: 0
+ Y: 0
+ Z: 0
+ Size:
+ X: 4
+ Y: 4
+ Z: 4
+ Name: MotionPlanning
+ Planned Path:
+ Color Enabled: false
+ Interrupt Display: false
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ Loop Animation: false
+ Robot Alpha: 0.5
+ Robot Color: 150; 50; 150
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Show Trail: false
+ State Display Time: 0.05 s
+ Trail Step Size: 1
+ Trajectory Topic: /display_planned_path
+ Use Sim Time: false
+ Planning Metrics:
+ Payload: 1
+ Show Joint Torques: false
+ Show Manipulability: false
+ Show Manipulability Index: false
+ Show Weight Limit: false
+ Planning Request:
+ Colliding Link Color: 255; 0; 0
+ Goal State Alpha: 1
+ Goal State Color: 250; 128; 0
+ Interactive Marker Size: 0.15
+ Joint Violation Color: 255; 0; 255
+ Planning Group: arm
+ Query Goal State: true
+ Query Start State: false
+ Show Workspace: false
+ Start State Alpha: 1
+ Start State Color: 0; 255; 0
+ Planning Scene Topic: /monitored_planning_scene
+ Robot Description: robot_description
+ Scene Geometry:
+ Scene Alpha: 0.9
+ Scene Color: 50; 230; 50
+ Scene Display Time: 0.01
+ Show Scene Geometry: true
+ Voxel Coloring: Z-Axis
+ Voxel Rendering: Occupied Voxels
+ Scene Robot:
+ Attached Body Color: 150; 50; 150
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ Robot Alpha: 1
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Value: true
+ - Class: rviz_default_plugins/TF
+ Enabled: false
+ Name: TF
+ Value: false
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: world
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 2.2
+ Focal Point:
+ X: 0
+ Y: 0.15
+ Z: 0.65
+ Name: Current View
+ Near Clip Distance: 0.01
+ Pitch: 0.4
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.9
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1000
+ Hide Left Dock: false
+ Hide Right Dock: false
+ MotionPlanning:
+ collapsed: false
+ Width: 1600
diff --git a/waybionic_moveit_config/scripts/ik_xyz_demo.py b/waybionic_moveit_config/scripts/ik_xyz_demo.py
new file mode 100755
index 0000000..de85fe3
--- /dev/null
+++ b/waybionic_moveit_config/scripts/ik_xyz_demo.py
@@ -0,0 +1,415 @@
+#!/usr/bin/env python3
+"""Demonstrate position-only IK along the Cartesian X, Y, and Z axes."""
+
+import copy
+import threading
+import time
+
+from control_msgs.action import FollowJointTrajectory
+from geometry_msgs.msg import Point, PoseStamped
+from moveit_msgs.msg import MoveItErrorCodes
+from moveit_msgs.srv import GetPositionIK
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.time import Time
+from sensor_msgs.msg import JointState
+from std_srvs.srv import Trigger
+from tf2_ros import Buffer, TransformException, TransformListener
+from trajectory_msgs.msg import JointTrajectoryPoint
+from visualization_msgs.msg import Marker, MarkerArray
+
+
+JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4"]
+READY_POSITION = [0.0, -0.7854, 0.0, 0.7854]
+
+
+class IkXyzDemo(Node):
+ """Drive the mock arm through small Cartesian targets using MoveIt IK."""
+
+ def __init__(self):
+ super().__init__("ik_xyz_demo")
+ self.declare_parameter("step_m", 0.04)
+ self.declare_parameter("move_seconds", 0.55)
+ self.declare_parameter("pause_seconds", 0.15)
+ self.declare_parameter("cycles", 1)
+ self.declare_parameter("run_on_start", False)
+ self.declare_parameter("startup_timeout_seconds", 15.0)
+
+ marker_qos = QoSProfile(depth=1)
+ marker_qos.durability = DurabilityPolicy.TRANSIENT_LOCAL
+ marker_qos.reliability = ReliabilityPolicy.RELIABLE
+ self.marker_pub = self.create_publisher(
+ MarkerArray, "/ik_demo/markers", marker_qos
+ )
+ self.target_pub = self.create_publisher(
+ PoseStamped, "/ik_demo/target", marker_qos
+ )
+ self.create_subscription(JointState, "/joint_states", self._on_joint_state, 10)
+ self.create_service(Trigger, "/ik_demo/replay", self._on_replay)
+
+ self.ik_client = self.create_client(GetPositionIK, "/compute_ik")
+ self.trajectory_client = ActionClient(
+ self,
+ FollowJointTrajectory,
+ "/arm_controller/follow_joint_trajectory",
+ )
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self)
+
+ self._joint_state = None
+ self._joint_lock = threading.Lock()
+ self._demo_lock = threading.Lock()
+ self._demo_running = False
+ self._demo_requested = threading.Event()
+ self._stop = threading.Event()
+ self._thread = threading.Thread(target=self._worker_loop, daemon=True)
+ self._thread.start()
+ if self.get_parameter("run_on_start").value:
+ self._request_demo()
+
+ def _request_demo(self):
+ with self._demo_lock:
+ if self._demo_running or self._stop.is_set():
+ return False
+ self._demo_running = True
+ self._demo_requested.set()
+ return True
+
+ def _on_replay(self, _request, response):
+ response.success = self._request_demo()
+ if response.success:
+ response.message = "XYZ IK demo accepted"
+ else:
+ response.message = "XYZ IK demo is already running"
+ return response
+
+ def _worker_loop(self):
+ while not self._stop.is_set():
+ self._demo_requested.wait(timeout=0.2)
+ if self._stop.is_set():
+ return
+ if not self._demo_requested.is_set():
+ continue
+ self._demo_requested.clear()
+ try:
+ self._run_demo()
+ except Exception as error: # Keep the replay service alive after a failed run.
+ self.get_logger().error(f"XYZ IK demo failed: {error}")
+ finally:
+ with self._demo_lock:
+ self._demo_running = False
+
+ def _on_joint_state(self, message):
+ with self._joint_lock:
+ self._joint_state = copy.deepcopy(message)
+
+ def _wait_for_future(self, future, timeout_sec):
+ deadline = time.monotonic() + timeout_sec
+ while rclpy.ok() and not self._stop.is_set() and not future.done():
+ if time.monotonic() >= deadline:
+ return None
+ time.sleep(0.05)
+ return future.result() if future.done() else None
+
+ def _wait_until_available(self, wait_for_endpoint, timeout_sec):
+ deadline = time.monotonic() + max(0.0, float(timeout_sec))
+ while rclpy.ok() and not self._stop.is_set():
+ remaining = deadline - time.monotonic()
+ if remaining <= 0.0:
+ return False
+ try:
+ available = wait_for_endpoint(timeout_sec=min(0.2, remaining))
+ except Exception:
+ if self._stop.is_set() or not rclpy.ok():
+ return False
+ raise
+ if available:
+ return True
+ return False
+
+ def _send_joint_positions(self, positions, seconds, rejection_timeout=0.0):
+ goal = FollowJointTrajectory.Goal()
+ goal.trajectory.joint_names = JOINT_NAMES
+ point = JointTrajectoryPoint()
+ point.positions = [float(value) for value in positions]
+ point.time_from_start = Duration(seconds=seconds).to_msg()
+ goal.trajectory.points = [point]
+
+ rejection_deadline = time.monotonic() + rejection_timeout
+ send_result = None
+ while rclpy.ok() and not self._stop.is_set():
+ send_result = self._wait_for_future(
+ self.trajectory_client.send_goal_async(goal), 5.0
+ )
+ if send_result is not None and send_result.accepted:
+ break
+ if time.monotonic() >= rejection_deadline:
+ break
+ if self._stop.wait(0.1):
+ return False
+
+ if send_result is None or not send_result.accepted:
+ self.get_logger().error("The arm controller rejected the trajectory")
+ return False
+
+ result = self._wait_for_future(send_result.get_result_async(), seconds + 5.0)
+ if result is None or result.result.error_code != 0:
+ self.get_logger().error("The arm controller did not complete the motion")
+ return False
+ return True
+
+ def _lookup_wrist_pose(self):
+ deadline = time.monotonic() + 10.0
+ while rclpy.ok() and not self._stop.is_set() and time.monotonic() < deadline:
+ try:
+ transform = self.tf_buffer.lookup_transform(
+ "world", "wrist", Time(), timeout=Duration(seconds=0.5)
+ )
+ pose = PoseStamped()
+ pose.header.frame_id = "world"
+ pose.pose.position.x = transform.transform.translation.x
+ pose.pose.position.y = transform.transform.translation.y
+ pose.pose.position.z = transform.transform.translation.z
+ pose.pose.orientation = transform.transform.rotation
+ return pose
+ except TransformException:
+ time.sleep(0.1)
+ return None
+
+ def _solve_ik(self, target):
+ with self._joint_lock:
+ joint_state = copy.deepcopy(self._joint_state)
+ if joint_state is None:
+ self.get_logger().error("No joint state is available for the IK seed")
+ return None
+
+ request = GetPositionIK.Request()
+ request.ik_request.group_name = "arm"
+ request.ik_request.robot_state.joint_state = joint_state
+ request.ik_request.pose_stamped = target
+ request.ik_request.timeout = Duration(seconds=2.0).to_msg()
+ request.ik_request.avoid_collisions = False
+
+ response = self._wait_for_future(self.ik_client.call_async(request), 4.0)
+ if response is None:
+ self.get_logger().warning("IK request timed out")
+ return None
+ if response.error_code.val != MoveItErrorCodes.SUCCESS:
+ self.get_logger().warning(
+ f"IK failed with MoveIt error code {response.error_code.val}"
+ )
+ return None
+
+ solution = dict(
+ zip(
+ response.solution.joint_state.name,
+ response.solution.joint_state.position,
+ )
+ )
+ if not all(name in solution for name in JOINT_NAMES):
+ self.get_logger().error("IK response omitted one or more arm joints")
+ return None
+ return [solution[name] for name in JOINT_NAMES]
+
+ @staticmethod
+ def _point(x, y, z):
+ point = Point()
+ point.x = x
+ point.y = y
+ point.z = z
+ return point
+
+ def _base_marker(self, marker_id, marker_type, namespace):
+ marker = Marker()
+ marker.header.frame_id = "world"
+ marker.header.stamp = self.get_clock().now().to_msg()
+ marker.ns = namespace
+ marker.id = marker_id
+ marker.type = marker_type
+ marker.action = Marker.ADD
+ marker.pose.orientation.w = 1.0
+ return marker
+
+ def _publish_markers(self, origin, target, label):
+ markers = []
+ axis_length = 0.16
+ colors = [
+ ("X", axis_length, 0.0, 0.0, 1.0, 0.1, 0.1),
+ ("Y", 0.0, axis_length, 0.0, 0.1, 1.0, 0.1),
+ ("Z", 0.0, 0.0, axis_length, 0.1, 0.4, 1.0),
+ ]
+ ox = origin.pose.position.x
+ oy = origin.pose.position.y
+ oz = origin.pose.position.z
+
+ for index, (name, dx, dy, dz, red, green, blue) in enumerate(colors):
+ arrow = self._base_marker(index, Marker.ARROW, "ik_axes")
+ arrow.points = [self._point(ox, oy, oz), self._point(ox + dx, oy + dy, oz + dz)]
+ arrow.scale.x = 0.008
+ arrow.scale.y = 0.018
+ arrow.scale.z = 0.025
+ arrow.color.r = red
+ arrow.color.g = green
+ arrow.color.b = blue
+ arrow.color.a = 1.0
+ markers.append(arrow)
+
+ text = self._base_marker(10 + index, Marker.TEXT_VIEW_FACING, "ik_axes")
+ text.pose.position = self._point(ox + dx, oy + dy, oz + dz)
+ text.scale.z = 0.035
+ text.color.r = red
+ text.color.g = green
+ text.color.b = blue
+ text.color.a = 1.0
+ text.text = name
+ markers.append(text)
+
+ target_marker = self._base_marker(20, Marker.SPHERE, "ik_target")
+ target_marker.pose.position = copy.deepcopy(target.pose.position)
+ target_marker.scale.x = 0.035
+ target_marker.scale.y = 0.035
+ target_marker.scale.z = 0.035
+ target_marker.color.r = 1.0
+ target_marker.color.g = 0.85
+ target_marker.color.b = 0.1
+ target_marker.color.a = 1.0
+ markers.append(target_marker)
+
+ label_marker = self._base_marker(21, Marker.TEXT_VIEW_FACING, "ik_target")
+ label_marker.pose.position = copy.deepcopy(target.pose.position)
+ label_marker.pose.position.z += 0.06
+ label_marker.scale.z = 0.04
+ label_marker.color.r = 1.0
+ label_marker.color.g = 1.0
+ label_marker.color.b = 1.0
+ label_marker.color.a = 1.0
+ label_marker.text = label
+ markers.append(label_marker)
+
+ self.marker_pub.publish(MarkerArray(markers=markers))
+ target.header.stamp = self.get_clock().now().to_msg()
+ self.target_pub.publish(target)
+
+ def _offset_pose(self, origin, offset):
+ target = copy.deepcopy(origin)
+ target.pose.position.x += offset[0]
+ target.pose.position.y += offset[1]
+ target.pose.position.z += offset[2]
+ return target
+
+ def _run_demo(self):
+ self.get_logger().info("Waiting for MoveIt IK and the arm controller...")
+ startup_timeout = self.get_parameter("startup_timeout_seconds").value
+ if not self._wait_until_available(
+ self.ik_client.wait_for_service, startup_timeout
+ ):
+ if rclpy.ok() and not self._stop.is_set():
+ self.get_logger().error("MoveIt IK service did not become available")
+ return
+ if not self._wait_until_available(
+ self.trajectory_client.wait_for_server, startup_timeout
+ ):
+ if rclpy.ok() and not self._stop.is_set():
+ self.get_logger().error(
+ "Arm trajectory action did not become available"
+ )
+ return
+
+ deadline = time.monotonic() + 10.0
+ ready = False
+ while rclpy.ok() and not self._stop.is_set():
+ with self._joint_lock:
+ ready = self._joint_state is not None
+ if ready or time.monotonic() >= deadline:
+ break
+ time.sleep(0.1)
+ if not ready:
+ self.get_logger().error("Joint states did not become available")
+ return
+
+ self.get_logger().info("Moving to the ready pose...")
+ move_seconds = max(0.1, float(self.get_parameter("move_seconds").value))
+ if not self._send_joint_positions(
+ READY_POSITION, max(0.8, move_seconds), rejection_timeout=5.0
+ ):
+ return
+ if self._stop.wait(0.15):
+ return
+
+ origin = self._lookup_wrist_pose()
+ if origin is None:
+ self.get_logger().error("Could not resolve the wrist pose in the world frame")
+ return
+
+ step = self.get_parameter("step_m").value
+ pause = max(0.0, float(self.get_parameter("pause_seconds").value))
+ sequence = [
+ ("X axis +", (step, 0.0, 0.0)),
+ ("Center", (0.0, 0.0, 0.0)),
+ ("Y axis +", (0.0, step, 0.0)),
+ ("Center", (0.0, 0.0, 0.0)),
+ ("Z axis +", (0.0, 0.0, step)),
+ ("Center", (0.0, 0.0, 0.0)),
+ ]
+
+ cycles = max(1, int(self.get_parameter("cycles").value))
+ self.get_logger().info(
+ "XYZ IK demo started. Red=X, green=Y, blue=Z."
+ )
+ for _ in range(cycles):
+ for label, offset in sequence:
+ if not rclpy.ok() or self._stop.is_set():
+ return
+ target = self._offset_pose(origin, offset)
+ self._publish_markers(origin, target, label)
+ self.get_logger().info(
+ f"{label}: x={target.pose.position.x:.3f}, "
+ f"y={target.pose.position.y:.3f}, "
+ f"z={target.pose.position.z:.3f}"
+ )
+ solution = self._solve_ik(target)
+ if solution is None:
+ self.get_logger().error(
+ f"XYZ IK demo aborted at {label}: no IK solution"
+ )
+ return
+ if not self._send_joint_positions(solution, move_seconds):
+ self.get_logger().error(
+ f"XYZ IK demo aborted at {label}: trajectory failed"
+ )
+ return
+ if self._stop.wait(pause):
+ return
+
+ self._publish_markers(origin, origin, "Manual IK ready")
+ self.get_logger().info(
+ "Automatic demo complete. Drag the red/green/blue goal handles in "
+ "RViz, then click Plan & Execute."
+ )
+
+ def destroy_node(self):
+ self._stop.set()
+ self._demo_requested.set()
+ if self._thread.is_alive():
+ self._thread.join(timeout=5.0)
+ super().destroy_node()
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = IkXyzDemo()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/waybionic_moveit_config/srdf/waybionic.srdf b/waybionic_moveit_config/srdf/waybionic.srdf
new file mode 100644
index 0000000..d6447c8
--- /dev/null
+++ b/waybionic_moveit_config/srdf/waybionic.srdf
@@ -0,0 +1,46 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/waybionic_moveit_config/test/test_ik_demo_launch.py b/waybionic_moveit_config/test/test_ik_demo_launch.py
new file mode 100644
index 0000000..f8b80da
--- /dev/null
+++ b/waybionic_moveit_config/test/test_ik_demo_launch.py
@@ -0,0 +1,234 @@
+"""End-to-end test for the headless XYZ inverse-kinematics demo."""
+
+import math
+import os
+import time
+import unittest
+
+from action_msgs.msg import GoalStatus
+from action_msgs.msg import GoalStatusArray
+
+from ament_index_python.packages import get_package_share_directory
+
+from controller_manager_msgs.srv import ListControllers
+
+from geometry_msgs.msg import PoseStamped
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+import launch_testing.actions
+
+import pytest
+
+import rclpy
+from rclpy.qos import qos_profile_action_status_default
+
+from sensor_msgs.msg import JointState
+
+from std_srvs.srv import Trigger
+
+
+JOINT_NAMES = ('joint1', 'joint2', 'joint3', 'joint4')
+TARGET_TOLERANCE_M = 0.005
+MOTION_TOLERANCE_RAD = 0.002
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+ """Launch the production MoveIt stack without RViz or an automatic run."""
+ package_share = get_package_share_directory('waybionic_moveit_config')
+ launch_file = os.path.join(package_share, 'launch', 'demo.launch.py')
+ demo = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(launch_file),
+ launch_arguments={
+ 'use_rviz': 'false',
+ 'auto_demo': 'false',
+ }.items(),
+ )
+
+ return LaunchDescription([demo, launch_testing.actions.ReadyToTest()])
+
+
+class TestIkDemoRuntime(unittest.TestCase):
+ """Verify replay targets, IK solutions, and mock-controller execution."""
+
+ @classmethod
+ def setUpClass(cls):
+ """Create one ROS node and collect all runtime evidence."""
+ rclpy.init()
+ cls.node = rclpy.create_node('test_ik_demo_runtime')
+ cls.targets = []
+ cls.joint_states = []
+ cls.succeeded_goals = set()
+
+ cls.node.create_subscription(
+ JointState,
+ '/joint_states',
+ cls._on_joint_state,
+ 100,
+ )
+ cls.node.create_subscription(
+ GoalStatusArray,
+ '/arm_controller/follow_joint_trajectory/_action/status',
+ cls._on_action_status,
+ qos_profile_action_status_default,
+ )
+ cls.node.create_subscription(
+ PoseStamped,
+ '/ik_demo/target',
+ cls._on_target,
+ 20,
+ )
+ cls.replay_client = cls.node.create_client(Trigger, '/ik_demo/replay')
+ cls.controllers_client = cls.node.create_client(
+ ListControllers,
+ '/controller_manager/list_controllers',
+ )
+
+ @classmethod
+ def tearDownClass(cls):
+ """Release the test node before launch_testing stops the stack."""
+ cls.node.destroy_node()
+ rclpy.shutdown()
+
+ @classmethod
+ def _on_target(cls, message):
+ position = message.pose.position
+ cls.targets.append(
+ (time.monotonic(), (position.x, position.y, position.z))
+ )
+
+ @classmethod
+ def _on_joint_state(cls, message):
+ positions = dict(zip(message.name, message.position))
+ if all(name in positions for name in JOINT_NAMES):
+ cls.joint_states.append((time.monotonic(), positions))
+
+ @classmethod
+ def _on_action_status(cls, message):
+ for status in message.status_list:
+ if status.status == GoalStatus.STATUS_SUCCEEDED:
+ cls.succeeded_goals.add(bytes(status.goal_info.goal_id.uuid))
+
+ def _spin_until(self, predicate, timeout_sec, failure_message):
+ deadline = time.monotonic() + timeout_sec
+ while time.monotonic() < deadline:
+ rclpy.spin_once(self.node, timeout_sec=0.05)
+ if predicate():
+ return
+ self.fail(failure_message)
+
+ def _assert_point_almost_equal(self, actual, expected):
+ distance = math.dist(actual, expected)
+ self.assertLessEqual(
+ distance,
+ TARGET_TOLERANCE_M,
+ f'target {actual} is {distance:.4f} m from expected {expected}',
+ )
+
+ def _assert_motion_between_targets(self, start_index, end_index):
+ start_time = self.targets[start_index][0]
+ end_time = self.targets[end_index][0]
+ samples = [
+ positions
+ for timestamp, positions in self.joint_states
+ if start_time <= timestamp <= end_time
+ ]
+ self.assertGreaterEqual(
+ len(samples),
+ 2,
+ f'not enough joint-state samples for target {start_index}',
+ )
+
+ largest_range = max(
+ max(sample[name] for sample in samples)
+ - min(sample[name] for sample in samples)
+ for name in JOINT_NAMES
+ )
+ self.assertGreater(
+ largest_range,
+ MOTION_TOLERANCE_RAD,
+ f'target {start_index} produced no observable controller motion',
+ )
+
+ def _wait_for_active_controllers(self, timeout_sec):
+ deadline = time.monotonic() + timeout_sec
+ while time.monotonic() < deadline:
+ request = ListControllers.Request()
+ future = self.controllers_client.call_async(request)
+ while not future.done() and time.monotonic() < deadline:
+ rclpy.spin_once(self.node, timeout_sec=0.05)
+ if not future.done():
+ break
+
+ response = future.result()
+ active = {
+ controller.name
+ for controller in response.controller
+ if controller.state == 'active'
+ }
+ if {'joint_state_broadcaster', 'arm_controller'} <= active:
+ return
+ time.sleep(0.05)
+ self.fail('mock controllers did not become active')
+
+ def test_replay_runs_xyz_ik_and_controller(self):
+ """Run one replay and prove all targets completed on mock hardware."""
+ self.assertTrue(
+ self.replay_client.wait_for_service(timeout_sec=30.0),
+ '/ik_demo/replay did not become available',
+ )
+ self.assertTrue(
+ self.controllers_client.wait_for_service(timeout_sec=30.0),
+ '/controller_manager/list_controllers did not become available',
+ )
+ self._spin_until(
+ lambda: bool(self.joint_states),
+ 30.0,
+ '/joint_states did not become available',
+ )
+ self._wait_for_active_controllers(30.0)
+
+ # Ignore any transient-local samples from before this explicit replay.
+ self.targets.clear()
+ self.joint_states.clear()
+ self.succeeded_goals.clear()
+
+ replay_future = self.replay_client.call_async(Trigger.Request())
+ self._spin_until(
+ replay_future.done,
+ 10.0,
+ 'replay service did not respond',
+ )
+ response = replay_future.result()
+ self.assertIsNotNone(response)
+ self.assertTrue(response.success, response.message)
+
+ self._spin_until(
+ lambda: len(self.targets) >= 7 and len(self.succeeded_goals) >= 7,
+ 45.0,
+ 'XYZ replay did not finish seven successful controller goals',
+ )
+
+ targets = [position for _, position in self.targets[:7]]
+ center = targets[1]
+ step = 0.04
+ expected_targets = [
+ (center[0] + step, center[1], center[2]),
+ center,
+ (center[0], center[1] + step, center[2]),
+ center,
+ (center[0], center[1], center[2] + step),
+ center,
+ center,
+ ]
+ for actual, expected in zip(targets, expected_targets):
+ self._assert_point_almost_equal(actual, expected)
+
+ # Each outward X/Y/Z target must create measured joint motion before
+ # the following center target is published.
+ self._assert_motion_between_targets(0, 1)
+ self._assert_motion_between_targets(2, 3)
+ self._assert_motion_between_targets(4, 5)
diff --git a/waybionic_moveit_config/test/test_ik_demo_timeout_launch.py b/waybionic_moveit_config/test/test_ik_demo_timeout_launch.py
new file mode 100644
index 0000000..55b7fb2
--- /dev/null
+++ b/waybionic_moveit_config/test/test_ik_demo_timeout_launch.py
@@ -0,0 +1,74 @@
+"""Regression test for bounded IK service discovery."""
+
+import time
+import unittest
+
+from launch import LaunchDescription
+
+from launch_ros.actions import Node
+
+import launch_testing.actions
+
+import pytest
+
+import rclpy
+
+from std_srvs.srv import Trigger
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+ """Start only the demo node, deliberately without MoveIt's IK service."""
+ demo = Node(
+ package='waybionic_moveit_config',
+ executable='ik_xyz_demo.py',
+ parameters=[{'startup_timeout_seconds': 0.25}],
+ )
+ return LaunchDescription([demo, launch_testing.actions.ReadyToTest()])
+
+
+class TestMissingIkService(unittest.TestCase):
+ """Ensure missing IK does not leave Replay permanently busy."""
+
+ @classmethod
+ def setUpClass(cls):
+ """Create a client for the demo's replay service."""
+ rclpy.init()
+ cls.node = rclpy.create_node('test_missing_ik_service')
+ cls.client = cls.node.create_client(Trigger, '/ik_demo/replay')
+
+ @classmethod
+ def tearDownClass(cls):
+ """Release the test node before launch_testing stops the demo."""
+ cls.node.destroy_node()
+ rclpy.shutdown()
+
+ def _call_replay(self, timeout_sec=2.0):
+ future = self.client.call_async(Trigger.Request())
+ rclpy.spin_until_future_complete(
+ self.node,
+ future,
+ timeout_sec=timeout_sec,
+ )
+ self.assertTrue(future.done(), 'replay service did not respond')
+ return future.result()
+
+ def test_timeout_releases_busy_state(self):
+ """Allow another replay after bounded IK discovery times out."""
+ self.assertTrue(
+ self.client.wait_for_service(timeout_sec=5.0),
+ '/ik_demo/replay did not become available',
+ )
+
+ first = self._call_replay()
+ self.assertTrue(first.success, first.message)
+
+ busy = self._call_replay()
+ self.assertFalse(
+ busy.success,
+ 'replay should be busy during IK discovery',
+ )
+
+ time.sleep(0.5)
+ retried = self._call_replay()
+ self.assertTrue(retried.success, retried.message)
diff --git a/waybionic_moveit_config/urdf/waybionic_moveit.urdf.xacro b/waybionic_moveit_config/urdf/waybionic_moveit.urdf.xacro
new file mode 100644
index 0000000..81e390c
--- /dev/null
+++ b/waybionic_moveit_config/urdf/waybionic_moveit.urdf.xacro
@@ -0,0 +1,64 @@
+
+
+
+
+
+
+
+
+ mock_components/GenericSystem
+ true
+
+
+
+
+ -3.14
+ 3.14
+
+
+ 0.0
+
+
+
+
+
+
+ -3.14
+ 3.14
+
+
+ 0.0
+
+
+
+
+
+
+
+
+ 0.0
+
+
+
+
+
+
+ -3.14
+ 3.14
+
+
+ 0.0
+
+
+
+
+
+
diff --git a/waybionic_rviz_plugins/CMakeLists.txt b/waybionic_rviz_plugins/CMakeLists.txt
index ade07c3..bf6957d 100644
--- a/waybionic_rviz_plugins/CMakeLists.txt
+++ b/waybionic_rviz_plugins/CMakeLists.txt
@@ -14,21 +14,23 @@ find_package(pluginlib REQUIRED)
find_package(Qt5 REQUIRED COMPONENTS Widgets)
find_package(rclcpp REQUIRED)
find_package(rviz_common REQUIRED)
-find_package(rviz_default_plugins REQUIRED)
+find_package(std_srvs REQUIRED)
set(THIS_PACKAGE_INCLUDE_DEPENDS
diagnostic_msgs
pluginlib
rclcpp
rviz_common
- rviz_default_plugins
+ std_srvs
)
add_library(${PROJECT_NAME} SHARED
include/waybionic_rviz_plugins/diagnostics_source.hpp
include/waybionic_rviz_plugins/diagnostics_panel.hpp
+ include/waybionic_rviz_plugins/ik_demo_panel.hpp
include/waybionic_rviz_plugins/ros_diagnostics_source.hpp
src/diagnostics_panel.cpp
+ src/ik_demo_panel.cpp
src/mock_diagnostics_source.cpp
src/ros_diagnostics_source.cpp
)
diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ik_demo_panel.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ik_demo_panel.hpp
new file mode 100644
index 0000000..42bcd72
--- /dev/null
+++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ik_demo_panel.hpp
@@ -0,0 +1,55 @@
+#ifndef WAYBIONIC_RVIZ_PLUGINS__IK_DEMO_PANEL_HPP_
+#define WAYBIONIC_RVIZ_PLUGINS__IK_DEMO_PANEL_HPP_
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+class QLabel;
+class QPushButton;
+class QTimer;
+
+namespace waybionic_rviz_plugins
+{
+
+/// Small RViz panel that requests a replay from the standalone IK demo node.
+///
+/// Service futures are polled by a Qt timer. This keeps every widget update on
+/// the GUI thread and avoids ROS callbacks retaining a pointer to this panel.
+class IkDemoPanel : public rviz_common::Panel
+{
+ Q_OBJECT
+
+public:
+ explicit IkDemoPanel(QWidget * parent = nullptr);
+ ~IkDemoPanel() override;
+
+ void onInitialize() override;
+
+private:
+ using ReplayService = std_srvs::srv::Trigger;
+ using ReplayClient = rclcpp::Client;
+
+ void cancelPendingRequest();
+ void requestReplay();
+ void pollReplayService();
+
+ rclcpp::Node::SharedPtr rviz_node_;
+ ReplayClient::SharedPtr replay_client_;
+ std::optional pending_request_;
+ std::chrono::steady_clock::time_point request_deadline_;
+ bool service_was_ready_{false};
+
+ QTimer * poll_timer_{nullptr};
+ QPushButton * replay_button_{nullptr};
+ QLabel * status_label_{nullptr};
+};
+
+} // namespace waybionic_rviz_plugins
+
+#endif // WAYBIONIC_RVIZ_PLUGINS__IK_DEMO_PANEL_HPP_
diff --git a/waybionic_rviz_plugins/package.xml b/waybionic_rviz_plugins/package.xml
index 1b076cd..a4db931 100644
--- a/waybionic_rviz_plugins/package.xml
+++ b/waybionic_rviz_plugins/package.xml
@@ -13,12 +13,13 @@
qtbase5-dev
rclcpp
rviz_common
- rviz_default_plugins
+ std_srvs
launch
launch_ros
rclpy
rviz2
+ rviz_default_plugins
ament_cmake_gtest
ament_cmake_lint_cmake
diff --git a/waybionic_rviz_plugins/plugin_description.xml b/waybionic_rviz_plugins/plugin_description.xml
index faa00a6..82ae77b 100644
--- a/waybionic_rviz_plugins/plugin_description.xml
+++ b/waybionic_rviz_plugins/plugin_description.xml
@@ -7,4 +7,12 @@
WayBionic ground station diagnostics, telemetry, and alerts panel for RViz2.
+
+
+ Controls the WayBionic Cartesian XYZ inverse-kinematics demonstration.
+
+
diff --git a/waybionic_rviz_plugins/src/ik_demo_panel.cpp b/waybionic_rviz_plugins/src/ik_demo_panel.cpp
new file mode 100644
index 0000000..94ddba1
--- /dev/null
+++ b/waybionic_rviz_plugins/src/ik_demo_panel.cpp
@@ -0,0 +1,153 @@
+#include "waybionic_rviz_plugins/ik_demo_panel.hpp"
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+namespace waybionic_rviz_plugins
+{
+namespace
+{
+
+constexpr auto kPollInterval = std::chrono::milliseconds(200);
+constexpr auto kRequestTimeout = std::chrono::seconds(3);
+constexpr char kReplayService[] = "/ik_demo/replay";
+
+} // namespace
+
+IkDemoPanel::IkDemoPanel(QWidget * parent)
+: rviz_common::Panel(parent)
+{
+ setMinimumWidth(260);
+
+ auto * layout = new QVBoxLayout(this);
+ layout->setContentsMargins(10, 10, 10, 10);
+ layout->setSpacing(8);
+
+ auto * title = new QLabel("XYZ Inverse Kinematics", this);
+ auto title_font = title->font();
+ title_font.setBold(true);
+ title->setFont(title_font);
+
+ auto * instructions = new QLabel(
+ "Move the mock arm along X, Y, and Z, then return it to center.", this);
+ instructions->setWordWrap(true);
+
+ replay_button_ = new QPushButton("Replay XYZ Demo", this);
+ replay_button_->setEnabled(false);
+ connect(replay_button_, &QPushButton::clicked, this, [this]() {requestReplay();});
+
+ status_label_ = new QLabel("Waiting for the IK demo service...", this);
+ status_label_->setWordWrap(true);
+
+ layout->addWidget(title);
+ layout->addWidget(instructions);
+ layout->addWidget(replay_button_);
+ layout->addWidget(status_label_);
+ layout->addStretch(1);
+}
+
+IkDemoPanel::~IkDemoPanel()
+{
+ if (poll_timer_ != nullptr) {
+ poll_timer_->stop();
+ }
+ cancelPendingRequest();
+ replay_client_.reset();
+}
+
+void IkDemoPanel::onInitialize()
+{
+ if (auto ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction().lock()) {
+ rviz_node_ = ros_node_abstraction->get_raw_node();
+ }
+
+ if (rviz_node_) {
+ replay_client_ = rviz_node_->create_client(kReplayService);
+ } else {
+ status_label_->setText("RViz ROS node is unavailable.");
+ }
+
+ poll_timer_ = new QTimer(this);
+ connect(poll_timer_, &QTimer::timeout, this, [this]() {pollReplayService();});
+ poll_timer_->start(static_cast(kPollInterval.count()));
+ pollReplayService();
+}
+
+void IkDemoPanel::cancelPendingRequest()
+{
+ if (pending_request_ && replay_client_) {
+ replay_client_->remove_pending_request(*pending_request_);
+ }
+ pending_request_.reset();
+}
+
+void IkDemoPanel::requestReplay()
+{
+ if (pending_request_) {
+ return;
+ }
+ if (!replay_client_ || !replay_client_->service_is_ready()) {
+ replay_button_->setEnabled(false);
+ status_label_->setText("IK demo service is unavailable.");
+ service_was_ready_ = false;
+ return;
+ }
+
+ try {
+ auto request = std::make_shared();
+ pending_request_.emplace(replay_client_->async_send_request(request));
+ request_deadline_ = std::chrono::steady_clock::now() + kRequestTimeout;
+ replay_button_->setEnabled(false);
+ status_label_->setText("Requesting XYZ demo replay...");
+ } catch (const std::exception & error) {
+ pending_request_.reset();
+ replay_button_->setEnabled(true);
+ status_label_->setText(QString("Could not request replay: %1").arg(error.what()));
+ }
+}
+
+void IkDemoPanel::pollReplayService()
+{
+ if (pending_request_) {
+ if (pending_request_->wait_for(std::chrono::seconds(0)) == std::future_status::ready) {
+ try {
+ const auto response = pending_request_->get();
+ status_label_->setText(QString::fromStdString(response->message));
+ } catch (const std::exception & error) {
+ status_label_->setText(QString("Replay request failed: %1").arg(error.what()));
+ }
+ pending_request_.reset();
+ } else if (std::chrono::steady_clock::now() >= request_deadline_) {
+ cancelPendingRequest();
+ status_label_->setText("Replay request timed out.");
+ }
+ }
+
+ const bool service_ready = replay_client_ && replay_client_->service_is_ready();
+ replay_button_->setEnabled(service_ready && !pending_request_);
+
+ if (service_ready != service_was_ready_) {
+ if (service_ready) {
+ status_label_->setText("Ready to replay the XYZ demo.");
+ } else if (!pending_request_) {
+ status_label_->setText("Waiting for /ik_demo/replay...");
+ }
+ service_was_ready_ = service_ready;
+ }
+}
+
+} // namespace waybionic_rviz_plugins
+
+PLUGINLIB_EXPORT_CLASS(waybionic_rviz_plugins::IkDemoPanel, rviz_common::Panel)
diff --git a/waybionic_rviz_plugins/test/test_package_metadata.py b/waybionic_rviz_plugins/test/test_package_metadata.py
index 7b08ac0..a9b98a5 100644
--- a/waybionic_rviz_plugins/test/test_package_metadata.py
+++ b/waybionic_rviz_plugins/test/test_package_metadata.py
@@ -16,6 +16,22 @@ def test_plugin_xml_registers_diagnostics_panel():
assert 'DiagnosticsPanel' in plugin_xml
+def test_plugin_xml_registers_ik_demo_panel():
+ plugin_xml = read_text('plugin_description.xml')
+ assert 'waybionic_rviz_plugins/IkDemoPanel' in plugin_xml
+ assert 'waybionic_rviz_plugins::IkDemoPanel' in plugin_xml
+
+
+def test_ik_demo_panel_build_metadata_is_complete():
+ cmake = read_text('CMakeLists.txt')
+ package_xml = read_text('package.xml')
+
+ assert 'include/waybionic_rviz_plugins/ik_demo_panel.hpp' in cmake
+ assert 'src/ik_demo_panel.cpp' in cmake
+ assert 'find_package(std_srvs REQUIRED)' in cmake
+ assert 'std_srvs' in package_xml
+
+
def test_plugin_xml_does_not_register_surgeon_panel():
plugin_xml = read_text('plugin_description.xml')
assert 'SurgeonCameraPanel' not in plugin_xml