Add hello_orchestrator_py demo package

src/demos/hello_orchestrator_py/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.8)
+project(hello_orchestrator_py)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(Python3 REQUIRED COMPONENTS Interpreter)
+
+# Install Python package
+ament_python_install_package(${PROJECT_NAME})
+
+# Install Python executables
+install(PROGRAMS
+  scripts/print_server.py
+  scripts/move_server.py
+  scripts/orchestrator_server.py
+  scripts/orchestrator_client.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install launch and config files
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/demos/hello_orchestrator_py/README.md

@@ -0,0 +1,113 @@
+# hello_orchestrator_py
+
+
+## Overview
+
+```
+                    ┌─────────────────┐
+                    │  orchestrator   │
+                    │     client      │
+                    └────────┬────────┘
+                             │ JSON task
+                             ▼
+                    ┌─────────────────┐
+                    │  orchestrator   │
+                    │     server      │
+                    └────────┬────────┘
+                             │ 
+              ┌──────────────┴──────────────┐
+              │                             │
+              ▼                             ▼
+     ┌─────────────────┐           ┌─────────────────┐
+     │  print_server   │           │   move_server   │
+     │  (logs message) │           │   (MTC motion)  │
+     └─────────────────┘           └────────┬────────┘
+                                            │
+                                            ▼
+                                   ┌─────────────────┐
+                                   │     MoveIt      │
+                                   │   (execution)   │
+                                   └─────────────────┘
+```
+
+
+## Prerequisites
+
+MoveIt must be running with `ExecuteTaskSolutionCapability`. Custom configs in `ur5e_moveit_configs` include this by default:
+- `ur_standalone_moveit_config`
+- `ur_zivid_epick_config`
+- `ur_zivid_hande_config`
+
+The demo uses hardcoded values that must exist in your MoveIt config:
+- **Planning group**: `ur_arm` (hardcoded in `base_stages.py` and `demo.launch.py`)
+- **Named state**: `moveit_home` (hardcoded in `demo_task.json`)
+
+## Quick Start
+
+### 1. Build
+
+```bash
+colcon build --packages-select \
+  ur_standalone_moveit_config \
+  hello_orchestrator_py \
+  hello_orchestrator_py_interfaces
+
+source install/setup.bash
+```
+
+### 2. Launch MoveIt (Terminal 1)
+
+```bash
+ros2 launch ur_standalone_moveit_config robot_bringup.launch.py use_fake_hardware:=true
+```
+
+Wait for: `"You can start planning now!"`
+
+### 3. Launch Demo Servers (Terminal 2)
+
+```bash
+ros2 launch hello_orchestrator_py demo.launch.py
+```
+
+### 4. Send Test Task (Terminal 3)
+
+```bash
+ros2 run hello_orchestrator_py orchestrator_client.py <path_to_task.json>
+```
+
+Example:
+```bash
+ros2 run hello_orchestrator_py orchestrator_client.py \
+  src/demos/hello_orchestrator_py/config/demo_task.json
+```
+
+## Architecture
+
+### Action Servers
+
+| Server | Purpose |
+|--------|---------|
+| `print_server` | Logs messages to console |
+| `move_server` | Moves robot to SRDF named poses via MTC |
+| `orchestrator_server` | Dispatches JSON tasks to specialized servers |
+
+### Task Format
+
+```json
+{
+  "tasks": [
+    {"type": "print", "message": "Hello"},
+    {"type": "move", "target_pose": "moveit_home"}
+  ]
+}
+```
+
+### Execution Flow
+
+1. `orchestrator_client` sends JSON to `orchestrator_server`
+2. Orchestrator iterates through tasks
+3. Each task dispatched to appropriate server (`print_server` or `move_server`)
+4. Server executes and returns result
+5. Orchestrator reports completion
+
+

src/demos/hello_orchestrator_py/config/demo_task.json

@@ -0,0 +1,20 @@
+{
+  "tasks": [
+    {
+      "type": "print",
+      "message": "Demo starting..."
+    },
+    {
+      "type": "move",
+      "target_pose": "moveit_home"
+    },
+    {
+      "type": "print",
+      "message": "Reached moveit_home position"
+    },
+    {
+      "type": "print",
+      "message": "Demo complete!"
+    }
+  ]
+}

src/demos/hello_orchestrator_py/hello_orchestrator_py/base_action_server.py

@@ -0,0 +1,84 @@
+"""Base class for action servers - handles goal lifecycle and error handling."""
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionServer, GoalResponse
+from rclpy.action.server import ServerGoalHandle
+
+
+class BaseActionServer(Node):
+    """Base class for action servers. Subclasses implement initialize_stages()."""
+
+    def __init__(self, node_name: str, action_name: str, action_type):
+        """Initialize action server."""
+        super().__init__(node_name)
+
+        self._executing = False
+        self._action_type = action_type
+        self._stages = None
+
+        self.initialize_stages()
+
+        if self._stages is None:
+            raise RuntimeError(f"{self.__class__.__name__} must set self._stages")
+
+        self._action_server = ActionServer(
+            self,
+            action_type,
+            action_name,
+            execute_callback=self._execute_callback,
+            goal_callback=self._goal_callback,
+        )
+
+        self.get_logger().info(f"{node_name} started on '{action_name}'")
+
+    def initialize_stages(self):
+        """Create and assign the stages instance. Override in subclass."""
+        raise NotImplementedError("Subclass must implement initialize_stages()")
+
+    def _goal_callback(self, goal_request) -> GoalResponse:
+        """Accept goal if not already executing."""
+        if self._executing:
+            self.get_logger().warning("Rejecting goal: server busy")
+            return GoalResponse.REJECT
+        return GoalResponse.ACCEPT
+
+    """    Note: cancel_callback is omitted - defaults to REJECT. Individual action servers
+    cannot yet cancel mid-execution (while performing tasks). Cancellation is handled at the
+    orchestrator level (between tasks).  """
+
+    def _execute_callback(self, goal_handle: ServerGoalHandle):
+        """Execute goal with error handling."""
+        self._executing = True
+
+        try:
+            success = self._stages.run(goal_handle.request)
+            result = self._action_type.Result()
+            result.success = success
+            if not success:
+                result.error_message = "Stage execution failed"
+
+            goal_handle.succeed() if success else goal_handle.abort()
+            return result
+
+        except Exception as e:
+            self.get_logger().error(f"Exception: {e}")
+            goal_handle.abort()
+            return self._action_type.Result(success=False, error_message=str(e))
+
+        finally:
+            self._executing = False
+
+
+def run_server(server_class, args=None):
+    """Run action server with standard ROS 2 lifecycle."""
+    rclpy.init(args=args)
+    node = server_class()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()

src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/base_stages.py

@@ -0,0 +1,68 @@
+"""Base MTC stages - common functionality for MTC tasks."""
+
+import rclcpp
+from moveit.task_constructor import core, stages
+from moveit_msgs.msg import MoveItErrorCodes
+
+# rclcpp MTC node initialization 
+rclcpp.init()
+_options = rclcpp.NodeOptions()
+_options.automatically_declare_parameters_from_overrides = True
+_options.allow_undeclared_parameters = True
+_mtc_node = rclcpp.Node("mtc_task_node", _options)
+
+
+class BaseStages:
+    """Base class for MTC stage implementations."""
+
+    def __init__(self, node):
+        """Initialize with ROS 2 node for logging."""
+        self.node = node
+        self.logger = node.get_logger()
+        self._mtc_node = _mtc_node
+        self.arm_group = "ur_arm"  # from moveit SRDF 
+
+    def create_task_template(self, task_name: str) -> core.Task:
+        """Create MTC task with CurrentState stage (Every MTC task needs CurrentState as first stage)."""
+        task = core.Task()
+        task.name = task_name
+        task.loadRobotModel(self._mtc_node)
+
+        current_state = stages.CurrentState("current state")
+        task.add(current_state)
+
+        return task
+
+    def make_pipeline_planner(self):
+        """Create OMPL pipeline planner."""
+        planner = core.PipelinePlanner(self._mtc_node, "ompl")
+        planner.max_velocity_scaling_factor = 0.2
+        planner.max_acceleration_scaling_factor = 0.2
+        return planner
+
+    def load_plan_execute(self, task: core.Task) -> bool:
+        """Plan and execute MTC task. Returns True on success."""
+        try:
+            task.init()
+
+            if not task.plan(max_solutions=1):
+                self.logger.error(f"Planning failed: {task.name}")
+                return False
+
+            if not task.solutions:
+                self.logger.error(f"No solutions found: {task.name}")
+                return False
+
+            self.logger.info(f"Planning succeeded: {task.name}")
+
+            result = task.execute(task.solutions[0])
+            if result.val != MoveItErrorCodes.SUCCESS:
+                self.logger.error(f"Execution failed: {task.name} (error: {result.val})")
+                return False
+
+            self.logger.info(f"Task completed: {task.name}")
+            return True
+
+        except Exception as e:
+            self.logger.error(f"Task failed: {e}")
+            return False

src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/move_stages.py

@@ -0,0 +1,26 @@
+"""MoveTo stages - MTC-based motion planning."""
+
+from moveit.task_constructor import stages
+from hello_orchestrator_py.stages.base_stages import BaseStages
+
+
+class MoveStages(BaseStages):
+    """Handles MoveTo action using MTC."""
+
+    def run(self, goal) -> bool:
+        """Execute MoveTo action.
+
+        MoveTo.setGoal() accepts: str (SRDF named pose), joint map,
+        PoseStamped, PointStamped, or RobotState. Demo only uses SRDF named poses.
+        """
+        task = self.create_task_template("MoveTo Task")
+        planner = self.make_pipeline_planner()
+
+        move_stage = stages.MoveTo(f"move_to_{goal.target_pose}", planner)
+        move_stage.group = self.arm_group
+        move_stage.setGoal(goal.target_pose)  # SRDF named pose
+
+        self.logger.info(f"Moving to named state: {goal.target_pose}")
+
+        task.add(move_stage)
+        return self.load_plan_execute(task)

src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/print_stages.py

@@ -0,0 +1,12 @@
+"""Print stages - simple message printing."""
+
+
+class PrintStages:
+    """Handles print action."""
+
+    def __init__(self, node):
+        self.logger = node.get_logger()
+
+    def run(self, goal) -> bool:
+        self.logger.info(f"MESSAGE: {goal.message}")
+        return True