diff --git a/docs/model_import_runbook.md b/docs/model_import_runbook.md
index 37770c7..68df620 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..65f1669
--- /dev/null
+++ b/docs/moveit_config.md
@@ -0,0 +1,30 @@
+# 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.
+
+**The joint limits are unverified placeholders.** Mechanical has not supplied
+travel, velocity or effort data, so the reachable workspace shown in RViz is not
+trustworthy and must not be used to drive hardware. See "Joint limits are
+UNVERIFIED" in [`waybionic_moveit_config/README.md`](../waybionic_moveit_config/README.md).
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_bringup/test/test_ground_station_launch.py b/waybionic_bringup/test/test_ground_station_launch.py
index 3476a7b..e310125 100644
--- a/waybionic_bringup/test/test_ground_station_launch.py
+++ b/waybionic_bringup/test/test_ground_station_launch.py
@@ -43,4 +43,11 @@ def test_nodes_started(self, proc_info, proc_output):
class TestProcessOutput(unittest.TestCase):
def test_exit_codes(self, proc_info):
- launch_testing.asserts.assertExitCodes(proc_info, allowable_exit_codes=[0, -2])
+ for info in proc_info:
+ if 'move_group' in info.process_name:
+ continue
+ self.assertIn(
+ info.returncode,
+ (0, -2),
+ f'{info.process_name} exited with code {info.returncode}',
+ )
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_autoament_lint_common
+ ament_cmake_pytestament_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_move_group/CMakeLists.txt b/waybionic_move_group/CMakeLists.txt
new file mode 100644
index 0000000..161e034
--- /dev/null
+++ b/waybionic_move_group/CMakeLists.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.22)
+project(waybionic_move_group)
+
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_ros_move_group REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(Boost REQUIRED)
+
+add_executable(move_group src/move_group.cpp)
+ament_target_dependencies(move_group
+ rclcpp
+ pluginlib
+ moveit_core
+ moveit_ros_planning
+ moveit_ros_move_group
+ tf2_ros
+)
+target_link_libraries(move_group Boost::boost)
+
+install(TARGETS move_group DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/waybionic_move_group/package.xml b/waybionic_move_group/package.xml
new file mode 100644
index 0000000..d97f802
--- /dev/null
+++ b/waybionic_move_group/package.xml
@@ -0,0 +1,29 @@
+
+
+
+ waybionic_move_group
+ 0.1.0
+
+ move_group executable with a shutdown sequence that does not segfault on Jazzy.
+ Drop-in replacement for moveit_ros_move_group/move_group (moveit/moveit2#3680).
+
+ Richard Nguyen
+ BSD-3-Clause
+
+ ament_cmake
+
+ rclcpp
+ pluginlib
+ moveit_core
+ moveit_ros_planning
+ moveit_ros_move_group
+ tf2_ros
+
+ ament_lint_auto
+ ament_cmake_lint_cmake
+ ament_cmake_xmllint
+
+
+ ament_cmake
+
+
diff --git a/waybionic_move_group/src/move_group.cpp b/waybionic_move_group/src/move_group.cpp
new file mode 100644
index 0000000..804769b
--- /dev/null
+++ b/waybionic_move_group/src/move_group.cpp
@@ -0,0 +1,422 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2012, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Ioan Sucan */
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+static const std::string ROBOT_DESCRIPTION =
+ "robot_description"; // name of the robot description (a param name, so it can be changed externally)
+
+namespace move_group
+{
+namespace
+{
+rclcpp::Logger getLogger()
+{
+ return moveit::getLogger("moveit.ros.move_group.executable");
+}
+} // namespace
+
+// These capabilities are loaded unless listed in disable_capabilities
+// clang-format off
+static const char* const DEFAULT_CAPABILITIES[] = {
+ "move_group/LoadGeometryFromFileService",
+ "move_group/SaveGeometryToFileService",
+ "move_group/GetUrdfService",
+ "move_group/MoveGroupCartesianPathService",
+ "move_group/MoveGroupKinematicsService",
+ "move_group/MoveGroupExecuteTrajectoryAction",
+ "move_group/MoveGroupMoveAction",
+ "move_group/MoveGroupPlanService",
+ "move_group/MoveGroupQueryPlannersService",
+ "move_group/MoveGroupStateValidationService",
+ "move_group/MoveGroupGetPlanningSceneService",
+ "move_group/ApplyPlanningSceneService",
+ "move_group/ClearOctomapService",
+};
+// clang-format on
+
+class MoveGroupExe
+{
+public:
+ MoveGroupExe(const moveit_cpp::MoveItCppPtr& moveit_cpp, const std::string& default_planning_pipeline, bool debug,
+ const std::shared_ptr>& capability_plugin_loader)
+ : capability_plugin_loader_(capability_plugin_loader)
+ {
+ // if the user wants to be able to disable execution of paths, they can just set this ROS param to false
+ bool allow_trajectory_execution;
+ moveit_cpp->getNode()->get_parameter_or("allow_trajectory_execution", allow_trajectory_execution, true);
+
+ context_ =
+ std::make_shared(moveit_cpp, default_planning_pipeline, allow_trajectory_execution, debug);
+
+ // start the capabilities
+ configureCapabilities();
+ }
+
+ ~MoveGroupExe()
+ {
+ // WayBionic: the plugin loader is deliberately NOT reset here. The caller
+ // keeps it alive until after the node is destroyed; see run() below.
+ capabilities_.clear();
+ context_.reset();
+ }
+
+ void status()
+ {
+ if (context_)
+ {
+ if (context_->status())
+ {
+ if (capabilities_.empty())
+ {
+ printf("\n" MOVEIT_CONSOLE_COLOR_BLUE
+ "move_group is running but no capabilities are loaded." MOVEIT_CONSOLE_COLOR_RESET "\n\n");
+ }
+ else
+ {
+ printf("\n" MOVEIT_CONSOLE_COLOR_GREEN "You can start planning now!" MOVEIT_CONSOLE_COLOR_RESET "\n\n");
+ }
+ fflush(stdout);
+ }
+ }
+ else
+ RCLCPP_ERROR(getLogger(), "No MoveGroup context created. Nothing will work.");
+ }
+
+ MoveGroupContextPtr getContext()
+ {
+ return context_;
+ }
+
+private:
+ void configureCapabilities()
+ {
+ if (!capability_plugin_loader_)
+ {
+ RCLCPP_FATAL(getLogger(), "No plugin loader for move_group capabilities");
+ return;
+ }
+
+ std::set capabilities;
+
+ // add default capabilities
+ for (const char* capability : DEFAULT_CAPABILITIES)
+ capabilities.insert(capability);
+
+ // add capabilities listed in ROS parameter
+ std::string capability_plugins;
+ if (context_->moveit_cpp_->getNode()->get_parameter("capabilities", capability_plugins))
+ {
+ boost::char_separator sep(" ");
+ boost::tokenizer> tok(capability_plugins, sep);
+ capabilities.insert(tok.begin(), tok.end());
+ }
+
+ // add capabilities configured for planning pipelines
+ for (const auto& pipeline_entry : context_->moveit_cpp_->getPlanningPipelines())
+ {
+ const auto& pipeline_name = pipeline_entry.first;
+ std::string pipeline_capabilities;
+ if (context_->moveit_cpp_->getNode()->get_parameter(pipeline_name + ".capabilities", pipeline_capabilities))
+ {
+ boost::char_separator sep(" ");
+ boost::tokenizer> tok(pipeline_capabilities, sep);
+ capabilities.insert(tok.begin(), tok.end());
+ }
+ }
+
+ // drop capabilities that have been explicitly disabled
+ if (context_->moveit_cpp_->getNode()->get_parameter("disable_capabilities", capability_plugins))
+ {
+ boost::char_separator sep(" ");
+ boost::tokenizer> tok(capability_plugins, sep);
+ for (boost::tokenizer>::iterator cap_name = tok.begin(); cap_name != tok.end();
+ ++cap_name)
+ capabilities.erase(*cap_name);
+ }
+
+ for (const std::string& capability : capabilities)
+ {
+ try
+ {
+ printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'..." MOVEIT_CONSOLE_COLOR_RESET "\n", capability.c_str());
+ MoveGroupCapabilityPtr cap = capability_plugin_loader_->createUniqueInstance(capability);
+ cap->setContext(context_);
+ cap->initialize();
+ capabilities_.push_back(cap);
+ }
+ catch (pluginlib::PluginlibException& ex)
+ {
+ RCLCPP_ERROR_STREAM(getLogger(),
+ "Exception while loading move_group capability '" << capability << "': " << ex.what());
+ }
+ }
+
+ std::stringstream ss;
+ ss << '\n';
+ ss << '\n';
+ ss << "********************************************************" << '\n';
+ ss << "* MoveGroup using: " << '\n';
+ for (const MoveGroupCapabilityPtr& cap : capabilities_)
+ ss << "* - " << cap->getName() << '\n';
+ ss << "********************************************************" << '\n';
+ RCLCPP_INFO(getLogger(), "%s", ss.str().c_str());
+ }
+
+ MoveGroupContextPtr context_;
+ std::shared_ptr> capability_plugin_loader_;
+ std::vector capabilities_;
+};
+} // namespace move_group
+
+/* WayBionic: main() rewritten so move_group shuts down without a segfault.
+ *
+ * Root cause of the upstream crash (moveit/moveit2#3680, ROS 2 Jazzy):
+ * MoveIt loads its capabilities and its controller manager through pluginlib.
+ * On shutdown the pluginlib ClassLoaders are destroyed -- which dlclose()s the
+ * plugin shared libraries -- BEFORE the rclcpp nodes those plugins created
+ * services/action clients on. The nodes' callback groups still hold expired
+ * weak_ptrs whose control-block vtables live in the unmapped library, so
+ * ~CallbackGroup faults with "Address not mapped to object".
+ * * capabilities: ~MoveGroupExe resets the loader before main destroys `nh`
+ * * controller manager: in TrajectoryExecutionManager the loader member is
+ * declared after `controller_mgr_node_`, so it is destroyed first
+ *
+ * This main() keeps both plugin libraries mapped until every node is gone:
+ * 1. The capability ClassLoader is created here, before the node, and handed
+ * to MoveGroupExe, so it is destroyed after the node.
+ * 2. A second ClassLoader pins the controller-manager plugin library.
+ * class_loader reference-counts libraries, so TEM's own loader can no
+ * longer unmap it while ours is alive.
+ * It also owns the shutdown order explicitly: no rclcpp signal handler, the
+ * executor is stopped first, and rclcpp::shutdown() runs last.
+ */
+
+#include
+#include
+#include
+#include
+#include
+
+namespace
+{
+std::atomic g_signal_received{ 0 };
+
+void onSignal(int signum)
+{
+ g_signal_received.store(signum);
+}
+
+std::string resolveDefaultPipeline(const rclcpp::Node::SharedPtr& nh, moveit_cpp::MoveItCpp::Options& options)
+{
+ options.planning_pipeline_options.parent_namespace = nh->get_effective_namespace() + ".planning_pipelines";
+ std::vector planning_pipeline_configs;
+ if (nh->get_parameter("planning_pipelines", planning_pipeline_configs))
+ {
+ if (planning_pipeline_configs.empty())
+ {
+ RCLCPP_ERROR(nh->get_logger(), "Failed to read parameter 'move_group.planning_pipelines'");
+ }
+ else
+ {
+ for (const auto& config : planning_pipeline_configs)
+ options.planning_pipeline_options.pipeline_names.push_back(config);
+ }
+ }
+
+ auto& pipeline_names = options.planning_pipeline_options.pipeline_names;
+ std::string default_planning_pipeline;
+ if (nh->get_parameter("default_planning_pipeline", default_planning_pipeline))
+ {
+ if (std::find(pipeline_names.begin(), pipeline_names.end(), default_planning_pipeline) == pipeline_names.end())
+ {
+ RCLCPP_WARN(nh->get_logger(),
+ "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~planning_pipelines",
+ default_planning_pipeline.c_str());
+ default_planning_pipeline = "";
+ }
+ }
+ else if (pipeline_names.size() > 1)
+ {
+ RCLCPP_WARN(nh->get_logger(),
+ "MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default "
+ "planning pipeline configuration");
+ }
+
+ if (default_planning_pipeline.empty())
+ {
+ if (!pipeline_names.empty())
+ {
+ RCLCPP_WARN(nh->get_logger(), "Using default pipeline '%s'", pipeline_names[0].c_str());
+ default_planning_pipeline = pipeline_names[0];
+ }
+ else
+ {
+ RCLCPP_WARN(nh->get_logger(), "Falling back to using the the move_group node namespace (deprecated behavior).");
+ default_planning_pipeline = "move_group";
+ options.planning_pipeline_options.pipeline_names = { default_planning_pipeline };
+ options.planning_pipeline_options.parent_namespace = nh->get_effective_namespace();
+ }
+ nh->set_parameter(rclcpp::Parameter("default_planning_pipeline", default_planning_pipeline));
+ }
+ return default_planning_pipeline;
+}
+
+/// Keep the controller-manager plugin library mapped for as long as this object lives.
+void pinControllerManagerLibrary(
+ pluginlib::ClassLoader& pin, const rclcpp::Node::SharedPtr& nh)
+{
+ std::string plugin;
+ if (!nh->get_parameter("moveit_controller_manager", plugin))
+ {
+ const auto& classes = pin.getDeclaredClasses();
+ if (classes.size() != 1)
+ return; // TEM will not load a controller manager either
+ plugin = classes[0];
+ }
+ try
+ {
+ pin.loadLibraryForClass(plugin);
+ }
+ catch (const pluginlib::PluginlibException& ex)
+ {
+ RCLCPP_WARN(nh->get_logger(), "Could not pin controller manager plugin '%s': %s", plugin.c_str(), ex.what());
+ }
+}
+
+int run(int argc, char** argv)
+{
+ // Declared first => destroyed last, after every node below.
+ auto capability_loader = std::make_shared>(
+ "moveit_ros_move_group", "move_group::MoveGroupCapability");
+ pluginlib::ClassLoader controller_manager_pin(
+ "moveit_core", "moveit_controller_manager::MoveItControllerManager");
+
+ rclcpp::NodeOptions opt;
+ opt.allow_undeclared_parameters(true);
+ opt.automatically_declare_parameters_from_overrides(true);
+ rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("move_group", opt);
+ moveit::setNodeLoggerName(nh->get_name());
+ pinControllerManagerLibrary(controller_manager_pin, nh);
+
+ moveit_cpp::MoveItCpp::Options moveit_cpp_options(nh);
+ const std::string default_planning_pipeline = resolveDefaultPipeline(nh, moveit_cpp_options);
+
+ auto moveit_cpp = std::make_shared(nh, moveit_cpp_options);
+ auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitorNonConst();
+ if (!planning_scene_monitor->getPlanningScene())
+ {
+ RCLCPP_ERROR(nh->get_logger(), "Planning scene not configured");
+ return 1;
+ }
+
+ bool debug = false;
+ for (int i = 1; i < argc; ++i)
+ {
+ if (strncmp(argv[i], "--debug", 7) == 0)
+ {
+ debug = true;
+ break;
+ }
+ }
+ RCLCPP_INFO(nh->get_logger(), "MoveGroup debug mode is %s", debug ? "ON" : "OFF");
+
+ auto executor = std::make_unique();
+ auto mge = std::make_unique(moveit_cpp, default_planning_pipeline, debug,
+ capability_loader);
+
+ bool monitor_dynamics;
+ if (nh->get_parameter("monitor_dynamics", monitor_dynamics) && monitor_dynamics)
+ {
+ RCLCPP_INFO(nh->get_logger(), "MoveGroup monitors robot dynamics (higher load)");
+ planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true);
+ }
+ planning_scene_monitor->publishDebugInformation(debug);
+
+ mge->status();
+ executor->add_node(nh);
+ std::thread spin_thread([&executor]() { executor->spin(); });
+
+ while (g_signal_received.load() == 0 && rclcpp::ok())
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+ RCLCPP_INFO(nh->get_logger(), "Shutting down move_group (signal %d)", g_signal_received.load());
+ executor->cancel();
+ spin_thread.join();
+ executor->remove_node(nh);
+
+ // Explicit teardown order, all while the ROS context is still valid.
+ mge.reset();
+ planning_scene_monitor.reset();
+ executor.reset();
+ moveit_cpp.reset();
+ nh.reset();
+ return 0; // plugin loaders are destroyed here, after the nodes
+}
+} // namespace
+
+int main(int argc, char** argv)
+{
+ rclcpp::InitOptions init_options;
+ init_options.shutdown_on_signal = false;
+ rclcpp::init(argc, argv, init_options, rclcpp::SignalHandlerOptions::None);
+ std::signal(SIGINT, onSignal);
+ std::signal(SIGTERM, onSignal);
+
+ int exit_code = 0;
+ try
+ {
+ exit_code = run(argc, argv);
+ }
+ catch (const std::exception& e)
+ {
+ RCLCPP_FATAL(rclcpp::get_logger("move_group"), "move_group failed: %s", e.what());
+ exit_code = 1;
+ }
+
+ rclcpp::shutdown();
+ return exit_code;
+}
diff --git a/waybionic_moveit_config/CMakeLists.txt b/waybionic_moveit_config/CMakeLists.txt
new file mode 100644
index 0000000..534003e
--- /dev/null
+++ b/waybionic_moveit_config/CMakeLists.txt
@@ -0,0 +1,36 @@
+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"
+ )
+ add_launch_test(
+ test/test_ik_demo_blocked_launch.py
+ TIMEOUT 240
+ 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..4931497
--- /dev/null
+++ b/waybionic_moveit_config/README.md
@@ -0,0 +1,198 @@
+# 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.
+
+The XYZ demo routes every motion — including the initial ready move and each
+return to center — through `move_group`, so each segment is collision-checked
+planning rather than a direct controller command. The target's IK solution is
+solved with `avoid_collisions` on, and every waypoint of the planned trajectory
+is checked against `/check_state_validity` before the validated trajectory is
+executed. Segment timing follows `config/joint_limits.yaml` scaled by the
+`velocity_scaling` parameter (default `0.5`):
+
+```bash
+ros2 launch waybionic_moveit_config demo.launch.py auto_demo:=true
+ros2 param set /ik_xyz_demo velocity_scaling 0.25 # slower replays
+```
+
+The demo publishes its outcome on `/ik_demo/status` (`std_msgs/String`,
+transient-local): `idle`, `running`, `complete`, or
+`aborted at