From c9b7af293b4326e003e1618566ecfbbbe061573b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 02:40:38 +0900 Subject: [PATCH 1/8] Remove dependency between service_app and sub-tester --- route_guidance_ros/scripts/test_sub_engine_with_service.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/test_sub_engine_with_service.py b/route_guidance_ros/scripts/test_sub_engine_with_service.py index f437db9c..a0e9d53d 100644 --- a/route_guidance_ros/scripts/test_sub_engine_with_service.py +++ b/route_guidance_ros/scripts/test_sub_engine_with_service.py @@ -7,7 +7,7 @@ import time from pyrois.RoIS_Common import Component_Status from utilities import setup_single_robot from collections import namedtuple -from service_app import Service_Application_IF +from pyrois.Service_Application_IF import Service_Application_IF import xmlrpc -- GitLab From 0b9cd763777af75fb5d8f6ffb0d1905b87b30edb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 02:50:18 +0900 Subject: [PATCH 2/8] Remove unnecessary functions on service_app --- route_guidance_ros/scripts/service_app.py | 54 +---------------------- 1 file changed, 1 insertion(+), 53 deletions(-) diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py index fc0a6146..e5a6b86e 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app.py @@ -15,39 +15,8 @@ class Service_Application_IF(pyrois_Service_Application_IF): """Service_Application_IF """ def __init__(self, uri, logger=None): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) + super().__init__(uri) self.command_id_table = {} - if logger is not None: - self.logger = logger - self.th = threading.Thread(target=self.event_poll_loop) - self.th.start() - - def event_poll_loop(self): - print("poll loop started") - while True: - # try: - # self.poll_event() - self.poll_event() - # except: - # print("Error ") - # continue - - def poll_event(self): - """poll_event - """ - msg = self._proxy.poll_event() - if msg is None: - return - (params, methodname) = xmlrpc.client.loads(msg) - #self.logger.debug('poll_event: '+methodname) - print("params: ", params, ", methodname: ", methodname) - if methodname == 'completed': - self.completed(params[0], params[1]) - elif methodname == 'notify_error': - self.notify_error(params[0], params[1]) - elif methodname == 'notify_event': - self.notify_event(params[0], params[1], params[2], params[3]) def completed(self, command_id, status): self.command_id_table[command_id] = True @@ -72,24 +41,3 @@ class Service_Application_IF(pyrois_Service_Application_IF): stautus = RoIS_HRI.ReturnCode_t(return_code) self.command_id_table[command_id] = False print(self.command_id_table) - - -def example_sa_IF(url, q): - try: - logger = logging.getLogger('Service_Application_IF') - logger.setLevel(logging.DEBUG) - ch = logging.handlers.QueueHandler(q) - # ch = logging.StreamHandler() - ch.setLevel(logging.DEBUG) - formatter = logging.Formatter( - '%(asctime)s - %(name)s - %(levelname)s - %(message)s') - ch.setFormatter(formatter) - logger.addHandler(ch) - a = Service_Application_IF(url, logger=logger) - except KeyboardInterrupt: - print("Interrupted") - - -if __name__ == '__main__': - q = queue.Queue() - example_sa_IF("http://localhost:8000", q) -- GitLab From d9812ab462bf09255cc471199ea5bfcec51eec76 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 02:55:12 +0900 Subject: [PATCH 3/8] Rename service_app.py => service_app_lv1.py --- .../scripts/{service_app.py => service_app_lv1.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename route_guidance_ros/scripts/{service_app.py => service_app_lv1.py} (95%) diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app_lv1.py similarity index 95% rename from route_guidance_ros/scripts/service_app.py rename to route_guidance_ros/scripts/service_app_lv1.py index e5a6b86e..786ed248 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -11,7 +11,7 @@ import socketserver import xmlrpc.server -class Service_Application_IF(pyrois_Service_Application_IF): +class Service(pyrois_Service_Application_IF): """Service_Application_IF """ def __init__(self, uri, logger=None): -- GitLab From 74350e62f0b93dd1a90a1afaea14c129d2a8b41c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 19:05:41 +0900 Subject: [PATCH 4/8] Make base of task manage system --- route_guidance_ros/scripts/service_app_lv1.py | 48 ++++++++++++++++--- 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index 786ed248..a00e0757 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -1,26 +1,48 @@ from main_engine_client import IF as RoIS_IF from pyrois.Service_Application_IF import Service_Application_IF as pyrois_Service_Application_IF from pyrois import RoIS_Service, RoIS_HRI +from utilities import setup_multi_robot import logging import queue import threading import time +import enum import socketserver import xmlrpc.server +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, is_finished=False): + self.task_kind = task_kind + self.is_finished = is_finished + + @staticmethod + def next(current_task): + return TaskState.__task_seq_table[current_task] + class Service(pyrois_Service_Application_IF): """Service_Application_IF """ - def __init__(self, uri, logger=None): + def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) - self.command_id_table = {} + self.commandid_taskstate_table = {} def completed(self, command_id, status): - self.command_id_table[command_id] = True - print(self.command_id_table) + self.commandid_taskstate_table[command_id] = True + print(self.commandid_taskstate_table) def notify_error(self, error_id, error_type): print('received error event {}({}) ', @@ -37,7 +59,19 @@ class Service(pyrois_Service_Application_IF): def go_robot_to(self, dest): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) - (return_code, command_id) = self._proxy.set_parameter('Navigation', [dest, "", ""]) + (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) - self.command_id_table[command_id] = False - print(self.command_id_table) + self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToUser) + print(self.commandid_taskstate_table) + + def run(self): + target_point = "point_c" + self.go_robot_to(target_point) + +if __name__ == '__main__': + process = setup_multi_robot() + time.sleep(5) + service = Service() + print("Starting service..") + service.run() + print("Finish.") -- GitLab From 66a81b52fae9828e80bddb7c6b97aa04309ec8b6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 20:58:16 +0900 Subject: [PATCH 5/8] Complete route-guidance level1 --- route_guidance_ros/scripts/service_app_lv1.py | 31 ++++++++++++++----- 1 file changed, 23 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index a00e0757..41ef2b4b 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -28,21 +28,28 @@ class TaskState(object): self.task_kind = task_kind self.is_finished = is_finished - @staticmethod - def next(current_task): - return TaskState.__task_seq_table[current_task] + def __repr__(self): + return "task_kind: {}, is_finished: {}".format(self.task_kind, self.is_finished) + + def next(self): + return TaskState.__task_seq_table[self.task_kind] class Service(pyrois_Service_Application_IF): - """Service_Application_IF - """ def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) + self.target_pos = "point_c" + self.dest_pos = "point_a" self.commandid_taskstate_table = {} def completed(self, command_id, status): - self.commandid_taskstate_table[command_id] = True + task = self.commandid_taskstate_table[command_id] print(self.commandid_taskstate_table) + next_task = task.next() + if next_task == TaskKind.NaviToGuide: + self.start_navi_to_guide(self.dest_pos) + elif next_task == TaskKind.End: + print("Route Guide Finished !!") def notify_error(self, error_id, error_type): print('received error event {}({}) ', @@ -56,7 +63,7 @@ class Service(pyrois_Service_Application_IF): subscribe_id, expire) - def go_robot_to(self, dest): + def start_navi_to_user(self, dest): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") @@ -64,9 +71,17 @@ class Service(pyrois_Service_Application_IF): self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToUser) print(self.commandid_taskstate_table) + def start_navi_to_guide(self, dest): + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + (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) + print(self.commandid_taskstate_table) + def run(self): target_point = "point_c" - self.go_robot_to(target_point) + self.start_navi_to_user(target_point) if __name__ == '__main__': process = setup_multi_robot() -- GitLab From 31de1351987ad84fed2dd403df40270a9e642c02 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 21:30:32 +0900 Subject: [PATCH 6/8] Refactor service_app_lv1 --- route_guidance_ros/scripts/service_app_lv1.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index 41ef2b4b..b0050bf5 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -2,6 +2,7 @@ from main_engine_client import IF as RoIS_IF from pyrois.Service_Application_IF import Service_Application_IF as pyrois_Service_Application_IF from pyrois import RoIS_Service, RoIS_HRI from utilities import setup_multi_robot +# from pyrois.RoIS_Common import Component_Status import logging import queue @@ -29,7 +30,7 @@ class TaskState(object): self.is_finished = is_finished def __repr__(self): - return "task_kind: {}, is_finished: {}".format(self.task_kind, self.is_finished) + return "".format(self.task_kind, self.is_finished) def next(self): return TaskState.__task_seq_table[self.task_kind] @@ -44,8 +45,8 @@ class Service(pyrois_Service_Application_IF): def completed(self, command_id, status): task = self.commandid_taskstate_table[command_id] - print(self.commandid_taskstate_table) next_task = task.next() + print("status is ", status) if next_task == TaskKind.NaviToGuide: self.start_navi_to_guide(self.dest_pos) elif next_task == TaskKind.End: @@ -64,25 +65,24 @@ class Service(pyrois_Service_Application_IF): expire) def start_navi_to_user(self, dest): - self._proxy = xmlrpc.client.ServerProxy(self._uri) - time.sleep(3) (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) print(self.commandid_taskstate_table) def start_navi_to_guide(self, dest): - self._proxy = xmlrpc.client.ServerProxy(self._uri) - time.sleep(3) (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) print(self.commandid_taskstate_table) def run(self): + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) target_point = "point_c" self.start_navi_to_user(target_point) + if __name__ == '__main__': process = setup_multi_robot() time.sleep(5) -- GitLab From 80f89477c9eee159a79e319250b2f799d8776fb5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 22:06:43 +0900 Subject: [PATCH 7/8] Use Component_Status to manage task --- route_guidance_ros/scripts/service_app_lv1.py | 28 +++++++++---------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index b0050bf5..a532d8ea 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -2,7 +2,7 @@ from main_engine_client import IF as RoIS_IF from pyrois.Service_Application_IF import Service_Application_IF as pyrois_Service_Application_IF from pyrois import RoIS_Service, RoIS_HRI from utilities import setup_multi_robot -# from pyrois.RoIS_Common import Component_Status +from pyrois.RoIS_Common import Component_Status import logging import queue @@ -18,6 +18,7 @@ class TaskKind(enum.Enum): NaviToGuide = 2 End = 3 + class TaskState(object): __task_seq_table = { TaskKind.NaviToUser: TaskKind.NaviToGuide, @@ -25,12 +26,12 @@ class TaskState(object): TaskKind.End: TaskKind.End } - def __init__(self, task_kind, is_finished=False): + def __init__(self, task_kind, status=Component_Status.BUSY): self.task_kind = task_kind - self.is_finished = is_finished + self.status = status def __repr__(self): - return "".format(self.task_kind, self.is_finished) + return "".format(self.task_kind, self.status) def next(self): return TaskState.__task_seq_table[self.task_kind] @@ -45,36 +46,33 @@ class Service(pyrois_Service_Application_IF): def completed(self, command_id, status): task = self.commandid_taskstate_table[command_id] + status = Component_Status(status) + task.status = status next_task = task.next() - print("status is ", status) if next_task == TaskKind.NaviToGuide: self.start_navi_to_guide(self.dest_pos) elif next_task == TaskKind.End: print("Route Guide Finished !!") + self.commandid_taskstate_table[command_id] = task + print(self.commandid_taskstate_table) def notify_error(self, error_id, error_type): - print('received error event {}({}) ', - error_id, - RoIS_Service.ErrorType(error_type).name) + print('received error event {}({}) '.format( \ + error_id, RoIS_Service.ErrorType(error_type).name)) def notify_event(self, event_id, event_type, subscribe_id, expire): - print('received event {} {} {} {}', - event_id, - event_type, - subscribe_id, - expire) + print('received event {} {} {} {}'.format( \ + event_id, event_type, subscribe_id, expire)) def start_navi_to_user(self, dest): (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) - print(self.commandid_taskstate_table) def start_navi_to_guide(self, dest): (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) - print(self.commandid_taskstate_table) def run(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) -- GitLab From 9e5301c00ffe10aca695f451281ac0ee2048d14e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 6 Dec 2019 15:38:08 +0900 Subject: [PATCH 8/8] Refactor importing mudules on service_app_lv1 --- route_guidance_ros/scripts/service_app_lv1.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index a532d8ea..5a220f81 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -1,5 +1,4 @@ -from main_engine_client import IF as RoIS_IF -from pyrois.Service_Application_IF import Service_Application_IF as pyrois_Service_Application_IF +from pyrois.Service_Application_IF import Service_Application_IF from pyrois import RoIS_Service, RoIS_HRI from utilities import setup_multi_robot from pyrois.RoIS_Common import Component_Status @@ -9,9 +8,8 @@ import queue import threading import time import enum +import xmlrpc -import socketserver -import xmlrpc.server class TaskKind(enum.Enum): NaviToUser = 1 @@ -37,7 +35,7 @@ class TaskState(object): return TaskState.__task_seq_table[self.task_kind] -class Service(pyrois_Service_Application_IF): +class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) self.target_pos = "point_c" -- GitLab