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