diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py deleted file mode 100644 index fc0a61466d5292eba2167273a070718442cbce40..0000000000000000000000000000000000000000 --- a/route_guidance_ros/scripts/service_app.py +++ /dev/null @@ -1,95 +0,0 @@ -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 - -import logging -import queue -import threading -import time - -import socketserver -import xmlrpc.server - - -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) - 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 - print(self.command_id_table) - - def notify_error(self, error_id, error_type): - print('received error event {}({}) ', - 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) - - 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, "", ""]) - 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) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py new file mode 100644 index 0000000000000000000000000000000000000000..5a220f816617ce71873608e6f9505e8d548de31f --- /dev/null +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -0,0 +1,88 @@ +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 + +import logging +import queue +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=Component_Status.BUSY): + 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] + + +class Service(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): + task = self.commandid_taskstate_table[command_id] + status = Component_Status(status) + task.status = status + 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 !!") + 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( \ + error_id, RoIS_Service.ErrorType(error_type).name)) + + def notify_event(self, 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) + + 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) + + 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) + service = Service() + print("Starting service..") + service.run() + print("Finish.") 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 f437db9cb8007c0ff1b813ac93544cd304ba59de..a0e9d53db73a7c1cf48322845a364a1e106a8672 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