async def create_tasknode(self, task_tree): global executor # generate task_node child_tasknode = TaskNode(task_tree) # self.child_tasknode.append(self.childtask_node) executor.add_node(child_tasknode) # added last added task ## execute client = ActionClient(self, TsDoTask, child_tasknode.name, callback_group=ReentrantCallbackGroup()) if not client.wait_for_server(timeout_sec=5.0): self.get_logger().error('No action server available') goal_handle.abort() result = TsDoTask.Result() result.message = "Abort" return result goal = TsDoTask.Goal() goal_handle_client = await client.send_goal_async(goal) if not goal_handle_client.accepted: self.get_logger().info("goal reject") goal_handle.abort() result = TsDoTask.Result() result.message = "Goal Reject" return result self.get_logger().info("goal accept") return goal_handle_client, child_tasknode
async def subtask(self, goal_handle): """サブタスクを実行する """ command = self.task_tree[1] self.subtask_client = ActionClient(self, TsDoSubtask, "subtask_node_" + str(command[0]), callback_group=ReentrantCallbackGroup()) self.child_clients.append(self.subtask_client) print(self.subtask_client) # readable = [await self.read_name(c) for c in command] # 表示用 # readable = command # readable = await self.read_name(command[0]) # self.get_logger().info(f'start "{readable}"') self.get_logger().info(f'service call "subtask_node_{command[0]} args: {command[1]}"') goal_msg = TsDoSubtask.Goal() if len(command) >= 2: goal_msg.arg_json = command[1] self.get_logger().info(f'command:{command[1]}') else: goal_msg.arg_json = "{}" self.goal_handle = await self.subtask_client.send_goal_async(goal_msg) self.goal_handles.append(self.goal_handle) if not self.goal_handle.accepted: self.get_logger().info("goal rejected") goal_handle.abort() result = TsDoTask.Result() result.message = "Goal Rejected" return result self.get_logger().info("goal accept") self.future_result = await self.goal_handle.get_result_async() result = TsDoTask.Result() #if self.future_result.done(): if True: self.get_logger().info(self.future_result.result.message) goal_handle.succeed() #result.message = self.future_result.result.message result.message = "Success" else: goal_handle.abort() result.message = self.future_result.result.message self.goal_handles.pop() """ goal_handle.succeed() result = TsDoTask.Result() result.message = "Success" """ # self.get_logger().info("Success") time.sleep(0.1) return result
async def execute_callback(self, goal_handle): result = TsDoTask.Result() result.message = await self.execute(goal_handle, self.task_tree) if result.message == "Success": goal_handle.succeed() else: goal_handle.abort() return result
async def execute_callback(self, goal_handle): result = TsDoTask.Result() if self.node_type == "serial": result = await self.serial(goal_handle) elif self.node_type == "parallel": result = await self.parallel(goal_handle) else: # subtask result = await self.subtask(goal_handle) return result
async def execute_callback(self, goal_handle): """タスク処理 """ global executor self.goal_handle = goal_handle task_id = goal_handle.request.task_id arg_json = goal_handle.request.arg_json if arg_json != '': # 引数が存在するとき self.arg_data = json.loads(arg_json) else: self.arg_data = {} self.get_logger().info(f'Execute task{task_id}, args:{self.arg_data}') task_tree = await self.convert_task_to_subtasks(task_id, self.arg_data) pprint(task_tree) # generate task_node task_node = TaskNode(task_tree) executor.add_node(task_node) # added last added task ## execute client = ActionClient(self, TsDoTask, task_node.name, callback_group=ReentrantCallbackGroup()) if not client.wait_for_server(timeout_sec=5.0): self.get_logger().error('No action server available') goal_handle.abort() result = TsReq.Result() result.message = "Abort" return result goal = TsDoTask.Goal() self.goal_handle_client = await client.send_goal_async(goal) if not self.goal_handle_client.accepted: self.get_logger().info("goal reject") goal_handle.abort() result = TsReq.Result() result.message = "Goal Reject" return result self.get_logger().info("goal accept") self.future_result = await self.goal_handle_client.get_result_async() # 結果を返す msg = self.future_result.result.message if msg == "Success": goal_handle.succeed() result = TsReq.Result() result.message = "Success" else: goal_handle.abort() result = TsReq.Result() result.message = msg # result.message = "Success" # task_node.destroy_node() return result
async def execute_callback(self, goal_handle): """タスク処理 """ global executor task_id = goal_handle.request.task_id data_str = goal_handle.request.arg_json if data_str != '': self.arg_data = json.loads(data_str) else: self.arg_data = {} # syntax analyze task_tree = await self.convert_task_to_subtasks(task_id, self.arg_data) if task_tree == []: # ERROR goal_handle.abort() result = TsReq.Result() result.message = "Syntax Error" return result # generate task_node task_node = TaskNode(task_tree) self.task_node_list.append(task_node) executor.add_node(self.task_node_list[len(self.task_node_list) - 1]) # added last added task ## execute client = ActionClient(self, TsDoTask, task_node.name, callback_group=ReentrantCallbackGroup()) client.wait_for_server() goal = TsDoTask.Goal() goal_handle_client = await client.send_goal_async(goal) self.goal_handles.append(goal_handle_client) if not goal_handle_client.accepted: self.get_logger().info("goal reject") goal_handle.abort() result = TsReq.Result() result.message = "Goal Reject" return result self.get_logger().info("goal accept") self.future_result = await goal_handle_client.get_result_async() #if not self.future_result.done(): if True: goal_handle.abort() result = TsReq.Result() result.message = "Abort" return result # return result goal_handle.succeed() result = TsReq.Result() result.message = "Success" task_node.destroy_node() return result
async def serial(self, goal_handle): """child task 1を実行した後,child task 2を実行する """ global executor # child task 1 ## generate child_task_node1 = TaskNode(self.task_tree[1], name_prefix=self.name) executor.add_node(child_task_node1) self.child_task_nodes.append(child_task_node1) self.get_logger().info(f"Generate {child_task_node1.name}") # time.sleep(1.0) ## execute self.client = ActionClient(self, TsDoTask, child_task_node1.name, callback_group=ReentrantCallbackGroup()) print(f"subtask1: {self.client}") self.child_clients.append(self.client) self.client.wait_for_server() goal = TsDoTask.Goal() self.get_logger().info(f"start {child_task_node1.name} send_goal_async") self.goal_handle_client = await self.client.send_goal_async(goal) self.get_logger().info(f"end {child_task_node1.name} send_goal_async") self.goal_handles.append(self.goal_handle_client) if not self.goal_handle_client.accepted: goal_handle.abort() result = TsDoTask.Result() result.message = "Goal Reject" return result self.get_logger().info(f"start {child_task_node1.name} get_result_async") self.future_result = await self.goal_handle_client.get_result_async() self.get_logger().info(f"end {child_task_node1.name} get_result_async") if not self.future_result.result.message == "Success": goal_handle.abort() result = TsDoTask.Result() result.message = "Abort" return result self.get_logger().info(f"Execute {child_task_node1.name}") ## destroy # self.child_clients.pop() # self.client.destroy() # self.child_task_nodes.pop() # try: # executor.remove_node(child_task_node1) # child_task_node1.destroy_node() # except(e): # print(e) self.goal_handles.pop() # child_task_node1.destroy_node() self.get_logger().info(f"Destroy {child_task_node1.name}") # cancelされていないか確認する if goal_handle.is_cancel_requested: goal_handle.canceled() result = TsDoTask.Result() result.message = "Canceled" self.get_logger().info('goal canceled') return Fibonacci.Result() # child task 2 ## generate child_task_node2 = TaskNode(self.task_tree[2], name_prefix=self.name) executor.add_node(child_task_node2) self.child_task_nodes.append(child_task_node2) self.get_logger().info(f"Generate {child_task_node2.name}") # time.sleep(1.0) ## execute self.client = ActionClient(self, TsDoTask, child_task_node2.name, callback_group=ReentrantCallbackGroup()) print(f"subtask2: {self.client}") self.child_clients.append(self.client) self.client.wait_for_server() goal = TsDoTask.Goal() self.get_logger().info(f"start {child_task_node2.name} send_goal_async") self.goal_handle_client = await self.client.send_goal_async(goal) self.get_logger().info(f"end {child_task_node2.name} send_goal_async") self.goal_handles.append(self.goal_handle_client) if not self.goal_handle_client.accepted: self.get_logger().info("subtask2 goal reject") goal_handle.abort() result = TsDoTask.Result() result.message = "Goal Reject" return result self.get_logger().info("subtask2 goal accept") self.get_logger().info(f"start {child_task_node2.name} get_result_async") self.future_result = await self.goal_handle_client.get_result_async() self.get_logger().info(f"end {child_task_node2.name} get_result_async") #if not self.future_result.done(): if not self.future_result.result.message == "Success": goal_handle.abort() result = TsDoTask.Result() result.message = "Abort" return result self.get_logger().info(f"Execute {child_task_node2.name}") ## destroy """ self.child_clients.pop() self.client.destroy() self.child_task_nodes.pop() # executor.remove_node(child_task_node2) # child_task_node2.destroy_node() self.goal_handles.pop() """ self.get_logger().info(f"Destroy {child_task_node2.name}") goal_handle.succeed() result = TsDoTask.Result() result.message = "Success" return result
async def parallel(self, goal_handle): """child task 1とchild task 2を並列に実行する """ global executor # generate child_task_node1 = TaskNode(self.task_tree[1], name_prefix=self.name) self.child_task_nodes.append(child_task_node1) executor.add_node(child_task_node1) child_task_node2 = TaskNode(self.task_tree[2], name_prefix=self.name) self.child_task_nodes.append(child_task_node2) executor.add_node(child_task_node2) time.sleep(1.0) # send goal self.client1 = ActionClient(self, TsDoTask, child_task_node1.name, callback_group=ReentrantCallbackGroup()) self.child_clients.append(self.client1) self.client1.wait_for_server() goal = TsDoTask.Goal() self.goal_handle_future1 = self.client1.send_goal_async(goal) self.client2 = ActionClient(self, TsDoTask, child_task_node2.name, callback_group=ReentrantCallbackGroup()) self.child_clients.append(self.client2) self.client2.wait_for_server() goal = TsDoTask.Goal() self.goal_handle_future1 = self.client2.send_goal_async(goal) # await send_goal_async self.goal_handle_client1 = await self.goal_handle_future1 self.goal_handles.append(self.goal_handle_client1) self.goal_handle_client2 = await self.goal_handle_future2 self.goal_handles.append(self.goal_handle_client2) if not self.goal_handle_client1.accepted: self.get_logger().info("subtask1 goal reject") goal_handle.abort() result = TsDoTask.Result() result.message = "Goal Reject" return result self.get_logger().info("subtask1 goal accept") if not self.goal_handle_client2.accepted: self.get_logger().info("subtask2 goal reject") goal_handle.abort() result = TsDoTask.Result() result.message = "Goal Reject" return result self.get_logger().info("subtask2 goal accept") # get result self.future_result1 = self.goal_handle_client1.get_result_async() self.future_result2 = self.goal_handle_client2.get_result_async() self.future_result = await self.future_result1 if not self.future_result.done(): goal_handle.abort() result = TsDoTask.Result() result.message = "Abort" return result self.future_result = await self.future_result2 if not self.future_result.done(): goal_handle.abort() result = TsDoTask.Result() result.message = "Abort" return result ## destroy self.child_task_nodes.pop() self.child_clients.pop() self.goal_handles.pop() # child_task_node1.destroy_node() self.child_task_nodes.pop() self.child_clients.pop() self.goal_handles.pop() # child_task_node2.destroy_node() goal_handle.succeed() result = TsDoTask.Result() result.message = "Success" return result