From e008f9b2c29f238f1eeb35126865aa038f96b906 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 13 Nov 2019 14:05:40 +0900 Subject: [PATCH 01/52] Add script of service application --- route_guidance_ros/scripts/service_app.py | 81 +++++++++++++++++++++++ 1 file changed, 81 insertions(+) create mode 100644 route_guidance_ros/scripts/service_app.py diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py new file mode 100644 index 00000000..479b7d74 --- /dev/null +++ b/route_guidance_ros/scripts/service_app.py @@ -0,0 +1,81 @@ +from toplevel_engine_client import IF as RoIS_IF +from pyrois.Service_Application_Base_example import Service_Application_Base as Example_Service_App_Base +from pyrois import RoIS_Service + +import queue +import threading +import time + +import socketserver +import xmlrpc.server + +class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + + +class Service_Application_Base(Example_Service_App_Base): + """Service_Application_Base + """ + def __init__(self): + self.event_queue = queue.Queue() + + def poll_event(self): + """poll_event + """ + msg = self.event_queue.get() + return msg + + def completed(self, command_id, status): + msg = xmlrpc.client.dumps((command_id, status), 'completed') + self.event_queue.put(msg) + + def notify_error(self, error_id, error_type): + msg = xmlrpc.client.dumps((error_id, error_type), 'notify_error') + self.event_queue.put(msg) + + def notify_event(self, event_id, event_type, subscribe_id, expire): + msg = xmlrpc.client.dumps( + (event_id, event_type, subscribe_id, expire), 'notify_event') + self.event_queue.put(msg) + + +def event_dispatch(sa): + """event_dispatch + """ + sa.completed("0", RoIS_Service.Completed_Status.OK.value) + time.sleep(0.1) + sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) + time.sleep(0.2) + sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") + + +# def event_dispatch_long(sa): +# sa.completed("0", RoIS_Service.Completed_Status.OK.value) +# time.sleep(1) +# sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) +# time.sleep(60) +# sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") + +def example_sa(port): + """example_sa + """ + sa = Service_Application_Base() + print("Service_Application built.") + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(sa,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(sa) + server.register_introspection_functions() + server.register_multicall_functions() + # print("server running") + server.serve_forever() + + +if __name__ == '__main__': + example_sa(8000) -- GitLab From 7f2cd5911336050db2699dee8f6327d04e288c6e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 13 Nov 2019 15:11:35 +0900 Subject: [PATCH 02/52] Add simple event system --- route_guidance_ros/scripts/service_app.py | 109 +++++++++--------- .../scripts/test_toplevel_engine.py | 4 + route_guidance_ros/scripts/toplevel_engine.py | 27 ++--- .../scripts/toplevel_engine_client.py | 7 +- 4 files changed, 74 insertions(+), 73 deletions(-) diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py index 479b7d74..0448c3c1 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app.py @@ -1,7 +1,8 @@ from toplevel_engine_client import IF as RoIS_IF -from pyrois.Service_Application_Base_example import Service_Application_Base as Example_Service_App_Base +from pyrois.Service_Application_IF import Service_Application_IF as pyrois_Service_Application_IF from pyrois import RoIS_Service +import logging import queue import threading import time @@ -9,73 +10,69 @@ import time import socketserver import xmlrpc.server -class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): - """ThreadingXMLRPCServer - """ - pass - -class Service_Application_Base(Example_Service_App_Base): - """Service_Application_Base +class Service_Application_IF(RoIS_Service.Service_Application_Base): + """Service_Application_IF """ - def __init__(self): - self.event_queue = queue.Queue() + def __init__(self, uri, logger=None): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + if logger is not None: + self.logger = logger + while True: + self.poll_event() def poll_event(self): """poll_event """ - msg = self.event_queue.get() - return msg + print("poll") + msg = self._proxy.poll_event() + if msg is None: + return + (params, methodname) = xmlrpc.client.loads(msg) + #self.logger.debug('poll_event: '+methodname) + # print(params,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): - msg = xmlrpc.client.dumps((command_id, status), 'completed') - self.event_queue.put(msg) + print('received completed event {} with status {}.', + command_id, + RoIS_Service.Completed_Status(status).name) def notify_error(self, error_id, error_type): - msg = xmlrpc.client.dumps((error_id, error_type), 'notify_error') - self.event_queue.put(msg) + print('received error event {}({}) ', + error_id, + RoIS_Service.ErrorType(error_type).name) def notify_event(self, event_id, event_type, subscribe_id, expire): - msg = xmlrpc.client.dumps( - (event_id, event_type, subscribe_id, expire), 'notify_event') - self.event_queue.put(msg) - - -def event_dispatch(sa): - """event_dispatch - """ - sa.completed("0", RoIS_Service.Completed_Status.OK.value) - time.sleep(0.1) - sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) - time.sleep(0.2) - sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") - - -# def event_dispatch_long(sa): -# sa.completed("0", RoIS_Service.Completed_Status.OK.value) -# time.sleep(1) -# sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) -# time.sleep(60) -# sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") - -def example_sa(port): - """example_sa - """ - sa = Service_Application_Base() - print("Service_Application built.") - - # start the timer to dispatch events - t = threading.Timer(0.1, event_dispatch, args=(sa,)) - t.start() - - # start the XML-RPC server - server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) - server.register_instance(sa) - server.register_introspection_functions() - server.register_multicall_functions() - # print("server running") - server.serve_forever() + print('received event {} {} {} {}', + event_id, + event_type, + subscribe_id, + expire) + + +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__': - example_sa(8000) + q = queue.Queue() + example_sa_IF("http://localhost:8000", q) diff --git a/route_guidance_ros/scripts/test_toplevel_engine.py b/route_guidance_ros/scripts/test_toplevel_engine.py index 36e2654a..809a6719 100644 --- a/route_guidance_ros/scripts/test_toplevel_engine.py +++ b/route_guidance_ros/scripts/test_toplevel_engine.py @@ -10,11 +10,15 @@ from utilities import setup_multi_robot process = setup_multi_robot() engine = Engine('http://127.0.0.1:8000') +print("Engine was established.") +time.sleep(10) engine.connect() for dest in [[25, 0], [0, 20], [5, 0]]: status = engine.set_parameter('Navigation', [dest, "", ""]) print(status) time.sleep(5) +engine.all_finished() +time.sleep(10) engine.disconnect() print("finish") diff --git a/route_guidance_ros/scripts/toplevel_engine.py b/route_guidance_ros/scripts/toplevel_engine.py index 77b280b1..01cc07b3 100644 --- a/route_guidance_ros/scripts/toplevel_engine.py +++ b/route_guidance_ros/scripts/toplevel_engine.py @@ -14,9 +14,11 @@ from __future__ import print_function import os import sys +import queue import numpy as np from pyrois import RoIS_HRI +from pyrois.Service_Application_Base_example import Service_Application_Base if sys.version_info.major == 2: import SocketServer @@ -168,12 +170,13 @@ class EventIF(RoIS_HRI.EventIF): from VirtualNavigation_client import VirtualNavigation_Client as VNavClient from sub_engine_client import IF as EngineClient -class IF(SystemIF, CommandIF, QueryIF, EventIF): +class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): """IF """ def __init__(self, Engine): super().__init__(Engine) self.command_id = 0 + self.event_queue = queue.Queue() self.component_clients = { 'Navigation': VNavClient('http://localhost:8041') } @@ -192,21 +195,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): self.command_id += 1 return (status, str(self.command_id)) - # def set_parameter(self, command_ref, parameters): - # dest = np.array(parameters[0]) - # robot_distance_table = {} - # for name, engine in self.engine_clients.items(): - # pos = engine.get_position()[3] - # pos = np.array(pos) - # robot_distance_table[name] = np.linalg.norm(dest - pos) - # nearest_robot = min(robot_distance_table, key=robot_distance_table.get) - # print("Nearest robot is '{}'".format(nearest_robot)) - - # target_engine = self.engine_clients[nearest_robot] - # target_engine.set_parameter(command_ref, parameters) - # status = RoIS_HRI.ReturnCode_t.OK.value - # self.command_id += 1 - # return (status, str(self.command_id)) + + def all_finished(self): + self.completed("1", 1) + self.completed("2", 1) + self.completed("3", 1) + status = RoIS_HRI.ReturnCode_t.OK.value + return (status, 1) class IF_server: diff --git a/route_guidance_ros/scripts/toplevel_engine_client.py b/route_guidance_ros/scripts/toplevel_engine_client.py index b522efb7..446dbf01 100644 --- a/route_guidance_ros/scripts/toplevel_engine_client.py +++ b/route_guidance_ros/scripts/toplevel_engine_client.py @@ -12,6 +12,7 @@ import xmlrpc.client from pyrois import RoIS_HRI +from pyrois.Service_Application_IF import Service_Application_IF class SystemIF(RoIS_HRI.SystemIF): @@ -117,12 +118,16 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) -class IF(SystemIF, CommandIF, QueryIF, EventIF): +class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF): """IF """ def __init__(self, uri): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) + def all_finished(self): + (command_id, status) = self._proxy.all_finished() + + if __name__ == "__main__": pass -- GitLab From 648a0087263ef4eb4432aa59b615a5712f7e47c4 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 15 Nov 2019 15:52:01 +0900 Subject: [PATCH 03/52] Rename toplevel => main --- .../scripts/{toplevel_engine.py => main_engine.py} | 0 .../{toplevel_engine_client.py => main_engine_client.py} | 0 route_guidance_ros/scripts/service_app.py | 2 +- .../{test_toplevel_engine.py => test_main_engine.py} | 2 +- route_guidance_ros/scripts/utilities.py | 8 ++++---- 5 files changed, 6 insertions(+), 6 deletions(-) rename route_guidance_ros/scripts/{toplevel_engine.py => main_engine.py} (100%) rename route_guidance_ros/scripts/{toplevel_engine_client.py => main_engine_client.py} (100%) rename route_guidance_ros/scripts/{test_toplevel_engine.py => test_main_engine.py} (91%) diff --git a/route_guidance_ros/scripts/toplevel_engine.py b/route_guidance_ros/scripts/main_engine.py similarity index 100% rename from route_guidance_ros/scripts/toplevel_engine.py rename to route_guidance_ros/scripts/main_engine.py diff --git a/route_guidance_ros/scripts/toplevel_engine_client.py b/route_guidance_ros/scripts/main_engine_client.py similarity index 100% rename from route_guidance_ros/scripts/toplevel_engine_client.py rename to route_guidance_ros/scripts/main_engine_client.py diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py index 0448c3c1..46016b37 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app.py @@ -1,4 +1,4 @@ -from toplevel_engine_client import IF as RoIS_IF +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 diff --git a/route_guidance_ros/scripts/test_toplevel_engine.py b/route_guidance_ros/scripts/test_main_engine.py similarity index 91% rename from route_guidance_ros/scripts/test_toplevel_engine.py rename to route_guidance_ros/scripts/test_main_engine.py index 809a6719..7b153fdc 100644 --- a/route_guidance_ros/scripts/test_toplevel_engine.py +++ b/route_guidance_ros/scripts/test_main_engine.py @@ -2,7 +2,7 @@ # 2. run this test. -from toplevel_engine_client import IF as Engine +from main_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status from utilities import setup_multi_robot diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 0a3eb229..416dd54b 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -30,9 +30,9 @@ class SubEngineWrapper(SubprocessWrapper): super().__init__(["python3", "sub_engine.py", str(engine_port)]) -class ToplevelEngineWrapper(SubprocessWrapper): +class MainEngineWrapper(SubprocessWrapper): def __init__(self): - super().__init__(["python3", "toplevel_engine.py"]) + super().__init__(["python3", "main_engine.py"]) def launch_components_and_subengines(**robot_port_table): @@ -57,8 +57,8 @@ def setup_single_robot(): def setup_multi_robot(): process = launch_components_and_subengines(robot1=8010, robot2=8020, robot3=8030) - process.append(ToplevelEngineWrapper()) - print("Toplevel Engine Constructed.") + process.append(MainEngineWrapper()) + print("Main Engine Constructed.") time.sleep(5) command = [ "rosrun", "route_guidance_ros", "VirtualNavigation.py", -- GitLab From 6b2b11344d01dde9f30a1163cfc0f007b0970a2f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 16 Nov 2019 19:24:44 +0900 Subject: [PATCH 04/52] Add event pooling system --- route_guidance_ros/scripts/Navigation.py | 7 +-- .../scripts/Navigation_client.py | 43 ++++++++++--------- 2 files changed, 26 insertions(+), 24 deletions(-) diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index fb38f75c..b754bf1c 100755 --- a/route_guidance_ros/scripts/Navigation.py +++ b/route_guidance_ros/scripts/Navigation.py @@ -69,9 +69,10 @@ class Event(RoIS_Common.Event): self._component = c self.event_queue = queue.Queue() - # def poll_event(self): - # msg = self.event_queue.get() - # return msg + def poll_event(self): + print("Navi Server") + msg = self.event_queue.get() + return msg from goal_sender_ros import GoalSenderROS diff --git a/route_guidance_ros/scripts/Navigation_client.py b/route_guidance_ros/scripts/Navigation_client.py index fc616a48..281ef6bb 100644 --- a/route_guidance_ros/scripts/Navigation_client.py +++ b/route_guidance_ros/scripts/Navigation_client.py @@ -72,26 +72,27 @@ class Event(RoIS_Common.Event): # if logger is not None: # self.logger = logger - # def start_th(self): - # self.th = threading.Thread(target=self.event_loop) - # self.th.start() - - # def event_loop(self): - # """event_loop - # """ - # while True: - # try: - # self.poll_event() - # except ConnectionRefusedError: - # break - - # def poll_event(self): - # """poll_event - # """ - # msg = self._e_proxy.poll_event() - # (params, methodname) = xmlrpc.client.loads(msg) - # #self.logger.debug('poll_event: '+methodname) - # print(params,methodname) + def start_th(self): + self.th = threading.Thread(target=self.event_loop) + self.th.start() + + def event_loop(self): + """event_loop + """ + while True: + print("Navigation_Client event_loop") + try: + self.poll_event() + except ConnectionRefusedError: + break + + def poll_event(self): + """poll_event + """ + msg = self._e_proxy.poll_event() + (params, methodname) = xmlrpc.client.loads(msg) + #self.logger.debug('poll_event: '+methodname) + print(params,methodname) # if methodname == 'speech_recognized': # self.speech_recognized(params[0], params[1]) @@ -104,4 +105,4 @@ class Navigation_Client(Command, Query, Event): self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] - # self.start_th() + self.start_th() -- GitLab From e7c018d5151b8fd0bc302cdc0a8dd651e793a347 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 16 Nov 2019 20:06:16 +0900 Subject: [PATCH 05/52] Add sample event action on Navigation component --- route_guidance_ros/scripts/Navigation.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index b754bf1c..bf3213c8 100755 --- a/route_guidance_ros/scripts/Navigation.py +++ b/route_guidance_ros/scripts/Navigation.py @@ -75,6 +75,7 @@ class Event(RoIS_Common.Event): return msg from goal_sender_ros import GoalSenderROS +import xmlrpclib class Navigation(Event, Command, Query): def __init__(self, c, robot_name): @@ -86,6 +87,10 @@ class Navigation(Event, Command, Query): self._component.Routing_Policy = "" def set_parameter(self, target_position, time_limit, routing_policy): + # Put event + msg = xmlrpclib.dumps(("AHI", "MOI"), 'set_parameter') + self.event_queue.put(msg) + self._goal_sender.send_goal(target_position) status = RoIS_HRI.ReturnCode_t.OK.value return status -- GitLab From 1d84b86f58333458e085e5ad63c7c5b3eb9cffa6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 17 Nov 2019 00:08:05 +0900 Subject: [PATCH 06/52] Upgrade Navigation component to send nav-finish-event --- route_guidance_ros/scripts/Navigation.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index bf3213c8..d11df496 100755 --- a/route_guidance_ros/scripts/Navigation.py +++ b/route_guidance_ros/scripts/Navigation.py @@ -77,6 +77,8 @@ class Event(RoIS_Common.Event): from goal_sender_ros import GoalSenderROS import xmlrpclib +from pyrois.RoIS_Common import Component_Status + class Navigation(Event, Command, Query): def __init__(self, c, robot_name): super(Navigation, self).__init__(c) @@ -85,11 +87,12 @@ class Navigation(Event, Command, Query): self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" + self.latest_nav_state = Component_Status.UNINITIALIZED + self.start_th() def set_parameter(self, target_position, time_limit, routing_policy): # Put event - msg = xmlrpclib.dumps(("AHI", "MOI"), 'set_parameter') - self.event_queue.put(msg) + self.latest_nav_state = Component_Status.BUSY self._goal_sender.send_goal(target_position) status = RoIS_HRI.ReturnCode_t.OK.value @@ -100,6 +103,19 @@ class Navigation(Event, Command, Query): c_status = self._goal_sender.get_goalsender_state().value return (status, c_status) + def start_th(self): + self.th = threading.Thread(target=self.nav_finish_event_loop) + self.th.start() + + def nav_finish_event_loop(self): + while True: + old_nav_state = self.latest_nav_state + self.latest_nav_state = self._goal_sender.get_goalsender_state() + if old_nav_state is Component_Status.BUSY and \ + self.latest_nav_state != old_nav_state: + msg = xmlrpclib.dumps(("AHI", "MOI"), 'set_parameter') + self.event_queue.put(msg) + def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) -- GitLab From 271fc3b6761430e8819a91b2a0259170cbca900d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 17 Nov 2019 16:44:34 +0900 Subject: [PATCH 07/52] Modify VirtualNavigation to send nav-finish event --- .../scripts/VirtualNavigation.py | 38 ++++++++++++++-- .../scripts/VirtualNavigation_client.py | 45 ++++++++++--------- .../scripts/test_main_engine.py | 2 +- 3 files changed, 60 insertions(+), 25 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 056888be..c5f6e36a 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -69,11 +69,15 @@ class Event(RoIS_Common.Event): self._component = c self.event_queue = queue.Queue() - # def poll_event(self): - # msg = self.event_queue.get() - # return msg + def poll_event(self): + print("!!!!!!!!!!!!!!!!polled!!!!!!!!!!!!!!!!!!!!!!!!!") + msg = self.event_queue.get() + return msg from goal_sender_ros import GoalSenderROS +from pyrois.RoIS_Common import Component_Status + +import xmlrpclib class VirtualNavigation(Event, Command, Query): def __init__(self, c, robot_names): @@ -82,8 +86,12 @@ class VirtualNavigation(Event, Command, Query): self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" + self.nav_state_table = \ + {robot_name : Component_Status.BUSY for robot_name in robot_names} + self.start_th() def set_parameter(self, target_position, time_limit, routing_policy): + print("VirtualNavigation::set_parameter") self._component.send_goal_to_nearest_robot(target_position) status = RoIS_HRI.ReturnCode_t.OK.value return status @@ -93,6 +101,26 @@ class VirtualNavigation(Event, Command, Query): c_status = self._goal_sender.get_goalsender_state().value return (status, c_status) + def start_th(self): + self.th = threading.Thread(target=self.nav_finish_event_loop) + self.th.start() + + def nav_finish_event_loop(self): + while True: + for robot_name, old_state in self.nav_state_table.items(): + current_state = self._component.get_nav_state(robot_name) + self.nav_state_table[robot_name] = current_state + if current_state != old_state and \ + old_state is Component_Status.BUSY: + print("EVENT WOWOWOWOOWOW") + msg = xmlrpclib.dumps(("AHI", "End"), 'set_param') + self.event_queue.put(msg) + + # unnecessary ? + def poll_event(self): + print("!!!!!!!!!!!!!!!!polled!!!!!!!!!!!!!!!!!!!!!!!!!") + msg = self.event_queue.get() + return msg def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) @@ -129,7 +157,11 @@ class Component(object): nearest_robot = min(distance_table, key=distance_table.get) print("nearest_robot is ", nearest_robot) self.__goal_senders[nearest_robot].send_goal(dest.tolist()) + return nearest_robot + def get_nav_state(self, robot_name): + goal_sender = self.__goal_senders[robot_name] + return goal_sender.get_goalsender_state() def example_n(port, robot_names): print("Starting node...") diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index be14ab33..1e4d0a86 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -41,6 +41,7 @@ class Command(RoIS_Common.Command): return status def set_parameter(self, target_position, time_limit, routing_policy): + print("VirtualNavigationClient::set_parameter") status = self._proxy.set_parameter(target_position, time_limit, routing_policy) status = RoIS_HRI.ReturnCode_t(status) return status @@ -72,26 +73,28 @@ class Event(RoIS_Common.Event): # if logger is not None: # self.logger = logger - # def start_th(self): - # self.th = threading.Thread(target=self.event_loop) - # self.th.start() - - # def event_loop(self): - # """event_loop - # """ - # while True: - # try: - # self.poll_event() - # except ConnectionRefusedError: - # break - - # def poll_event(self): - # """poll_event - # """ - # msg = self._e_proxy.poll_event() - # (params, methodname) = xmlrpc.client.loads(msg) - # #self.logger.debug('poll_event: '+methodname) - # print(params,methodname) + def start_th(self): + self.th = threading.Thread(target=self.event_loop) + self.th.start() + + def event_loop(self): + """event_loop + """ + while True: + try: + self.poll_event() + except ConnectionRefusedError: + break + + def poll_event(self): + """poll_event + """ + print("Waiting for msg") + msg = self._e_proxy.poll_event() + print("Caught msg:", msg) + (params, methodname) = xmlrpc.client.loads(msg) + #self.logger.debug('poll_event: '+methodname) + print("VirtualNavigation poll_event", params,methodname) # if methodname == 'speech_recognized': # self.speech_recognized(params[0], params[1]) @@ -104,4 +107,4 @@ class VirtualNavigation_Client(Command, Query, Event): self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] - # self.start_th() + self.start_th() diff --git a/route_guidance_ros/scripts/test_main_engine.py b/route_guidance_ros/scripts/test_main_engine.py index 7b153fdc..c40dc793 100644 --- a/route_guidance_ros/scripts/test_main_engine.py +++ b/route_guidance_ros/scripts/test_main_engine.py @@ -17,7 +17,7 @@ for dest in [[25, 0], [0, 20], [5, 0]]: status = engine.set_parameter('Navigation', [dest, "", ""]) print(status) time.sleep(5) -engine.all_finished() +# engine.all_finished() time.sleep(10) engine.disconnect() -- GitLab From ae1de49ca52540232473aae59d796ddff0fc9251 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 18 Nov 2019 13:47:25 +0900 Subject: [PATCH 08/52] Modify navigation-finish event message --- route_guidance_ros/scripts/Navigation.py | 2 +- route_guidance_ros/scripts/VirtualNavigation.py | 6 +++--- route_guidance_ros/scripts/test_main_engine.py | 2 +- route_guidance_ros/scripts/test_sub_engine.py | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index d11df496..0e53006f 100755 --- a/route_guidance_ros/scripts/Navigation.py +++ b/route_guidance_ros/scripts/Navigation.py @@ -113,7 +113,7 @@ class Navigation(Event, Command, Query): self.latest_nav_state = self._goal_sender.get_goalsender_state() if old_nav_state is Component_Status.BUSY and \ self.latest_nav_state != old_nav_state: - msg = xmlrpclib.dumps(("AHI", "MOI"), 'set_parameter') + msg = xmlrpclib.dumps(("Navigation", "Finished"), 'set_parameter') self.event_queue.put(msg) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index c5f6e36a..a7148faa 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -87,7 +87,7 @@ class VirtualNavigation(Event, Command, Query): self._component.Time_Limit = 10 self._component.Routing_Policy = "" self.nav_state_table = \ - {robot_name : Component_Status.BUSY for robot_name in robot_names} + {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} self.start_th() def set_parameter(self, target_position, time_limit, routing_policy): @@ -112,8 +112,8 @@ class VirtualNavigation(Event, Command, Query): self.nav_state_table[robot_name] = current_state if current_state != old_state and \ old_state is Component_Status.BUSY: - print("EVENT WOWOWOWOOWOW") - msg = xmlrpclib.dumps(("AHI", "End"), 'set_param') + print("Adding new EVENT") + msg = xmlrpclib.dumps(("Nav", "Fin"), 'set_param') self.event_queue.put(msg) # unnecessary ? diff --git a/route_guidance_ros/scripts/test_main_engine.py b/route_guidance_ros/scripts/test_main_engine.py index c40dc793..c00e159b 100644 --- a/route_guidance_ros/scripts/test_main_engine.py +++ b/route_guidance_ros/scripts/test_main_engine.py @@ -19,6 +19,6 @@ for dest in [[25, 0], [0, 20], [5, 0]]: time.sleep(5) # engine.all_finished() -time.sleep(10) +time.sleep(30) engine.disconnect() print("finish") diff --git a/route_guidance_ros/scripts/test_sub_engine.py b/route_guidance_ros/scripts/test_sub_engine.py index a8724fc6..46664c3a 100644 --- a/route_guidance_ros/scripts/test_sub_engine.py +++ b/route_guidance_ros/scripts/test_sub_engine.py @@ -18,7 +18,7 @@ for dest in destinations: navi_status = Component_Status.BUSY while navi_status != Component_Status.READY: server_status, navi_status = engine.get_navi_status() - print("TEST: status = {}".format(navi_status)) + # print("TEST: status = {}".format(navi_status)) time.sleep(1) position = engine.get_position() print("TEST: position = {}".format(position)) -- GitLab From 5dcd81301ba1b95fba527af76efda21a5cce6492 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 18 Nov 2019 16:27:54 +0900 Subject: [PATCH 09/52] Launch VirtualNavigation server before main_engine --- route_guidance_ros/scripts/utilities.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 416dd54b..8ff7edbd 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -57,15 +57,15 @@ def setup_single_robot(): def setup_multi_robot(): process = launch_components_and_subengines(robot1=8010, robot2=8020, robot3=8030) - process.append(MainEngineWrapper()) - print("Main Engine Constructed.") - time.sleep(5) command = [ "rosrun", "route_guidance_ros", "VirtualNavigation.py", "8041", "robot1", "robot2", "robot3" ] process.append(SubprocessWrapper(command)) print("Virtual Navigation Component Constructed.") + time.sleep(5) + process.append(MainEngineWrapper()) + print("Main Engine Constructed.") time.sleep(10) return process -- GitLab From 052825875638bcc0b97f58876e47a783da0a2a01 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 18 Nov 2019 16:32:02 +0900 Subject: [PATCH 10/52] Fix event_polling loop --- route_guidance_ros/scripts/VirtualNavigation_client.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index 1e4d0a86..bd5bb602 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -62,6 +62,7 @@ class Query(RoIS_Common.Query): (status, target_position, time_limit, routing_policy) = self._proxy.get_parameter() return (RoIS_HRI.ReturnCode_t(status), target_position, time_limit, routing_policy) +import time class Event(RoIS_Common.Event): """Event @@ -81,10 +82,15 @@ class Event(RoIS_Common.Event): """event_loop """ while True: + time.sleep(1) try: self.poll_event() - except ConnectionRefusedError: - break + except xmlrpc.client.ProtocolError as e: + print("ProtocolError", e) + continue + except ConnectionRefusedError as e: + print("ConnectionRefusedError ", e) + continue def poll_event(self): """poll_event -- GitLab From 5abd1c3709130d4232551a57d11b9268441d8aea Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 19 Nov 2019 13:29:09 +0900 Subject: [PATCH 11/52] Add initial positions of robots --- route_guidance_ros/scripts/main_engine.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 01cc07b3..b345c4d7 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -185,6 +185,11 @@ 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) + } for engine_client in self.engine_clients.values(): engine_client.connect() -- GitLab From 05625c0eeab1daae29010f25488ec18581f5f677 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 19 Nov 2019 14:08:58 +0900 Subject: [PATCH 12/52] Add event callback function to send event to main-engine --- .../scripts/VirtualNavigation_client.py | 13 ++++++++++++- route_guidance_ros/scripts/main_engine.py | 2 +- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index bd5bb602..da6c6eac 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -108,9 +108,20 @@ class Event(RoIS_Common.Event): class VirtualNavigation_Client(Command, Query, Event): """VirtualNavigation_Client """ - def __init__(self, uri): + def __init__(self, uri, event_cb): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] + self.event_cb = event_cb self.start_th() + + def poll_event(self): + """poll_event + """ + print("Waiting for msg") + msg = self._e_proxy.poll_event() + print("Caught msg:", msg) + (params, methodname) = xmlrpc.client.loads(msg) + print("VirtualNavigation poll_event", params,methodname) + self.event_cb("completed", 1) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index b345c4d7..77983a5a 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -178,7 +178,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.command_id = 0 self.event_queue = queue.Queue() self.component_clients = { - 'Navigation': VNavClient('http://localhost:8041') + 'Navigation': VNavClient('http://localhost:8041', self.completed) } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), -- GitLab From 26401e8ca2c43512a795f9af712b0be5b85cc68c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 19 Nov 2019 14:09:50 +0900 Subject: [PATCH 13/52] Add command_id_table to manage command and task --- route_guidance_ros/scripts/service_app.py | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py index 46016b37..421c6cdf 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app.py @@ -1,6 +1,6 @@ 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 +from pyrois import RoIS_Service, RoIS_HRI import logging import queue @@ -17,15 +17,19 @@ class Service_Application_IF(RoIS_Service.Service_Application_Base): 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 while True: - self.poll_event() + try: + self.poll_event() + except ConnectionRefusedError: + continue def poll_event(self): """poll_event """ - print("poll") + # print("poll") msg = self._proxy.poll_event() if msg is None: return @@ -40,9 +44,8 @@ class Service_Application_IF(RoIS_Service.Service_Application_Base): self.notify_event(params[0], params[1], params[2], params[3]) def completed(self, command_id, status): - print('received completed event {} with status {}.', - command_id, - RoIS_Service.Completed_Status(status).name) + self.command_id_table[command_id] = True + print(self.command_id_table) def notify_error(self, error_id, error_type): print('received error event {}({}) ', @@ -56,6 +59,12 @@ class Service_Application_IF(RoIS_Service.Service_Application_Base): subscribe_id, expire) + def go_robot_to(pos): + (return_code, command_id) = self._proxy.set_parameter('Navigation', [dest, "", ""]) + stautus = RoIS_HRI.ReturnCode_t(return_code) + self.command_id_table[command_id_table] = False + print(self.command_id_table) + def example_sa_IF(url, q): try: -- GitLab From a03e0e0db6c17ffef9589e6e6be69fbf742b497c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 19 Nov 2019 15:56:56 +0900 Subject: [PATCH 14/52] Change params of event message --- route_guidance_ros/scripts/VirtualNavigation_client.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index da6c6eac..0274b0fd 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -124,4 +124,5 @@ class VirtualNavigation_Client(Command, Query, Event): print("Caught msg:", msg) (params, methodname) = xmlrpc.client.loads(msg) print("VirtualNavigation poll_event", params,methodname) - self.event_cb("completed", 1) + self.event_cb("1", "completed") + # self.event_cb(command_id, "completed") -- GitLab From 4c62709fbb6abc452fe688f0beb093f973bdf6b5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 19 Nov 2019 15:57:27 +0900 Subject: [PATCH 15/52] Upgrade service_app to poll event --- route_guidance_ros/scripts/service_app.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py index 421c6cdf..5a0dcfe2 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app.py @@ -20,22 +20,27 @@ class Service_Application_IF(RoIS_Service.Service_Application_Base): 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() - except ConnectionRefusedError: + except: + print("Error ") continue def poll_event(self): """poll_event """ - # print("poll") msg = self._proxy.poll_event() if msg is None: return (params, methodname) = xmlrpc.client.loads(msg) #self.logger.debug('poll_event: '+methodname) - # print(params,methodname) + print("params: ", params, ", methodname: ", methodname) if methodname == 'completed': self.completed(params[0], params[1]) elif methodname == 'notify_error': @@ -59,10 +64,12 @@ class Service_Application_IF(RoIS_Service.Service_Application_Base): subscribe_id, expire) - def go_robot_to(pos): + 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_table] = False + self.command_id_table[command_id] = False print(self.command_id_table) -- GitLab From fa920330d69e58c1a2fcfbe4b54c37d4da75a3ca Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 19 Nov 2019 15:57:43 +0900 Subject: [PATCH 16/52] Add test script for navigation with service_app --- route_guidance_ros/scripts/test_app.py | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 route_guidance_ros/scripts/test_app.py diff --git a/route_guidance_ros/scripts/test_app.py b/route_guidance_ros/scripts/test_app.py new file mode 100644 index 00000000..e16f7de0 --- /dev/null +++ b/route_guidance_ros/scripts/test_app.py @@ -0,0 +1,23 @@ +# 1. launch multi navigation. +# 2. run this test. + + +from service_app import Service_Application_IF as Service +import time +from pyrois.RoIS_Common import Component_Status +from utilities import setup_multi_robot + +process = setup_multi_robot() + +time.sleep(10) +print("launching Service...") +service = Service('http://localhost:8000') +print("Service was established.") +for dest in [[25, 0], [0, 20], [5, 0]]: + status = service.go_robot_to(dest) + print(status) + time.sleep(5) +# service.all_finished() + +time.sleep(30) +print("finish") -- GitLab From dd2a958617d6cad0e78ca3d3af5a9b18650446bf Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 20 Nov 2019 14:51:30 +0900 Subject: [PATCH 17/52] Make service_app inheriting Service_Application_IF --- route_guidance_ros/scripts/service_app.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py index 5a0dcfe2..e2d0209c 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app.py @@ -11,7 +11,7 @@ import socketserver import xmlrpc.server -class Service_Application_IF(RoIS_Service.Service_Application_Base): +class Service_Application_IF(pyrois_Service_Application_IF): """Service_Application_IF """ def __init__(self, uri, logger=None): -- GitLab From 289ffee6c082d5d2468d764a08d4f8bc8fd3a88e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 20 Nov 2019 15:12:51 +0900 Subject: [PATCH 18/52] Remove unused function on main_engine --- route_guidance_ros/scripts/main_engine.py | 8 -------- route_guidance_ros/scripts/main_engine_client.py | 3 --- 2 files changed, 11 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 77983a5a..7723aa1c 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -201,14 +201,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): return (status, str(self.command_id)) - def all_finished(self): - self.completed("1", 1) - self.completed("2", 1) - self.completed("3", 1) - status = RoIS_HRI.ReturnCode_t.OK.value - return (status, 1) - - class IF_server: """IF_Server """ diff --git a/route_guidance_ros/scripts/main_engine_client.py b/route_guidance_ros/scripts/main_engine_client.py index 446dbf01..1e7e3a18 100644 --- a/route_guidance_ros/scripts/main_engine_client.py +++ b/route_guidance_ros/scripts/main_engine_client.py @@ -125,9 +125,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) - def all_finished(self): - (command_id, status) = self._proxy.all_finished() - if __name__ == "__main__": pass -- GitLab From 5a3532303a8a5deea3883bed12cdd3eb0553b84a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 20 Nov 2019 16:56:13 +0900 Subject: [PATCH 19/52] Add callback function of Navifation event --- route_guidance_ros/scripts/Navigation_client.py | 4 +++- route_guidance_ros/scripts/sub_engine.py | 6 +++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Navigation_client.py b/route_guidance_ros/scripts/Navigation_client.py index 281ef6bb..e76a2c3a 100644 --- a/route_guidance_ros/scripts/Navigation_client.py +++ b/route_guidance_ros/scripts/Navigation_client.py @@ -93,6 +93,7 @@ class Event(RoIS_Common.Event): (params, methodname) = xmlrpc.client.loads(msg) #self.logger.debug('poll_event: '+methodname) print(params,methodname) + self.event_cb(params, methodname) # if methodname == 'speech_recognized': # self.speech_recognized(params[0], params[1]) @@ -100,9 +101,10 @@ class Event(RoIS_Common.Event): class Navigation_Client(Command, Query, Event): """Navigation_Client """ - def __init__(self, uri): + def __init__(self, uri, event_cb): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] + self.event_cb = event_cb self.start_th() diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 638b0f79..60d21a1d 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -176,7 +176,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application super().__init__(Engine) self.command_id = 0 self.compoent_clients = { - 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), + 'Navigation': NavClient('http://localhost:' + str(engine_port+1), self.completed), 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) } @@ -190,6 +190,10 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application self.command_id += 1 return (status, str(self.command_id)) + def completed(self, params, methodname): + print("SubEngine completed called.") + print(params, methodname) + ### Remove ### def get_navi_status(self): status, c_status = self.compoent_clients['Navigation'].component_status() -- GitLab From 4f4fa8f0795a11b7b2b2bce6f7a99242b0c0d16a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 20 Nov 2019 16:56:32 +0900 Subject: [PATCH 20/52] [TMP] Remove exception catcher --- route_guidance_ros/scripts/service_app.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py index e2d0209c..fc0a6146 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app.py @@ -26,11 +26,12 @@ class Service_Application_IF(pyrois_Service_Application_IF): def event_poll_loop(self): print("poll loop started") while True: - try: - self.poll_event() - except: - print("Error ") - continue + # try: + # self.poll_event() + self.poll_event() + # except: + # print("Error ") + # continue def poll_event(self): """poll_event -- GitLab From bdc736095b440a00d7c451165f7279baaf672931 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 20 Nov 2019 16:57:06 +0900 Subject: [PATCH 21/52] [WIP] Add second sub_engine tester with service --- .../scripts/test_sub_engine2.py | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 route_guidance_ros/scripts/test_sub_engine2.py diff --git a/route_guidance_ros/scripts/test_sub_engine2.py b/route_guidance_ros/scripts/test_sub_engine2.py new file mode 100644 index 00000000..599353a3 --- /dev/null +++ b/route_guidance_ros/scripts/test_sub_engine2.py @@ -0,0 +1,44 @@ +# 1. launch single navigation. +# 2. run this test. + + +from sub_engine_client import IF as Engine +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 +import xmlrpc + + +class Service(Service_Application_IF): + def __init__(self, uri): + super().__init__(uri) + CommandStatus = namedtuple('CommandStatus', [ + 'is_finished', + ]) + self.commmand_status_table = {} + + def completed(self, command_id, status): + self.commmand_status_table[command_id].is_finished = True + + def go_robot_to(self,dest): + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + status, command_id = \ + self._proxy.set_parameter('Navigation',[dest, "", ""]) + self.commmand_status_table[command_id] = CommandStatus(False) + + + +process = setup_single_robot() +time.sleep(10) +service = Service('http://127.0.0.1:8010') + + +destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] +for dest in destinations: + service.go_robot_to(dest) + time.sleep(10) + +print("finish") -- GitLab From f5fdffb13a5d8265e0d643f1105b087b0a907d0b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 21 Nov 2019 17:16:26 +0900 Subject: [PATCH 22/52] Add analyze_c_status function --- route_guidance_ros/scripts/main_engine.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 7723aa1c..8d2b20cc 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -16,6 +16,7 @@ import os import sys import queue import numpy as np +import threading from pyrois import RoIS_HRI from pyrois.Service_Application_Base_example import Service_Application_Base @@ -169,6 +170,7 @@ class EventIF(RoIS_HRI.EventIF): from VirtualNavigation_client import VirtualNavigation_Client as VNavClient from sub_engine_client import IF as EngineClient +from pyrois.RoIS_Common import Component_Status class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): """IF @@ -196,10 +198,22 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def set_parameter(self, component_ref, parameters): print("Using VirtualNavigation") target_component = self.component_clients[component_ref] - status = target_component.set_parameter(*parameters).value self.command_id += 1 + status = target_component.set_parameter(*parameters).value + th = threading.Thread( + target=self.analyze_c_status, + daemon=True, + args=(self.component_clients[component_ref], self.command_id)) return (status, str(self.command_id)) + def analyze_c_status(self, component, command_id): + while True: + (_, c_status) = component.component_status() + if c_status == Component_Status.READY.value: + print("WOW") + self.completed(command_id, c_status) + return + class IF_server: """IF_Server -- GitLab From ed67690d142525286a80fb832778deb3f4588c34 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 22 Nov 2019 00:07:11 +0900 Subject: [PATCH 23/52] Commnent out unsupported functions on test --- route_guidance_ros/scripts/test_sub_engine.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/test_sub_engine.py b/route_guidance_ros/scripts/test_sub_engine.py index 46664c3a..c6b6aad0 100644 --- a/route_guidance_ros/scripts/test_sub_engine.py +++ b/route_guidance_ros/scripts/test_sub_engine.py @@ -17,11 +17,11 @@ for dest in destinations: print(engine.set_parameter('Navigation',[dest, "", ""])) navi_status = Component_Status.BUSY while navi_status != Component_Status.READY: - server_status, navi_status = engine.get_navi_status() + # server_status, navi_status = engine.get_navi_status() # print("TEST: status = {}".format(navi_status)) time.sleep(1) - position = engine.get_position() - print("TEST: position = {}".format(position)) + # position = engine.get_position() + # print("TEST: position = {}".format(position)) time.sleep(3) #print(engine.analysis_c_status()) -- GitLab From 9727ed3adabb7a05d976462076521789ade7470d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 22 Nov 2019 00:08:19 +0900 Subject: [PATCH 24/52] Add analyze_c_status function on sub-engine --- route_guidance_ros/scripts/main_engine.py | 1 + route_guidance_ros/scripts/sub_engine.py | 13 ++++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 8d2b20cc..c5f1b62a 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -204,6 +204,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): target=self.analyze_c_status, daemon=True, args=(self.component_clients[component_ref], self.command_id)) + th.start() return (status, str(self.command_id)) def analyze_c_status(self, component, command_id): diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 60d21a1d..c8297256 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -168,6 +168,7 @@ class EventIF(RoIS_HRI.EventIF): from Navigation_client import Navigation_Client as NavClient from System_Information_client import System_Information_Client as SysClient +from pyrois.RoIS_Common import Component_Status class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): """IF @@ -182,12 +183,14 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application def set_parameter(self, component_ref, parameters): status = None + self.command_id += 1 if not component_ref in self.compoent_clients: status = RoIS_HRI.ReturnCode_t.ERROR.value else: target_component_client = self.compoent_clients[component_ref] status = target_component_client.set_parameter(*parameters).value - self.command_id += 1 + th = threading.Thread(target=self.analyze_c_status, daemon=True, args=(target_component_client, self.command_id)) + th.start() return (status, str(self.command_id)) def completed(self, params, methodname): @@ -205,6 +208,14 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application return (status.value, timestamp, robot_ref, position_data) ############## + def analyze_c_status(self, component, command_id): + while True: + (_, c_status) = component.component_status() + if c_status == Component_Status.READY.value: + print("WOW") + self.completed(command_id, c_status) + return + class IF_server: """IF_Server """ -- GitLab From cdf66d93bd51fcecedd92ad0d6ef1362f62d16d7 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 22 Nov 2019 13:17:28 +0900 Subject: [PATCH 25/52] Fix KeyError of CommandStatus class --- route_guidance_ros/scripts/test_sub_engine2.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/test_sub_engine2.py b/route_guidance_ros/scripts/test_sub_engine2.py index 599353a3..d118cfe0 100644 --- a/route_guidance_ros/scripts/test_sub_engine2.py +++ b/route_guidance_ros/scripts/test_sub_engine2.py @@ -11,12 +11,13 @@ from service_app import Service_Application_IF import xmlrpc +CommandStatus = namedtuple('CommandStatus', [ + 'is_finished', +]) + class Service(Service_Application_IF): def __init__(self, uri): super().__init__(uri) - CommandStatus = namedtuple('CommandStatus', [ - 'is_finished', - ]) self.commmand_status_table = {} def completed(self, command_id, status): -- GitLab From eaddf06f6f288761e9afc62ce5e6b4155d6827a0 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 22 Nov 2019 13:17:51 +0900 Subject: [PATCH 26/52] Change base class of IF (Abs => Example) --- route_guidance_ros/scripts/sub_engine.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index c8297256..677ae9af 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -166,11 +166,12 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) +from pyrois.Service_Application_Base_example import Service_Application_Base from Navigation_client import Navigation_Client as NavClient from System_Information_client import System_Information_Client as SysClient from pyrois.RoIS_Common import Component_Status -class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): +class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): """IF """ def __init__(self, Engine, engine_port): @@ -215,6 +216,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application print("WOW") self.completed(command_id, c_status) return + class IF_server: """IF_Server -- GitLab From 0a850c12bca5f5ca3c29f0ee9af79308b96836ad Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 22 Nov 2019 13:41:46 +0900 Subject: [PATCH 27/52] Add sleep after Service established --- route_guidance_ros/scripts/test_sub_engine2.py | 1 + 1 file changed, 1 insertion(+) diff --git a/route_guidance_ros/scripts/test_sub_engine2.py b/route_guidance_ros/scripts/test_sub_engine2.py index d118cfe0..602185df 100644 --- a/route_guidance_ros/scripts/test_sub_engine2.py +++ b/route_guidance_ros/scripts/test_sub_engine2.py @@ -35,6 +35,7 @@ class Service(Service_Application_IF): process = setup_single_robot() time.sleep(10) service = Service('http://127.0.0.1:8010') +time.sleep(10) destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] -- GitLab From ef46662daaa64a86076926c041fa37525190e19b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 22 Nov 2019 13:48:06 +0900 Subject: [PATCH 28/52] Add event_queue on sub-engine --- route_guidance_ros/scripts/sub_engine.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 677ae9af..d44b07b8 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -170,6 +170,7 @@ from pyrois.Service_Application_Base_example import Service_Application_Base from Navigation_client import Navigation_Client as NavClient from System_Information_client import System_Information_Client as SysClient from pyrois.RoIS_Common import Component_Status +import queue class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): """IF @@ -177,6 +178,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def __init__(self, Engine, engine_port): super().__init__(Engine) self.command_id = 0 + self.event_queue = queue.Queue() self.compoent_clients = { 'Navigation': NavClient('http://localhost:' + str(engine_port+1), self.completed), 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) @@ -190,8 +192,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): else: target_component_client = self.compoent_clients[component_ref] status = target_component_client.set_parameter(*parameters).value - th = threading.Thread(target=self.analyze_c_status, daemon=True, args=(target_component_client, self.command_id)) - th.start() + # th = threading.Thread(target=self.analyze_c_status, daemon=True, args=(target_component_client, self.command_id)) + # th.start() return (status, str(self.command_id)) def completed(self, params, methodname): -- GitLab From b90d177fda37532196606eac9e5960a91ccf0b70 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 22 Nov 2019 13:49:39 +0900 Subject: [PATCH 29/52] Revert "Add callback function of Navifation event" This reverts commit 5a3532303a8a5deea3883bed12cdd3eb0553b84a. --- route_guidance_ros/scripts/Navigation_client.py | 4 +--- route_guidance_ros/scripts/sub_engine.py | 6 +----- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/Navigation_client.py b/route_guidance_ros/scripts/Navigation_client.py index e76a2c3a..281ef6bb 100644 --- a/route_guidance_ros/scripts/Navigation_client.py +++ b/route_guidance_ros/scripts/Navigation_client.py @@ -93,7 +93,6 @@ class Event(RoIS_Common.Event): (params, methodname) = xmlrpc.client.loads(msg) #self.logger.debug('poll_event: '+methodname) print(params,methodname) - self.event_cb(params, methodname) # if methodname == 'speech_recognized': # self.speech_recognized(params[0], params[1]) @@ -101,10 +100,9 @@ class Event(RoIS_Common.Event): class Navigation_Client(Command, Query, Event): """Navigation_Client """ - def __init__(self, uri, event_cb): + def __init__(self, uri): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] - self.event_cb = event_cb self.start_th() diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index d44b07b8..14f26819 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -180,7 +180,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.command_id = 0 self.event_queue = queue.Queue() self.compoent_clients = { - 'Navigation': NavClient('http://localhost:' + str(engine_port+1), self.completed), + 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) } @@ -196,10 +196,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): # th.start() return (status, str(self.command_id)) - def completed(self, params, methodname): - print("SubEngine completed called.") - print(params, methodname) - ### Remove ### def get_navi_status(self): status, c_status = self.compoent_clients['Navigation'].component_status() -- GitLab From 4a6bb46736a122d1deb9d86ca331b857651caed5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 22 Nov 2019 18:41:35 +0900 Subject: [PATCH 30/52] Remove old event polling system from Navigation --- route_guidance_ros/scripts/Navigation.py | 18 ------------- .../scripts/Navigation_client.py | 25 ------------------- 2 files changed, 43 deletions(-) diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index 0e53006f..593f1fc5 100755 --- a/route_guidance_ros/scripts/Navigation.py +++ b/route_guidance_ros/scripts/Navigation.py @@ -69,10 +69,6 @@ class Event(RoIS_Common.Event): self._component = c self.event_queue = queue.Queue() - def poll_event(self): - print("Navi Server") - msg = self.event_queue.get() - return msg from goal_sender_ros import GoalSenderROS import xmlrpclib @@ -88,7 +84,6 @@ class Navigation(Event, Command, Query): self._component.Time_Limit = 10 self._component.Routing_Policy = "" self.latest_nav_state = Component_Status.UNINITIALIZED - self.start_th() def set_parameter(self, target_position, time_limit, routing_policy): # Put event @@ -103,19 +98,6 @@ class Navigation(Event, Command, Query): c_status = self._goal_sender.get_goalsender_state().value return (status, c_status) - def start_th(self): - self.th = threading.Thread(target=self.nav_finish_event_loop) - self.th.start() - - def nav_finish_event_loop(self): - while True: - old_nav_state = self.latest_nav_state - self.latest_nav_state = self._goal_sender.get_goalsender_state() - if old_nav_state is Component_Status.BUSY and \ - self.latest_nav_state != old_nav_state: - msg = xmlrpclib.dumps(("Navigation", "Finished"), 'set_parameter') - self.event_queue.put(msg) - def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) diff --git a/route_guidance_ros/scripts/Navigation_client.py b/route_guidance_ros/scripts/Navigation_client.py index 281ef6bb..3212bc17 100644 --- a/route_guidance_ros/scripts/Navigation_client.py +++ b/route_guidance_ros/scripts/Navigation_client.py @@ -72,30 +72,6 @@ class Event(RoIS_Common.Event): # if logger is not None: # self.logger = logger - def start_th(self): - self.th = threading.Thread(target=self.event_loop) - self.th.start() - - def event_loop(self): - """event_loop - """ - while True: - print("Navigation_Client event_loop") - try: - self.poll_event() - except ConnectionRefusedError: - break - - def poll_event(self): - """poll_event - """ - msg = self._e_proxy.poll_event() - (params, methodname) = xmlrpc.client.loads(msg) - #self.logger.debug('poll_event: '+methodname) - print(params,methodname) - # if methodname == 'speech_recognized': - # self.speech_recognized(params[0], params[1]) - class Navigation_Client(Command, Query, Event): """Navigation_Client @@ -105,4 +81,3 @@ class Navigation_Client(Command, Query, Event): self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] - self.start_th() -- GitLab From 1cacf48e66b17a3272d1566e46b2adfbdf2e9f7b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 22 Nov 2019 18:41:55 +0900 Subject: [PATCH 31/52] Add debug code of analyze_c_status function --- route_guidance_ros/scripts/sub_engine.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 14f26819..1eabad40 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -171,6 +171,7 @@ from Navigation_client import Navigation_Client as NavClient from System_Information_client import System_Information_Client as SysClient from pyrois.RoIS_Common import Component_Status import queue +import time class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): """IF @@ -209,9 +210,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def analyze_c_status(self, component, command_id): while True: + time.sleep(3) + print("sub_engine analyze_c_status for command_id ", command_id) (_, c_status) = component.component_status() + print("c_status is ", c_status) if c_status == Component_Status.READY.value: - print("WOW") self.completed(command_id, c_status) return -- GitLab From 84bd76187611ff4bd7f8d82c8240d061cf8dfec1 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 00:40:04 +0900 Subject: [PATCH 32/52] Enable analyze_c_status loop --- route_guidance_ros/scripts/sub_engine.py | 10 ++++++---- route_guidance_ros/scripts/test_sub_engine2.py | 4 +++- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 1eabad40..5491e103 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -191,10 +191,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): if not component_ref in self.compoent_clients: status = RoIS_HRI.ReturnCode_t.ERROR.value else: + print("[set_parameter] component_ref started") target_component_client = self.compoent_clients[component_ref] status = target_component_client.set_parameter(*parameters).value - # th = threading.Thread(target=self.analyze_c_status, daemon=True, args=(target_component_client, self.command_id)) - # th.start() + th = threading.Thread(target=self.analyze_c_status, daemon=True, args=(target_component_client, self.command_id)) + th.start() return (status, str(self.command_id)) ### Remove ### @@ -210,14 +211,15 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def analyze_c_status(self, component, command_id): while True: - time.sleep(3) + time.sleep(5) print("sub_engine analyze_c_status for command_id ", command_id) (_, c_status) = component.component_status() print("c_status is ", c_status) if c_status == Component_Status.READY.value: + print("completed called for command_id ", command_id) self.completed(command_id, c_status) return - + class IF_server: """IF_Server diff --git a/route_guidance_ros/scripts/test_sub_engine2.py b/route_guidance_ros/scripts/test_sub_engine2.py index 602185df..9970c4ee 100644 --- a/route_guidance_ros/scripts/test_sub_engine2.py +++ b/route_guidance_ros/scripts/test_sub_engine2.py @@ -20,7 +20,9 @@ class Service(Service_Application_IF): super().__init__(uri) self.commmand_status_table = {} + def completed(self, command_id, status): + print("Service completed called.") self.commmand_status_table[command_id].is_finished = True def go_robot_to(self,dest): @@ -41,6 +43,6 @@ time.sleep(10) destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] for dest in destinations: service.go_robot_to(dest) - time.sleep(10) + time.sleep(30) print("finish") -- GitLab From 2e9e5ad30170ebf3c8ec474589ab59916861ffe6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 01:08:26 +0900 Subject: [PATCH 33/52] Fix component status type error --- route_guidance_ros/scripts/sub_engine.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 5491e103..b1d782c5 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -215,9 +215,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): print("sub_engine analyze_c_status for command_id ", command_id) (_, c_status) = component.component_status() print("c_status is ", c_status) - if c_status == Component_Status.READY.value: + if c_status == Component_Status.READY: print("completed called for command_id ", command_id) - self.completed(command_id, c_status) + self.completed(command_id, c_status.value) return -- GitLab From 7f7a07142ce9766ae53ccaebf6449cd404264080 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 01:29:59 +0900 Subject: [PATCH 34/52] Fix type of command_id on analyze_c_status --- route_guidance_ros/scripts/sub_engine.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index b1d782c5..4df6e8f9 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -194,7 +194,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): print("[set_parameter] component_ref started") target_component_client = self.compoent_clients[component_ref] status = target_component_client.set_parameter(*parameters).value - th = threading.Thread(target=self.analyze_c_status, daemon=True, args=(target_component_client, self.command_id)) + th = threading.Thread(target=self.analyze_c_status, daemon=True, args=(target_component_client, str(self.command_id))) th.start() return (status, str(self.command_id)) -- GitLab From c6546f02a515d22cef2c55ffbf0a784ba73df937 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 01:49:08 +0900 Subject: [PATCH 35/52] Remake test_sub_engine2 with event subscription --- .../scripts/test_sub_engine2.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/test_sub_engine2.py b/route_guidance_ros/scripts/test_sub_engine2.py index 9970c4ee..10b08ddc 100644 --- a/route_guidance_ros/scripts/test_sub_engine2.py +++ b/route_guidance_ros/scripts/test_sub_engine2.py @@ -11,27 +11,36 @@ from service_app import Service_Application_IF import xmlrpc -CommandStatus = namedtuple('CommandStatus', [ - 'is_finished', -]) +class CommandStatus(object): + def __init__(self, is_finished): + self.is_finished = is_finished + + def __repr__(self): + return "CommandStatus ".format(self.is_finished) + class Service(Service_Application_IF): def __init__(self, uri): super().__init__(uri) self.commmand_status_table = {} - def completed(self, command_id, status): print("Service completed called.") + print("type of command_id is ", type(command_id)) + print(self.commmand_status_table[command_id]) self.commmand_status_table[command_id].is_finished = True + print("commmand_status: ", self.commmand_status_table) - def go_robot_to(self,dest): + def go_robot_to(self, dest): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) status, command_id = \ self._proxy.set_parameter('Navigation',[dest, "", ""]) self.commmand_status_table[command_id] = CommandStatus(False) + print("commmand_status: ", self.commmand_status_table) + def run(self): + pass process = setup_single_robot() -- GitLab From 447eb53fb26a32639990dd48692012ab9d236fe9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 02:00:58 +0900 Subject: [PATCH 36/52] Modify test to wait until a robot reaches goal --- route_guidance_ros/scripts/test_sub_engine2.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/route_guidance_ros/scripts/test_sub_engine2.py b/route_guidance_ros/scripts/test_sub_engine2.py index 10b08ddc..734ee9c7 100644 --- a/route_guidance_ros/scripts/test_sub_engine2.py +++ b/route_guidance_ros/scripts/test_sub_engine2.py @@ -23,6 +23,7 @@ class Service(Service_Application_IF): def __init__(self, uri): super().__init__(uri) self.commmand_status_table = {} + self.is_navigation_running = False def completed(self, command_id, status): print("Service completed called.") @@ -30,8 +31,10 @@ class Service(Service_Application_IF): print(self.commmand_status_table[command_id]) self.commmand_status_table[command_id].is_finished = True print("commmand_status: ", self.commmand_status_table) + self.is_navigation_running = False def go_robot_to(self, dest): + self.is_navigation_running = True self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) status, command_id = \ @@ -40,7 +43,11 @@ class Service(Service_Application_IF): print("commmand_status: ", self.commmand_status_table) def run(self): - pass + destinations = [[5, 0], [10, 0], [25, 0], [25, 20]] + for dest in destinations: + self.go_robot_to(dest) + while self.is_navigation_running: + time.sleep(1) process = setup_single_robot() @@ -48,10 +55,6 @@ time.sleep(10) service = Service('http://127.0.0.1:8010') time.sleep(10) - -destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] -for dest in destinations: - service.go_robot_to(dest) - time.sleep(30) +service.run() print("finish") -- GitLab From 6b1b12b9b98f0d3a98b92297fd2636dc8a26aee9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 02:07:52 +0900 Subject: [PATCH 37/52] Rename test_sub_engine2 => test_sub_engine_with_service --- .../{test_sub_engine2.py => test_sub_engine_with_service.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename route_guidance_ros/scripts/{test_sub_engine2.py => test_sub_engine_with_service.py} (100%) diff --git a/route_guidance_ros/scripts/test_sub_engine2.py b/route_guidance_ros/scripts/test_sub_engine_with_service.py similarity index 100% rename from route_guidance_ros/scripts/test_sub_engine2.py rename to route_guidance_ros/scripts/test_sub_engine_with_service.py -- GitLab From 1ebe9cbc42bf386597e08db9ab0fa3e37ec0b0f8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 02:38:18 +0900 Subject: [PATCH 38/52] Upgrade layout of debug code --- route_guidance_ros/scripts/sub_engine.py | 4 +--- .../scripts/test_sub_engine_with_service.py | 12 +++++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 4df6e8f9..6a90c252 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -212,11 +212,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def analyze_c_status(self, component, command_id): while True: time.sleep(5) - print("sub_engine analyze_c_status for command_id ", command_id) (_, c_status) = component.component_status() - print("c_status is ", c_status) + print("command_id: {}, status: {}".format(command_id, c_status)) if c_status == Component_Status.READY: - print("completed called for command_id ", command_id) self.completed(command_id, c_status.value) return 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 734ee9c7..01f0dc39 100644 --- a/route_guidance_ros/scripts/test_sub_engine_with_service.py +++ b/route_guidance_ros/scripts/test_sub_engine_with_service.py @@ -16,7 +16,7 @@ class CommandStatus(object): self.is_finished = is_finished def __repr__(self): - return "CommandStatus ".format(self.is_finished) + return "CommandStatus\n".format(self.is_finished) class Service(Service_Application_IF): @@ -26,11 +26,11 @@ class Service(Service_Application_IF): self.is_navigation_running = False def completed(self, command_id, status): - print("Service completed called.") - print("type of command_id is ", type(command_id)) print(self.commmand_status_table[command_id]) self.commmand_status_table[command_id].is_finished = True - print("commmand_status: ", self.commmand_status_table) + print("----------------------------------------------") + print("commmand_status:\n", self.commmand_status_table) + print("----------------------------------------------") self.is_navigation_running = False def go_robot_to(self, dest): @@ -40,7 +40,9 @@ class Service(Service_Application_IF): status, command_id = \ self._proxy.set_parameter('Navigation',[dest, "", ""]) self.commmand_status_table[command_id] = CommandStatus(False) - print("commmand_status: ", self.commmand_status_table) + print("----------------------------------------------") + print("commmand_status:\n", self.commmand_status_table) + print("----------------------------------------------") def run(self): destinations = [[5, 0], [10, 0], [25, 0], [25, 20]] -- GitLab From fb7fbebd88217fbc5f0bbe80cd11957a619c952e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 15:25:19 +0900 Subject: [PATCH 39/52] Give event_queue to VirtualNavigation_client --- route_guidance_ros/scripts/VirtualNavigation_client.py | 6 +++--- route_guidance_ros/scripts/main_engine.py | 3 +-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index 0274b0fd..0cb040ff 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -108,12 +108,12 @@ class Event(RoIS_Common.Event): class VirtualNavigation_Client(Command, Query, Event): """VirtualNavigation_Client """ - def __init__(self, uri, event_cb): + def __init__(self, uri, event_queue): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] - self.event_cb = event_cb + self.event_queue = event_queue self.start_th() def poll_event(self): @@ -124,5 +124,5 @@ class VirtualNavigation_Client(Command, Query, Event): print("Caught msg:", msg) (params, methodname) = xmlrpc.client.loads(msg) print("VirtualNavigation poll_event", params,methodname) - self.event_cb("1", "completed") + # self.event_cb("1", "completed") # self.event_cb(command_id, "completed") diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index c5f1b62a..200aa01e 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -180,7 +180,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.command_id = 0 self.event_queue = queue.Queue() self.component_clients = { - 'Navigation': VNavClient('http://localhost:8041', self.completed) + 'Navigation': VNavClient('http://localhost:8041', self.event_queue) } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), @@ -211,7 +211,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): while True: (_, c_status) = component.component_status() if c_status == Component_Status.READY.value: - print("WOW") self.completed(command_id, c_status) return -- GitLab From d9f3bb8a1efe57b72375cd7ab0e9081dbaaef4e9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 15:32:38 +0900 Subject: [PATCH 40/52] Use engine_event_queue to send event from VNav --- route_guidance_ros/scripts/VirtualNavigation.py | 2 +- route_guidance_ros/scripts/VirtualNavigation_client.py | 5 +++-- route_guidance_ros/scripts/main_engine.py | 10 ++++++---- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index a7148faa..17321c38 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -113,7 +113,7 @@ class VirtualNavigation(Event, Command, Query): if current_state != old_state and \ old_state is Component_Status.BUSY: print("Adding new EVENT") - msg = xmlrpclib.dumps(("Nav", "Fin"), 'set_param') + msg = xmlrpclib.dumps(("1", robot_name), 'completed') self.event_queue.put(msg) # unnecessary ? diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index 0cb040ff..03e3e5ad 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -108,12 +108,12 @@ class Event(RoIS_Common.Event): class VirtualNavigation_Client(Command, Query, Event): """VirtualNavigation_Client """ - def __init__(self, uri, event_queue): + def __init__(self, uri, engine_event_queue): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] - self.event_queue = event_queue + self.engine_event_queue = engine_event_queue self.start_th() def poll_event(self): @@ -124,5 +124,6 @@ class VirtualNavigation_Client(Command, Query, Event): print("Caught msg:", msg) (params, methodname) = xmlrpc.client.loads(msg) print("VirtualNavigation poll_event", params,methodname) + self.engine_event_queue.put((params, methodname)) # self.event_cb("1", "completed") # self.event_cb(command_id, "completed") diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 200aa01e..e323528f 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -209,10 +209,12 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def analyze_c_status(self, component, command_id): while True: - (_, c_status) = component.component_status() - if c_status == Component_Status.READY.value: - self.completed(command_id, c_status) - return + tmp = self.event_queue.get() + print("tmp") + # (_, c_status) = component.component_status() + # if c_status == Component_Status.READY.value: + # self.completed(command_id, c_status) + # return class IF_server: -- GitLab From ba8b4de74aceee899e658001f07ee67c52e9bf71 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 15:44:13 +0900 Subject: [PATCH 41/52] Upgrade debug code for event_queue --- route_guidance_ros/scripts/main_engine.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index e323528f..c4535840 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -209,8 +209,10 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def analyze_c_status(self, component, command_id): while True: - tmp = self.event_queue.get() - print("tmp") + # Is event_queue thread-safe ? + (command_id, robot_name), method_name = self.event_queue.get() + print("{} reaches goal (command_id: {})".format(robot_name, command_id)) + time.sleep(3) # (_, c_status) = component.component_status() # if c_status == Component_Status.READY.value: # self.completed(command_id, c_status) -- GitLab From 05387288af4ea368db8d73926c7be2cf2202f7a9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 15:48:53 +0900 Subject: [PATCH 42/52] Modify to send target_pos and robot_name as event --- route_guidance_ros/scripts/VirtualNavigation.py | 1 + route_guidance_ros/scripts/main_engine.py | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 17321c38..e617de4c 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -86,6 +86,7 @@ class VirtualNavigation(Event, Command, Query): self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" + self.goal_robot_table = {} self.nav_state_table = \ {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} self.start_th() diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index c4535840..7a6e8ab5 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -210,8 +210,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def analyze_c_status(self, component, command_id): while True: # Is event_queue thread-safe ? - (command_id, robot_name), method_name = self.event_queue.get() - print("{} reaches goal (command_id: {})".format(robot_name, command_id)) + (target_pos, robot_name), method_name = self.event_queue.get() + print("{} reaches goal ({})".format(robot_name, target_pos)) time.sleep(3) # (_, c_status) = component.component_status() # if c_status == Component_Status.READY.value: -- GitLab From 4fddc422c9d1b0e400e40c72caead302f7a91247 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 15:53:09 +0900 Subject: [PATCH 43/52] Split function send_goal_to_nearest_robotname To get_nearest_robotname + send_goal_to_robot --- route_guidance_ros/scripts/VirtualNavigation.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index e617de4c..3ad3effe 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -150,7 +150,6 @@ class Component(object): pos = amcl_pose.pose.pose.position self.__robot_positions[robot_name] = np.array([pos.x, pos.y]) - def send_goal_to_nearest_robot(self, dest): dest = np.array(dest) distance_table = \ @@ -160,6 +159,16 @@ class Component(object): self.__goal_senders[nearest_robot].send_goal(dest.tolist()) return nearest_robot + def get_nearest_robotname(self, position): + dest = np.array(dest) + distance_table = \ + {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} + nearest_robot = min(distance_table, key=distance_table.get) + return nearest_robot + + def send_goal_to_robot(self, goal, robot): + self.__goal_senders[robot].send_goal(goal.tolist()) + def get_nav_state(self, robot_name): goal_sender = self.__goal_senders[robot_name] return goal_sender.get_goalsender_state() -- GitLab From 28aaf96a52c95b64d0447273f2ec412264a22ac1 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 23 Nov 2019 16:00:53 +0900 Subject: [PATCH 44/52] Upgrade to send target_pos and robot_name as vnav event --- route_guidance_ros/scripts/VirtualNavigation.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 3ad3effe..a5053204 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -86,14 +86,17 @@ class VirtualNavigation(Event, Command, Query): self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" - self.goal_robot_table = {} + self.robot_goal_table = {} self.nav_state_table = \ {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} self.start_th() def set_parameter(self, target_position, time_limit, routing_policy): print("VirtualNavigation::set_parameter") - self._component.send_goal_to_nearest_robot(target_position) + # self._component.send_goal_to_nearest_robot(target_position) + nearest_robot = self._component.get_nearest_robotname(target_position) + self._component.send_goal_to_robot(target_position, nearest_robot) + self.goal_robot_table[nearest_robot] = target_position status = RoIS_HRI.ReturnCode_t.OK.value return status @@ -114,7 +117,7 @@ class VirtualNavigation(Event, Command, Query): if current_state != old_state and \ old_state is Component_Status.BUSY: print("Adding new EVENT") - msg = xmlrpclib.dumps(("1", robot_name), 'completed') + msg = xmlrpclib.dumps((self.robot_goal_table[robot_name], robot_name), 'completed') self.event_queue.put(msg) # unnecessary ? -- GitLab From e34e4f497c8d76d16cca4702bfeb05068445da06 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 24 Nov 2019 00:24:22 +0900 Subject: [PATCH 45/52] Fix runtime errors around type of values --- route_guidance_ros/scripts/VirtualNavigation.py | 6 +++--- .../scripts/VirtualNavigation_client.py | 2 +- route_guidance_ros/scripts/main_engine.py | 16 ++++++++++++++-- 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index a5053204..17149941 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -96,7 +96,7 @@ class VirtualNavigation(Event, Command, Query): # self._component.send_goal_to_nearest_robot(target_position) nearest_robot = self._component.get_nearest_robotname(target_position) self._component.send_goal_to_robot(target_position, nearest_robot) - self.goal_robot_table[nearest_robot] = target_position + self.robot_goal_table[nearest_robot] = str(target_position) status = RoIS_HRI.ReturnCode_t.OK.value return status @@ -162,7 +162,7 @@ class Component(object): self.__goal_senders[nearest_robot].send_goal(dest.tolist()) return nearest_robot - def get_nearest_robotname(self, position): + def get_nearest_robotname(self, dest): dest = np.array(dest) distance_table = \ {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} @@ -170,7 +170,7 @@ class Component(object): return nearest_robot def send_goal_to_robot(self, goal, robot): - self.__goal_senders[robot].send_goal(goal.tolist()) + self.__goal_senders[robot].send_goal(goal) def get_nav_state(self, robot_name): goal_sender = self.__goal_senders[robot_name] diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index 03e3e5ad..08beb15b 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -124,6 +124,6 @@ class VirtualNavigation_Client(Command, Query, Event): print("Caught msg:", msg) (params, methodname) = xmlrpc.client.loads(msg) print("VirtualNavigation poll_event", params,methodname) - self.engine_event_queue.put((params, methodname)) + self.engine_event_queue.put(msg) # self.event_cb("1", "completed") # self.event_cb(command_id, "completed") diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 7a6e8ab5..3a1a6794 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -171,6 +171,7 @@ class EventIF(RoIS_HRI.EventIF): from VirtualNavigation_client import VirtualNavigation_Client as VNavClient from sub_engine_client import IF as EngineClient from pyrois.RoIS_Common import Component_Status +import time class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): """IF @@ -192,6 +193,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): '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() @@ -203,15 +205,25 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): th = threading.Thread( target=self.analyze_c_status, daemon=True, - args=(self.component_clients[component_ref], self.command_id)) + args=(self.component_clients[component_ref], str(self.command_id))) th.start() + goal = parameters[0] + goal.append(0) + self.goal_command_table[str(goal)] = str(self.command_id) return (status, str(self.command_id)) def analyze_c_status(self, component, command_id): while True: # Is event_queue thread-safe ? - (target_pos, robot_name), method_name = self.event_queue.get() + msg = self.event_queue.get() + (target_pos, robot_name), methodname = xmlrpc.client.loads(msg) print("{} reaches goal ({})".format(robot_name, target_pos)) + print("goal_command_table: ", self.goal_command_table) + print("target_pos: {} ({})".format(target_pos, type(target_pos))) + if self.goal_command_table[str(target_pos)] == command_id: + print("Command (id: {}) was completed by {}".format(command_id, robot_name)) + self.completed(command_id, Component_Status.READY.value) + return time.sleep(3) # (_, c_status) = component.component_status() # if c_status == Component_Status.READY.value: -- GitLab From 557bc1c23c958fc84d4d48349ae079c6aa1f7372 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 24 Nov 2019 03:58:25 +0900 Subject: [PATCH 46/52] Add service script for test of main engine --- .../scripts/test_main_engine_with_service.py | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 route_guidance_ros/scripts/test_main_engine_with_service.py diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py new file mode 100644 index 00000000..7c5be10d --- /dev/null +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -0,0 +1,35 @@ +# 1. launch multi navigation. +# 2. run this test. + + +from main_engine_client import IF as Engine +import time +from pyrois.RoIS_Common import Component_Status +from pyrois.Service_Application_IF import Service_Application_IF +from utilities import setup_multi_robot + + +class Service(Service_Application_IF): + def __init__(self, uri="http://localhost:8000"): + super().__init__(uri) + + def completed(self, command_id, status): + print("Service command_id {} was completed.".format(command_id)) + + +process = setup_multi_robot() + +engine = Engine('http://127.0.0.1:8000') +print("Engine was established.") +time.sleep(10) +service = Service() +engine.connect() +for dest in [[25, 10], [0, 20], [5, 0]]: + status = engine.set_parameter('Navigation', [dest, "", ""]) + print(status) + time.sleep(3) +# engine.all_finished() + +time.sleep(30) +engine.disconnect() +print("finish") -- GitLab From 88005a2f8df19bc4dcd435448ef5941073d13a26 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 24 Nov 2019 04:06:23 +0900 Subject: [PATCH 47/52] Rename variable of event_queue for component --- route_guidance_ros/scripts/main_engine.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 3a1a6794..416269ba 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -180,8 +180,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): super().__init__(Engine) self.command_id = 0 self.event_queue = queue.Queue() + self.component_event_queue = queue.Queue() self.component_clients = { - 'Navigation': VNavClient('http://localhost:8041', self.event_queue) + 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue) } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), @@ -214,8 +215,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def analyze_c_status(self, component, command_id): while True: - # Is event_queue thread-safe ? - msg = self.event_queue.get() + # Is component_event_queue thread-safe ? + msg = self.component_event_queue.get() (target_pos, robot_name), methodname = xmlrpc.client.loads(msg) print("{} reaches goal ({})".format(robot_name, target_pos)) print("goal_command_table: ", self.goal_command_table) -- GitLab From 165f3729525a5a7a3691c6bf60ab6b4302275dc0 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 24 Nov 2019 14:10:23 +0900 Subject: [PATCH 48/52] Remove unnecessary print debug code --- route_guidance_ros/scripts/VirtualNavigation.py | 11 ++--------- .../scripts/VirtualNavigation_client.py | 12 ------------ route_guidance_ros/scripts/main_engine.py | 12 +++--------- .../scripts/test_main_engine_with_service.py | 2 +- 4 files changed, 6 insertions(+), 31 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 17149941..c89e7c43 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -70,7 +70,6 @@ class Event(RoIS_Common.Event): self.event_queue = queue.Queue() def poll_event(self): - print("!!!!!!!!!!!!!!!!polled!!!!!!!!!!!!!!!!!!!!!!!!!") msg = self.event_queue.get() return msg @@ -92,7 +91,6 @@ class VirtualNavigation(Event, Command, Query): self.start_th() def set_parameter(self, target_position, time_limit, routing_policy): - print("VirtualNavigation::set_parameter") # self._component.send_goal_to_nearest_robot(target_position) nearest_robot = self._component.get_nearest_robotname(target_position) self._component.send_goal_to_robot(target_position, nearest_robot) @@ -116,15 +114,10 @@ class VirtualNavigation(Event, Command, Query): self.nav_state_table[robot_name] = current_state if current_state != old_state and \ old_state is Component_Status.BUSY: - print("Adding new EVENT") msg = xmlrpclib.dumps((self.robot_goal_table[robot_name], robot_name), 'completed') self.event_queue.put(msg) + del self.robot_goal_table[robot_name] - # unnecessary ? - def poll_event(self): - print("!!!!!!!!!!!!!!!!polled!!!!!!!!!!!!!!!!!!!!!!!!!") - msg = self.event_queue.get() - return msg def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) @@ -137,6 +130,7 @@ from geometry_msgs.msg import PoseWithCovarianceStamped from functools import partial + class Component(object): def __init__(self, robot_names): self.__robot_names = robot_names @@ -148,7 +142,6 @@ class Component(object): PoseWithCovarianceStamped, partial(self.update_position, robot_name)) - def update_position(self, robot_name, amcl_pose): pos = amcl_pose.pose.pose.position self.__robot_positions[robot_name] = np.array([pos.x, pos.y]) diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index 08beb15b..54d44369 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -41,7 +41,6 @@ class Command(RoIS_Common.Command): return status def set_parameter(self, target_position, time_limit, routing_policy): - print("VirtualNavigationClient::set_parameter") status = self._proxy.set_parameter(target_position, time_limit, routing_policy) status = RoIS_HRI.ReturnCode_t(status) return status @@ -95,14 +94,8 @@ class Event(RoIS_Common.Event): def poll_event(self): """poll_event """ - print("Waiting for msg") msg = self._e_proxy.poll_event() - print("Caught msg:", msg) (params, methodname) = xmlrpc.client.loads(msg) - #self.logger.debug('poll_event: '+methodname) - print("VirtualNavigation poll_event", params,methodname) - # if methodname == 'speech_recognized': - # self.speech_recognized(params[0], params[1]) class VirtualNavigation_Client(Command, Query, Event): @@ -119,11 +112,6 @@ class VirtualNavigation_Client(Command, Query, Event): def poll_event(self): """poll_event """ - print("Waiting for msg") msg = self._e_proxy.poll_event() - print("Caught msg:", msg) (params, methodname) = xmlrpc.client.loads(msg) - print("VirtualNavigation poll_event", params,methodname) self.engine_event_queue.put(msg) - # self.event_cb("1", "completed") - # self.event_cb(command_id, "completed") diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 416269ba..c0d3744b 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -199,7 +199,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): engine_client.connect() def set_parameter(self, component_ref, parameters): - print("Using VirtualNavigation") target_component = self.component_clients[component_ref] self.command_id += 1 status = target_component.set_parameter(*parameters).value @@ -218,18 +217,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): # Is component_event_queue thread-safe ? msg = self.component_event_queue.get() (target_pos, robot_name), methodname = xmlrpc.client.loads(msg) - print("{} reaches goal ({})".format(robot_name, target_pos)) - print("goal_command_table: ", self.goal_command_table) - print("target_pos: {} ({})".format(target_pos, type(target_pos))) if self.goal_command_table[str(target_pos)] == command_id: - print("Command (id: {}) was completed by {}".format(command_id, robot_name)) + print("Command (id: {}) was completed by {} which reached goal {}."\ + .format(command_id, robot_name, target_pos)) self.completed(command_id, Component_Status.READY.value) + del self.goal_command_table[str(target_pos)] return time.sleep(3) - # (_, c_status) = component.component_status() - # if c_status == Component_Status.READY.value: - # self.completed(command_id, c_status) - # return class IF_server: diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index 7c5be10d..854568fe 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -27,7 +27,7 @@ engine.connect() for dest in [[25, 10], [0, 20], [5, 0]]: status = engine.set_parameter('Navigation', [dest, "", ""]) print(status) - time.sleep(3) + time.sleep(45) # engine.all_finished() time.sleep(30) -- GitLab From 99373d2c2387c8eb60bf528e3557fb4f26978921 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 24 Nov 2019 15:39:15 +0900 Subject: [PATCH 49/52] Fix unrecongized command event on analuze_c_status --- route_guidance_ros/scripts/main_engine.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index c0d3744b..f76f9e9d 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -223,6 +223,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.completed(command_id, Component_Status.READY.value) del self.goal_command_table[str(target_pos)] return + else: + self.component_event_queue.put(msg) time.sleep(3) -- GitLab From aab06c1e37d3daff3319a8a8be75f31cac2f81cc Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 24 Nov 2019 19:41:55 +0900 Subject: [PATCH 50/52] Upgrade test service app for multi robot service --- .../scripts/test_main_engine_with_service.py | 47 ++++++++++++------- 1 file changed, 29 insertions(+), 18 deletions(-) diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index 854568fe..60718ad2 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -7,29 +7,40 @@ import time from pyrois.RoIS_Common import Component_Status from pyrois.Service_Application_IF import Service_Application_IF from utilities import setup_multi_robot +import xmlrpc class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000"): super().__init__(uri) + self.current_dest_index = 0 + self.is_running = False def completed(self, command_id, status): print("Service command_id {} was completed.".format(command_id)) - - -process = setup_multi_robot() - -engine = Engine('http://127.0.0.1:8000') -print("Engine was established.") -time.sleep(10) -service = Service() -engine.connect() -for dest in [[25, 10], [0, 20], [5, 0]]: - status = engine.set_parameter('Navigation', [dest, "", ""]) - print(status) - time.sleep(45) -# engine.all_finished() - -time.sleep(30) -engine.disconnect() -print("finish") + self.current_dest_index += 1 + self.is_running = False + + 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, "", ""]) + print("go_robot_to {} called with command_id {}".format(dest, command_id)) + self.is_running = True + + def run(self): + destinations = [ + [25, 10], [0, 20], [5, 0], [10, 0], [25, 20], [0, 5] + ] + for dest in destinations: + self.go_robot_to(dest) + while self.is_running: + time.sleep(3) + + +if __name__ == '__main__': + process = setup_multi_robot() + time.sleep(10) + service = Service() + service.run() + print("finish") -- GitLab From 903700ba59141cf976980797ace3d62a2fe421ff Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 24 Nov 2019 20:43:56 +0900 Subject: [PATCH 51/52] Add functions to send goal and event asynchronously --- route_guidance_ros/scripts/VirtualNavigation.py | 9 +++++++++ route_guidance_ros/scripts/goal_sender_ros.py | 6 ++++++ 2 files changed, 15 insertions(+) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index c89e7c43..04104b73 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -118,6 +118,11 @@ class VirtualNavigation(Event, Command, Query): self.event_queue.put(msg) del self.robot_goal_table[robot_name] + def send_goal_and_event(self, goal, robot_name): + status = self._goal_sender.send_goal_to_robot_async(goal, robot_name) + msg = xmlrpclib.dumps((goal, robot_name), 'completed') + self.event_queue.put(msg) + def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) @@ -169,6 +174,10 @@ class Component(object): goal_sender = self.__goal_senders[robot_name] return goal_sender.get_goalsender_state() + def send_goal_to_robot_async(self, goal, robot): + return self.__goal_senders[robot].send_goal_and_wait(goal) + + def example_n(port, robot_names): print("Starting node...") component = Component(robot_names) diff --git a/route_guidance_ros/scripts/goal_sender_ros.py b/route_guidance_ros/scripts/goal_sender_ros.py index 3b69730d..d7f6f355 100644 --- a/route_guidance_ros/scripts/goal_sender_ros.py +++ b/route_guidance_ros/scripts/goal_sender_ros.py @@ -48,6 +48,12 @@ class GoalSenderROS(object): goal = pose_to_goal(pose) self.action_client.send_goal(goal) + def send_goal_and_wait(self, pose_xy): + pose_xy.append(0) + pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)] + goal = pose_to_goal(pose) + return self.action_client.send_goal_and_wait(goal) + def get_goalsender_state(self): state = self.action_client.get_state() return GoalSenderROS.GoalSenderState[state] -- GitLab From 33f8db34209d77b62a615dad2da706f3149ef3ff Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 24 Nov 2019 22:11:12 +0900 Subject: [PATCH 52/52] Use send_goal_to_robot_async on VirtualNavigation compoent --- .../scripts/VirtualNavigation.py | 31 ++++--------------- 1 file changed, 6 insertions(+), 25 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 04104b73..0abc14df 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -88,38 +88,19 @@ class VirtualNavigation(Event, Command, Query): self.robot_goal_table = {} self.nav_state_table = \ {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} - self.start_th() def set_parameter(self, target_position, time_limit, routing_policy): - # self._component.send_goal_to_nearest_robot(target_position) nearest_robot = self._component.get_nearest_robotname(target_position) - self._component.send_goal_to_robot(target_position, nearest_robot) - self.robot_goal_table[nearest_robot] = str(target_position) + th = threading.Thread( + target=self.send_goal_and_event, + args=(target_position, nearest_robot)) + th.daemon = True + th.start() status = RoIS_HRI.ReturnCode_t.OK.value return status - def component_status(self): - status = RoIS_HRI.ReturnCode_t.OK.value - c_status = self._goal_sender.get_goalsender_state().value - return (status, c_status) - - def start_th(self): - self.th = threading.Thread(target=self.nav_finish_event_loop) - self.th.start() - - def nav_finish_event_loop(self): - while True: - for robot_name, old_state in self.nav_state_table.items(): - current_state = self._component.get_nav_state(robot_name) - self.nav_state_table[robot_name] = current_state - if current_state != old_state and \ - old_state is Component_Status.BUSY: - msg = xmlrpclib.dumps((self.robot_goal_table[robot_name], robot_name), 'completed') - self.event_queue.put(msg) - del self.robot_goal_table[robot_name] - def send_goal_and_event(self, goal, robot_name): - status = self._goal_sender.send_goal_to_robot_async(goal, robot_name) + status = self._component.send_goal_to_robot_async(goal, robot_name) msg = xmlrpclib.dumps((goal, robot_name), 'completed') self.event_queue.put(msg) -- GitLab