From 666ee6de5806d878d3c27c7c34541c4601e4b145 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 18:31:50 +0900 Subject: [PATCH 01/10] Add requirements of transitions --- route_guidance_ros/requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/route_guidance_ros/requirements.txt b/route_guidance_ros/requirements.txt index ff3e89cf..5495fb3e 100644 --- a/route_guidance_ros/requirements.txt +++ b/route_guidance_ros/requirements.txt @@ -1,3 +1,4 @@ pyrois numpy termcolor +transitions -- GitLab From 9287f807fcf2f9ff18684abfdfe8fce815034c8b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 18:32:20 +0900 Subject: [PATCH 02/10] Make Service as StateMachine with transitions mudule --- route_guidance_ros/scripts/service_app_lv2.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 56abfeff..4c0cc252 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -35,10 +35,21 @@ class TaskState(object): def next(self): return TaskState.__task_seq_table[self.task_kind] +from transitions import Machine class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) + # buid state machine + states = ['Init', 'NaviToUser', 'NaviToGuide', 'End'] + transitions = [ + {'trigger': 'StartService', 'source': 'Init', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, + {'trigger': 'NaviToUserSucceeded', 'source': 'NaviToUser', 'dest': 'NaviToGuide', 'after': 'start_navi_to_guide'}, + {'trigger': 'NaviToUserFailed', 'source': 'NaviToUser', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, + {'trigger': 'NaviToGuideSucceeded', 'source': 'NaviToGuide', 'dest': 'End'}, + ] + self.machine = Machine(self, states=states, transitions=transitions, initial='Init') + self.target_pos = "point_c" self.dest_pos = "point_a" self.commandid_taskstate_table = {} -- GitLab From b37e95925d723ebe691737b0743994caa5309a07 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 19:21:37 +0900 Subject: [PATCH 03/10] Use callback functions of state-machine --- route_guidance_ros/scripts/service_app_lv2.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 4c0cc252..9f016f36 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -35,6 +35,7 @@ class TaskState(object): def next(self): return TaskState.__task_seq_table[self.task_kind] +from termcolor import colored from transitions import Machine class Service(Service_Application_IF): @@ -46,12 +47,11 @@ class Service(Service_Application_IF): {'trigger': 'StartService', 'source': 'Init', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, {'trigger': 'NaviToUserSucceeded', 'source': 'NaviToUser', 'dest': 'NaviToGuide', 'after': 'start_navi_to_guide'}, {'trigger': 'NaviToUserFailed', 'source': 'NaviToUser', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, - {'trigger': 'NaviToGuideSucceeded', 'source': 'NaviToGuide', 'dest': 'End'}, + {'trigger': 'NaviToGuideSucceeded', 'source': 'NaviToGuide', 'dest': 'End', 'after': 'end_service' }, ] - self.machine = Machine(self, states=states, transitions=transitions, initial='Init') - self.target_pos = "point_c" self.dest_pos = "point_a" + self.machine = Machine(self, states=states, transitions=transitions, initial='Init') self.commandid_taskstate_table = {} def completed(self, command_id, status): @@ -60,9 +60,9 @@ class Service(Service_Application_IF): task.status = status next_task = task.next() if next_task == TaskKind.NaviToGuide: - self.start_navi_to_guide(self.dest_pos) + self.NaviToUserSucceeded(self.dest_pos) elif next_task == TaskKind.End: - print("Route Guide Finished !!") + self.NaviToGuideSucceeded() self.commandid_taskstate_table[command_id] = task print(self.commandid_taskstate_table) @@ -84,11 +84,13 @@ class Service(Service_Application_IF): stautus = RoIS_HRI.ReturnCode_t(return_code) self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToGuide) + def end_service(self): + print(colored("Route Guidance Completed !", 'cyan')) + def run(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) - target_point = "point_c" - self.start_navi_to_user(target_point) + self.StartService(self.target_pos) if __name__ == '__main__': -- GitLab From 77d923266186c7631e8ffb6b6c89ed4b6acf268b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 19:46:45 +0900 Subject: [PATCH 04/10] Remove old code for task management --- route_guidance_ros/scripts/service_app_lv2.py | 44 +++---------------- 1 file changed, 7 insertions(+), 37 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 9f016f36..8b9598bf 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -10,34 +10,10 @@ import threading import time import enum import xmlrpc - - -class TaskKind(enum.Enum): - NaviToUser = 1 - NaviToGuide = 2 - End = 3 - - -class TaskState(object): - __task_seq_table = { - TaskKind.NaviToUser: TaskKind.NaviToGuide, - TaskKind.NaviToGuide: TaskKind.End, - TaskKind.End: TaskKind.End - } - - def __init__(self, task_kind, status=None): - self.task_kind = task_kind - self.status = status - - def __repr__(self): - return "".format(self.task_kind, self.status) - - def next(self): - return TaskState.__task_seq_table[self.task_kind] - from termcolor import colored from transitions import Machine + class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) @@ -52,19 +28,13 @@ class Service(Service_Application_IF): self.target_pos = "point_c" self.dest_pos = "point_a" self.machine = Machine(self, states=states, transitions=transitions, initial='Init') - self.commandid_taskstate_table = {} - def completed(self, command_id, status): - task = self.commandid_taskstate_table[command_id] - status = Completed_Status(status) - task.status = status - next_task = task.next() - if next_task == TaskKind.NaviToGuide: + def completed(self, command_id, completed_status): + completed_status = Completed_Status(completed_status) + if self.state == 'NaviToUser': self.NaviToUserSucceeded(self.dest_pos) - elif next_task == TaskKind.End: + elif self.state == 'NaviToGuide': self.NaviToGuideSucceeded() - self.commandid_taskstate_table[command_id] = task - print(self.commandid_taskstate_table) def notify_error(self, error_id, error_type): print('received error event {}({}) '.format( \ @@ -75,14 +45,14 @@ class Service(Service_Application_IF): event_id, event_type, subscribe_id, expire)) def start_navi_to_user(self, dest): + print(colored("NaviToUser started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) - self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToUser) def start_navi_to_guide(self, dest): + print(colored("NaviToGuide started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) - self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToGuide) def end_service(self): print(colored("Route Guidance Completed !", 'cyan')) -- GitLab From dbecc17fec9a321bebc71934302834a6a71b2149 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 19:59:41 +0900 Subject: [PATCH 05/10] Use Task enum class --- route_guidance_ros/scripts/service_app_lv2.py | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 8b9598bf..e38956ed 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -14,26 +14,33 @@ from termcolor import colored from transitions import Machine +class Task(enum.Enum): + Init = 'Init' + NaviToUser = 'NaviToUser' + NaviToGuide = 'NaviToGuide' + End = 'End' + + class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) # buid state machine - states = ['Init', 'NaviToUser', 'NaviToGuide', 'End'] + states = [Task.Init, Task.NaviToUser, Task.NaviToGuide, Task.End] transitions = [ - {'trigger': 'StartService', 'source': 'Init', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, - {'trigger': 'NaviToUserSucceeded', 'source': 'NaviToUser', 'dest': 'NaviToGuide', 'after': 'start_navi_to_guide'}, - {'trigger': 'NaviToUserFailed', 'source': 'NaviToUser', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, - {'trigger': 'NaviToGuideSucceeded', 'source': 'NaviToGuide', 'dest': 'End', 'after': 'end_service' }, + {'trigger': 'StartService', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviToUserSucceeded', 'source': Task.NaviToUser, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'NaviToUserFailed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviToGuideSucceeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, ] self.target_pos = "point_c" self.dest_pos = "point_a" - self.machine = Machine(self, states=states, transitions=transitions, initial='Init') + self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init) def completed(self, command_id, completed_status): completed_status = Completed_Status(completed_status) - if self.state == 'NaviToUser': + if self.state == Task.NaviToUser: self.NaviToUserSucceeded(self.dest_pos) - elif self.state == 'NaviToGuide': + elif self.state == Task.NaviToGuide: self.NaviToGuideSucceeded() def notify_error(self, error_id, error_type): -- GitLab From 548d3ec1ec18686155a53c4d32289349366b06da Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 20:16:47 +0900 Subject: [PATCH 06/10] Unify trigger of NaviToUser and NaviToGuidance --- route_guidance_ros/scripts/service_app_lv2.py | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index e38956ed..5926e186 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -27,21 +27,21 @@ class Service(Service_Application_IF): # buid state machine states = [Task.Init, Task.NaviToUser, Task.NaviToGuide, Task.End] transitions = [ - {'trigger': 'StartService', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, - {'trigger': 'NaviToUserSucceeded', 'source': Task.NaviToUser, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, - {'trigger': 'NaviToUserFailed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, - {'trigger': 'NaviToGuideSucceeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, + {'trigger': 'StartService', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSucceeded', 'source': Task.NaviToUser, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'NaviFailed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSucceeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, ] self.target_pos = "point_c" self.dest_pos = "point_a" - self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init) + self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) def completed(self, command_id, completed_status): completed_status = Completed_Status(completed_status) - if self.state == Task.NaviToUser: - self.NaviToUserSucceeded(self.dest_pos) - elif self.state == Task.NaviToGuide: - self.NaviToGuideSucceeded() + if completed_status == Completed_Status.OK: + self.NaviSucceeded(self.dest_pos) + elif completed_status == Completed_Status.ERROR: + self.NaviFailed(self.target_pos) def notify_error(self, error_id, error_type): print('received error event {}({}) '.format( \ @@ -51,17 +51,19 @@ class Service(Service_Application_IF): print('received event {} {} {} {}'.format( \ event_id, event_type, subscribe_id, expire)) - def start_navi_to_user(self, dest): + def start_navi_to_user(self, event): + dest = event.args[0] print(colored("NaviToUser started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) - def start_navi_to_guide(self, dest): + def start_navi_to_guide(self, event): + dest = event.args[0] print(colored("NaviToGuide started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) - def end_service(self): + def end_service(self, event): print(colored("Route Guidance Completed !", 'cyan')) def run(self): -- GitLab From 3d09de2bec035f2a33d51db6ff951a06309e125c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 20:42:57 +0900 Subject: [PATCH 07/10] Fix type of navigation error code ERROR => ABORT --- route_guidance_ros/scripts/service_app_lv2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 5926e186..f5b54273 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -40,7 +40,7 @@ class Service(Service_Application_IF): completed_status = Completed_Status(completed_status) if completed_status == Completed_Status.OK: self.NaviSucceeded(self.dest_pos) - elif completed_status == Completed_Status.ERROR: + elif completed_status == Completed_Status.ABORT: self.NaviFailed(self.target_pos) def notify_error(self, error_id, error_type): -- GitLab From 7038a855ae70c4195971521c16bb589f3a7fc4d9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 23:37:35 +0900 Subject: [PATCH 08/10] Move init_pos_table from main_engine to VirtualNavigation --- route_guidance_ros/scripts/VirtualNavigation.py | 5 +++++ route_guidance_ros/scripts/main_engine.py | 5 ----- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 0a19ecad..5c7f88a8 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -86,6 +86,11 @@ class VirtualNavigation(Event, Command, Query): self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" + self.init_pos_table = { + 'robot1': [0.0, 0.0 ], + 'robot2': [10.0, 0.0 ], + 'robot3': [0.0, 10.0] + } self.robot_goal_table = {} self.nav_state_table = \ {robot_name : Component_Status.READY for robot_name in robot_names} diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 283f41b9..a80f77ed 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -191,11 +191,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 'robot2': EngineClient('http://localhost:8020'), 'robot3': EngineClient('http://localhost:8030') } - self.init_pos_table = { - 'robot1': (0.0, 0.0 ), - 'robot2': (10.0, 0.0 ), - 'robot3': (0.0, 10.0) - } self.goal_command_table = {} for engine_client in self.engine_clients.values(): engine_client.connect() -- GitLab From 1a7885a88b5ae2ff704e3b70b64995a474eaffcb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 10 Dec 2019 00:17:11 +0900 Subject: [PATCH 09/10] Modify VNAV to back stucked robot to init position --- route_guidance_ros/scripts/VirtualNavigation.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 5c7f88a8..e0d6d952 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# -*- coding: utf-8 -*- # VirtualNavigation.py # @@ -87,9 +88,9 @@ class VirtualNavigation(Event, Command, Query): self._component.Time_Limit = 10 self._component.Routing_Policy = "" self.init_pos_table = { - 'robot1': [0.0, 0.0 ], - 'robot2': [10.0, 0.0 ], - 'robot3': [0.0, 10.0] + 'robot1': [0.0, 0.0, 0.0], + 'robot2': [10.0, 0.0, 0.0], + 'robot3': [0.0, 10.0, 0.0] } self.robot_goal_table = {} self.nav_state_table = \ @@ -104,6 +105,14 @@ class VirtualNavigation(Event, Command, Query): return position def set_parameter(self, target_position, time_limit, routing_policy): + # ERRORなロボットは初期位置に戻す + for robot_name, state in self.nav_state_table.items(): + if state == Component_Status.ERROR: + init_pos = self.init_pos_table[robot_name] + self._component.send_goal_to_robot(init_pos, robot_name) + self.nav_state_table[robot_name] = Component_Status.BUSY + + # READYなロボットの中でゴールに最も近い位置にいるものにNavigationさせる target_coord = self.position_to_coord(target_position) nearest_robot = self._component.get_nearest_robotname(target_coord, self.nav_state_table) th = threading.Thread( -- GitLab From f537231bf7aab962cd4e7fc535486161e4a797d6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 10 Dec 2019 01:06:12 +0900 Subject: [PATCH 10/10] Update robot state when back to home is finished --- .../scripts/VirtualNavigation.py | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index e0d6d952..2cd3a62f 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -106,11 +106,7 @@ class VirtualNavigation(Event, Command, Query): def set_parameter(self, target_position, time_limit, routing_policy): # ERRORなロボットは初期位置に戻す - for robot_name, state in self.nav_state_table.items(): - if state == Component_Status.ERROR: - init_pos = self.init_pos_table[robot_name] - self._component.send_goal_to_robot(init_pos, robot_name) - self.nav_state_table[robot_name] = Component_Status.BUSY + self.back_stucked_robots_to_init_pos() # READYなロボットの中でゴールに最も近い位置にいるものにNavigationさせる target_coord = self.position_to_coord(target_position) @@ -133,6 +129,21 @@ class VirtualNavigation(Event, Command, Query): msg = xmlrpclib.dumps((goal, robot_name, status.value), 'completed') self.event_queue.put(msg) + def back_stucked_robots_to_init_pos(self): + def reset_robot_pos_and_state(robot_name): + init_pos = self.init_pos_table[robot_name] + init_pos = self.position_to_coord(init_pos) + self._component.send_goal_to_robot_async(init_pos, robot_name) + self.nav_state_table[robot_name] = Component_Status.READY + + for robot_name, state in self.nav_state_table.items(): + if state == Component_Status.ERROR: + th = threading.Thread( + target=reset_robot_pos_and_state, + args=(robot_name, )) + th.daemon = True + th.start() + def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) -- GitLab