diff --git a/src/demos/hello_orchestrator_py/CMakeLists.txt b/src/demos/hello_orchestrator_py/CMakeLists.txt new file mode 100644 index 000000000..5edbbae5b --- /dev/null +++ b/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() diff --git a/src/demos/hello_orchestrator_py/README.md b/src/demos/hello_orchestrator_py/README.md new file mode 100644 index 000000000..738c6856e --- /dev/null +++ b/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 +``` + +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 + + diff --git a/src/demos/hello_orchestrator_py/config/demo_task.json b/src/demos/hello_orchestrator_py/config/demo_task.json new file mode 100644 index 000000000..d4335f923 --- /dev/null +++ b/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!" + } + ] +} diff --git a/src/demos/hello_orchestrator_py/hello_orchestrator_py/__init__.py b/src/demos/hello_orchestrator_py/hello_orchestrator_py/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/demos/hello_orchestrator_py/hello_orchestrator_py/base_action_server.py b/src/demos/hello_orchestrator_py/hello_orchestrator_py/base_action_server.py new file mode 100644 index 000000000..908f299f3 --- /dev/null +++ b/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() diff --git a/src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/__init__.py b/src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/base_stages.py b/src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/base_stages.py new file mode 100644 index 000000000..a73d3d207 --- /dev/null +++ b/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 diff --git a/src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/move_stages.py b/src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/move_stages.py new file mode 100644 index 000000000..3cb1a4a12 --- /dev/null +++ b/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) diff --git a/src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/print_stages.py b/src/demos/hello_orchestrator_py/hello_orchestrator_py/stages/print_stages.py new file mode 100644 index 000000000..072b19a3d --- /dev/null +++ b/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 diff --git a/src/demos/hello_orchestrator_py/launch/demo.launch.py b/src/demos/hello_orchestrator_py/launch/demo.launch.py new file mode 100644 index 000000000..2dc97801c --- /dev/null +++ b/src/demos/hello_orchestrator_py/launch/demo.launch.py @@ -0,0 +1,56 @@ +"""Launch file for hello_orchestrator_py demo.""" + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Generate launch description for the demo. + Launches: + 1. Print action server + 2. Move action server (with MTC/OMPL params) + 3. Orchestrator action server + """ + + # MTC parameters (needed by move_server) + mtc_params = { + 'ompl': { + 'planning_plugin': 'ompl_interface/OMPLPlanner', + 'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization', + }, + 'robot_description_kinematics': { + 'ur_arm': { + 'kinematics_solver': 'kdl_kinematics_plugin/KDLKinematicsPlugin', + 'kinematics_solver_search_resolution': 0.001, + 'kinematics_solver_timeout': 1.0, + 'kinematics_solver_attempts': 10, + } + } + } + + return LaunchDescription([ + # Print action server + Node( + package='hello_orchestrator_py', + executable='print_server.py', + name='print_server_py', + output='screen', + ), + + # Move action server + Node( + package='hello_orchestrator_py', + executable='move_server.py', + name='move_server_py', + output='screen', + parameters=[mtc_params], + ), + + # Orchestrator action server + Node( + package='hello_orchestrator_py', + executable='orchestrator_server.py', + name='orchestrator_server_py', + output='screen', + ), + ]) diff --git a/src/demos/hello_orchestrator_py/package.xml b/src/demos/hello_orchestrator_py/package.xml new file mode 100644 index 000000000..e82c40565 --- /dev/null +++ b/src/demos/hello_orchestrator_py/package.xml @@ -0,0 +1,25 @@ + + + + hello_orchestrator_py + 0.0.0 + Python demo of orchestrator pattern dispatching to specialized MTC action servers + abondada + BSD-3-Clause + + ament_cmake + ament_cmake_python + + hello_orchestrator_py_interfaces + + rclpy + moveit_task_constructor_core + moveit_ros_planning_interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/demos/hello_orchestrator_py/resource/hello_orchestrator_py b/src/demos/hello_orchestrator_py/resource/hello_orchestrator_py new file mode 100644 index 000000000..e69de29bb diff --git a/src/demos/hello_orchestrator_py/scripts/move_server.py b/src/demos/hello_orchestrator_py/scripts/move_server.py new file mode 100755 index 000000000..66d72670c --- /dev/null +++ b/src/demos/hello_orchestrator_py/scripts/move_server.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +"""MoveTo action server - moves robot using MTC.""" + +from hello_orchestrator_py.base_action_server import BaseActionServer, run_server +from hello_orchestrator_py.stages.move_stages import MoveStages +from hello_orchestrator_py_interfaces.action import MoveToNamedState + + +class MoveActionServer(BaseActionServer): + """Action server for MoveTo operations using MTC.""" + + def __init__(self): + super().__init__( + node_name="move_server_py", + action_name="move_to_named_state_py", + action_type=MoveToNamedState, + ) + + def initialize_stages(self): + self._stages = MoveStages(self) + + +def main(args=None): + run_server(MoveActionServer, args) + + +if __name__ == '__main__': + main() diff --git a/src/demos/hello_orchestrator_py/scripts/orchestrator_client.py b/src/demos/hello_orchestrator_py/scripts/orchestrator_client.py new file mode 100755 index 000000000..47b2a210e --- /dev/null +++ b/src/demos/hello_orchestrator_py/scripts/orchestrator_client.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +"""Client that sends JSON tasks to the orchestrator.""" + +import json +import sys + +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient + +from hello_orchestrator_py_interfaces.action import OrchestratorTask + + +class OrchestratorClient(Node): + """Client for sending tasks to the orchestrator.""" + + def __init__(self): + super().__init__('orchestrator_client') + self._action_client = ActionClient(self, OrchestratorTask, 'orchestrator_task_py') + + def send_task(self, task_dict): + """Send task to orchestrator. Returns True on success.""" + goal = OrchestratorTask.Goal() + goal.task_json = json.dumps(task_dict) + + self.get_logger().info('Waiting for orchestrator...') + self._action_client.wait_for_server() + + self.get_logger().info(f"Sending task with {len(task_dict['tasks'])} steps") + send_goal_future = self._action_client.send_goal_async( + goal, feedback_callback=self.feedback_callback + ) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + self.get_logger().error('Goal rejected') + return False + + get_result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, get_result_future) + + result = get_result_future.result().result + if result.success: + self.get_logger().info(f'Task completed ({result.completed_steps} steps)') + else: + self.get_logger().error(f'Task failed: {result.error_message}') + + return result.success + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info( + f'Step {feedback.current_step}/{feedback.total_steps}: {feedback.current_action}' + ) + + +def main(args=None): + rclpy.init(args=args) + client = OrchestratorClient() + + if len(sys.argv) < 2: + client.get_logger().error('Usage: orchestrator_client ') + client.destroy_node() + rclpy.shutdown() + return 1 + task_file = sys.argv[1] + + try: + with open(task_file, 'r') as f: + task = json.load(f) + except (FileNotFoundError, json.JSONDecodeError) as e: + client.get_logger().error(f'Failed to load task file: {e}') + client.destroy_node() + rclpy.shutdown() + return 1 + + client.get_logger().info(f'Loading task from: {task_file}') + success = client.send_task(task) + + client.destroy_node() + rclpy.shutdown() + return 0 if success else 1 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/src/demos/hello_orchestrator_py/scripts/orchestrator_server.py b/src/demos/hello_orchestrator_py/scripts/orchestrator_server.py new file mode 100755 index 000000000..f710331f2 --- /dev/null +++ b/src/demos/hello_orchestrator_py/scripts/orchestrator_server.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3 +"""Orchestrator server - dispatches JSON tasks to specialized action servers.""" + +import json + +import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer, ActionClient, GoalResponse, CancelResponse +from rclpy.action.server import ServerGoalHandle + +from hello_orchestrator_py_interfaces.action import ( + OrchestratorTask, + PrintMessage, + MoveToNamedState, +) + + +class OrchestratorServer(Node): + """Orchestrator that dispatches tasks to print/move action servers.""" + + def __init__(self): + super().__init__("orchestrator_server_py") + + self._executing = False + self._print_client = ActionClient(self, PrintMessage, "print_message_py") + self._move_client = ActionClient(self, MoveToNamedState, "move_to_named_state_py") + + self._action_server = ActionServer( + self, + OrchestratorTask, + "orchestrator_task_py", + execute_callback=self._execute_callback, + goal_callback=self._goal_callback, + cancel_callback=self._cancel_callback, + ) + + self.get_logger().info("Orchestrator server started on 'orchestrator_task_py'") + + def _goal_callback(self, goal_request) -> GoalResponse: + if self._executing: + self.get_logger().warning("Goal rejected: orchestrator busy") + return GoalResponse.REJECT + return GoalResponse.ACCEPT + + def _cancel_callback(self, goal_handle: ServerGoalHandle) -> CancelResponse: + # Orchestrator accepts cancel - stops between tasks (not mid-task). + return CancelResponse.ACCEPT + + def _execute_callback(self, goal_handle: ServerGoalHandle): + self._executing = True + try: + return self._execute(goal_handle) + finally: + self._executing = False + + def _execute(self, goal_handle: ServerGoalHandle) -> OrchestratorTask.Result: + result = OrchestratorTask.Result() + result.success = False + result.completed_steps = 0 + feedback = OrchestratorTask.Feedback() + + try: + task_json = json.loads(goal_handle.request.task_json) + tasks = task_json["tasks"] + except (json.JSONDecodeError, KeyError) as e: + result.error_message = f"Invalid task JSON: {e}" + goal_handle.abort() + return result + + feedback.total_steps = len(tasks) + self.get_logger().info(f"Executing {len(tasks)} tasks") + + for i, task in enumerate(tasks): + if goal_handle.is_cancel_requested: + result.error_message = "Task was canceled" + result.completed_steps = i + goal_handle.canceled() + return result + + task_type = task.get("type") + if not task_type: + result.error_message = f"Task {i} missing 'type' field" + result.completed_steps = i + goal_handle.abort() + return result + + feedback.current_step = i + 1 + feedback.current_action = task_type + goal_handle.publish_feedback(feedback) + self.get_logger().info(f"Step {i+1}/{len(tasks)}: {task_type}") + + if task_type == "print": + success = self._dispatch_print(task) + elif task_type == "move": + success = self._dispatch_move(task) + else: + result.error_message = f"Unknown task type: {task_type}" + result.completed_steps = i + goal_handle.abort() + return result + + if not success: + result.error_message = f"Task {i} ({task_type}) failed" + result.completed_steps = i + goal_handle.abort() + return result + + result.completed_steps = i + 1 + + result.success = True + result.error_message = "" + goal_handle.succeed() + self.get_logger().info("All tasks completed successfully") + return result + + def _call_action(self, client, goal, timeout_sec: float = 60.0) -> bool: + """Generic action client helper.""" + if not client.wait_for_server(timeout_sec=5.0): + self.get_logger().error("Action server not available") + return False + + send_future = client.send_goal_async(goal) + rclpy.spin_until_future_complete(self, send_future, timeout_sec=10.0) + + if not send_future.done(): + self.get_logger().error("Goal acceptance timeout") + return False + + goal_handle = send_future.result() + if not goal_handle or not goal_handle.accepted: + self.get_logger().error("Goal rejected") + return False + + result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, result_future, timeout_sec=timeout_sec) + + if not result_future.done(): + self.get_logger().error("Action timeout") + return False + + return result_future.result().result.success + + def _dispatch_print(self, task: dict) -> bool: + if "message" not in task: + self.get_logger().error("Print task missing 'message' field") + return False + goal = PrintMessage.Goal() + goal.message = task["message"] + return self._call_action(self._print_client, goal, timeout_sec=10.0) + + def _dispatch_move(self, task: dict) -> bool: + if "target_pose" not in task: + self.get_logger().error("Move task missing 'target_pose' field") + return False + goal = MoveToNamedState.Goal() + goal.target_pose = task["target_pose"] + return self._call_action(self._move_client, goal, timeout_sec=60.0) + + +def main(args=None): + rclpy.init(args=args) + node = OrchestratorServer() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/demos/hello_orchestrator_py/scripts/print_server.py b/src/demos/hello_orchestrator_py/scripts/print_server.py new file mode 100755 index 000000000..0b3bfdcb8 --- /dev/null +++ b/src/demos/hello_orchestrator_py/scripts/print_server.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +"""Print action server""" + +from hello_orchestrator_py.base_action_server import BaseActionServer, run_server +from hello_orchestrator_py.stages.print_stages import PrintStages +from hello_orchestrator_py_interfaces.action import PrintMessage + + +class PrintActionServer(BaseActionServer): + """Action server for printing messages.""" + + def __init__(self): + super().__init__( + node_name="print_server_py", + action_name="print_message_py", + action_type=PrintMessage, + ) + + def initialize_stages(self): + self._stages = PrintStages(self) + + +def main(args=None): + run_server(PrintActionServer, args) + + +if __name__ == '__main__': + main() diff --git a/src/demos/hello_orchestrator_py_interfaces/CMakeLists.txt b/src/demos/hello_orchestrator_py_interfaces/CMakeLists.txt new file mode 100644 index 000000000..b08590fa7 --- /dev/null +++ b/src/demos/hello_orchestrator_py_interfaces/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.8) +project(hello_orchestrator_py_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "action/PrintMessage.action" + "action/MoveToNamedState.action" + "action/OrchestratorTask.action" + ADD_LINTER_TESTS +) + +ament_package() diff --git a/src/demos/hello_orchestrator_py_interfaces/action/MoveToNamedState.action b/src/demos/hello_orchestrator_py_interfaces/action/MoveToNamedState.action new file mode 100644 index 000000000..c28797e44 --- /dev/null +++ b/src/demos/hello_orchestrator_py_interfaces/action/MoveToNamedState.action @@ -0,0 +1,8 @@ +# Goal +string target_pose +--- +# Result +bool success +string error_message +--- +# Feedback (empty - internal action, not user-facing) diff --git a/src/demos/hello_orchestrator_py_interfaces/action/OrchestratorTask.action b/src/demos/hello_orchestrator_py_interfaces/action/OrchestratorTask.action new file mode 100644 index 000000000..64422f3c1 --- /dev/null +++ b/src/demos/hello_orchestrator_py_interfaces/action/OrchestratorTask.action @@ -0,0 +1,12 @@ +# Goal +string task_json +--- +# Result +bool success +string error_message +int32 completed_steps +--- +# Feedback (user-facing progress updates) +int32 current_step +int32 total_steps +string current_action diff --git a/src/demos/hello_orchestrator_py_interfaces/action/PrintMessage.action b/src/demos/hello_orchestrator_py_interfaces/action/PrintMessage.action new file mode 100644 index 000000000..22ebfe13d --- /dev/null +++ b/src/demos/hello_orchestrator_py_interfaces/action/PrintMessage.action @@ -0,0 +1,8 @@ +# Goal +string message +--- +# Result +bool success +string error_message +--- +# Feedback (empty - internal action, not user-facing) diff --git a/src/demos/hello_orchestrator_py_interfaces/package.xml b/src/demos/hello_orchestrator_py_interfaces/package.xml new file mode 100644 index 000000000..cb893f044 --- /dev/null +++ b/src/demos/hello_orchestrator_py_interfaces/package.xml @@ -0,0 +1,22 @@ + + + + hello_orchestrator_py_interfaces + 0.0.1 + Action interfaces for hello_orchestrator_py + abondada + BSD-3-Clause + + ament_cmake + rosidl_default_generators + + action_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + +