API source snapshot · 2026-09-24
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 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() |