From 31c177df184c77bcaffe406b034c0dc5a5a2fe28 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 14:50:08 +0900 Subject: [PATCH 01/20] [TMP] Add Virtual_Speech_Recognition series --- .../scripts/Dummy_Speech_Recognition.py | 0 .../scripts/Virtual_Speech_Recognition.py | 180 ++++++++++++++++++ .../Virtual_Speech_Recognition_client.py | 125 ++++++++++++ 3 files changed, 305 insertions(+) mode change 100644 => 100755 route_guidance_ros/scripts/Dummy_Speech_Recognition.py create mode 100755 route_guidance_ros/scripts/Virtual_Speech_Recognition.py create mode 100644 route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py old mode 100644 new mode 100755 diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py new file mode 100755 index 00000000..66923e74 --- /dev/null +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py @@ -0,0 +1,180 @@ +# (Dummy) Speech_Recognition.py +# + +"""Speech_Recognition +""" + +import sys +import queue +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, c): + self._component = c + + def start(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def set_parameter(self, languages, grammer, rule): + self._component.Recognizable_Languages = recognizable_languages + self._component.Languages = languages + self._component.Grammer = grammer + self._component.Rule = rule + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, c): + self._component = c + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + + def get_parameter(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return (status, self._component.Languages, self._component.Grammer, self._component.Rule) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + def poll_event(self): + """poll_event + """ + msg = self.event_queue.get() + return msg + + def speech_recognized(self, timestamp, recognized_text): + """speech_recognition + """ + msg = xmlrpc.client.dumps((timestamp, recognized_text), 'speech_recognized') + self.event_queue.put(msg) + + def speech_input_started(self, timestamp): + msg = xmlrpc.client.dumps((timestamp), 'speech_input_started') + self.event_queue.put(msg) + + def speech_input_finished(self, timestamp): + msg = xmlrpc.client.dumps((timestamp), 'speech_input_finished') + self.event_queue.put(msg) + +import random + +class Speech_Recognition(Event, Command, Query): + """Speech_Recognition + """ + def __init__(self, c): + super().__init__(c) + self._component = c + self._component.Recognizable_Languages = set("") + self._component.Languages = set("English") + self._component.Grammer = "" #??? + self._component.Rule = "" #??? + + def set_parameter(self, languages, grammer, rule): + print("Speech_Recognition::set_parameter called.") + th = threading.Thread( + target=self.wait_and_send_speech_recognized_event, + daemon=True) + th.start() + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def wait_and_send_speech_recognized_event(self): + detected_text = "point_a" + delay_duration = random.randint(3, 10) + time.sleep(delay_duration) + self.speech_recognized(datetime.now(), [detected_text]) + + def speech_recognized(self, timestamp, recognized_text): + """speech_recognition + """ + msg = (timestamp, recognized_text), 'speech_recognized' + msg = xmlrpc.client.dumps(*msg) + self.event_queue.put(msg) + + +class component: + """component + """ + def __init__(self): + self._state = False + + +def event_dispatch(sr): + """event_dispatch + """ + sr.speech_recognized(datetime.now().isoformat(), [""]) + time.sleep(0.5) + sr.speech_recognized(datetime.now().isoformat(), [""]) + + +def example_sr(port): + """example_sr + """ + c = component() + sr = Speech_Recognition(c) + + # start the timer to dispatch events + t = threading.Timer(3, event_dispatch, args=(sr,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(sr) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + if len(sys.argv) < 2: + print("python3 ") + raise RuntimeError + + port = int(sys.argv[1]) + example_sr(port) diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py new file mode 100644 index 00000000..d1280d88 --- /dev/null +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py @@ -0,0 +1,125 @@ +# (Dummy) Speech_Recognition_Client.py + +"""Speech_Recognition_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def start(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.start()) + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) + return status + + def set_parameter(self, languages, grammer, rule): + s = self._proxy.set_parameter(languages, grammer, rule) + status = RoIS_HRI.ReturnCode_t(s) + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def component_status(self): + (status, c_status) = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + def get_parameter(self): + (status, recognizable_languages, languages, grammer, rule) = self._proxy.get_parameter() + return (RoIS_HRI.ReturnCode_t(status), recognizable_languages, languages, grammer, rule) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, uri): + self._uri = uri + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + # 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) + if methodname == 'speech_recognized': + self.speech_recognized(params[0], params[1]) + + def speech_recognized(self, timestamp, recognized_text): + self.events.append((timestamp,recognized_text)) + print("speech_recognized",timestamp, recognized_text) + # self.logger.debug('received completed event' + # + command_id + # + RoIS_Service.Completed_Status(status).name) + + def speech_input_started(self, timestamp): + self.events.append((timestamp)) + print("speech_input_started", timestamp) + + def speech_input_finished(self, timestamp): + self.events.append((timestamp)) + print("speech_input_started", timestamp) + + +class Speech_Recognition_Client(Command, Query, Event): + """Speech_Recognition_Client + """ + 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.engine_event_queue = engine_event_queue + self.start_th() + + def poll_event(self): + """poll_event + """ + msg = self._e_proxy.poll_event() + (params, methodname) = xmlrpc.client.loads(msg) + print("[Speech_Recognition_Client::poll_event]", params, methodname) + msg = xmlrpc.client.dumps((methodname, params), 'completed') + self.engine_event_queue.put(msg) -- GitLab From 0f6a8738ee6ddb93a3cdd8e191badd7466c54a6f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 14:59:28 +0900 Subject: [PATCH 02/20] Modify Virtual_Speech_Recognition to specify robot --- .../scripts/Virtual_Speech_Recognition.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py index 66923e74..4cca3b6d 100755 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py @@ -107,14 +107,21 @@ import random class Speech_Recognition(Event, Command, Query): """Speech_Recognition """ - def __init__(self, c): + def __init__(self, c, robot_names): super().__init__(c) + self._robot_names = robot_names + self._target_robot = None self._component = c self._component.Recognizable_Languages = set("") self._component.Languages = set("English") self._component.Grammer = "" #??? self._component.Rule = "" #??? + def set_target_robot(self, robot_name): + if not robot_name in self._robot_names: + raise RuntimeError("[SR-server] Invalid robot name.") + self._target_robot = robot_name + def set_parameter(self, languages, grammer, rule): print("Speech_Recognition::set_parameter called.") th = threading.Thread( @@ -133,7 +140,7 @@ class Speech_Recognition(Event, Command, Query): def speech_recognized(self, timestamp, recognized_text): """speech_recognition """ - msg = (timestamp, recognized_text), 'speech_recognized' + msg = (timestamp, recognized_text, self._target_robot), 'speech_recognized' msg = xmlrpc.client.dumps(*msg) self.event_queue.put(msg) -- GitLab From b86ad23e37c0a3c5f8c1193aa74e98804f48adac Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 15:06:44 +0900 Subject: [PATCH 03/20] Get detected_text from stdin --- route_guidance_ros/scripts/Dummy_Speech_Recognition.py | 4 ++-- route_guidance_ros/scripts/Virtual_Speech_Recognition.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 66923e74..8ab1ea8e 100755 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -125,8 +125,8 @@ class Speech_Recognition(Event, Command, Query): return status def wait_and_send_speech_recognized_event(self): - detected_text = "point_a" - delay_duration = random.randint(3, 10) + detected_text = input("SR >>> ") + delay_duration = random.randint(0, 5) time.sleep(delay_duration) self.speech_recognized(datetime.now(), [detected_text]) diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py index 4cca3b6d..7b88484f 100755 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py @@ -132,8 +132,8 @@ class Speech_Recognition(Event, Command, Query): return status def wait_and_send_speech_recognized_event(self): - detected_text = "point_a" - delay_duration = random.randint(3, 10) + detected_text = input("SR >>> ") + delay_duration = random.randint(0, 5) time.sleep(delay_duration) self.speech_recognized(datetime.now(), [detected_text]) -- GitLab From 9b9154e70768f7911798742ec6600d6357445a50 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 15:20:01 +0900 Subject: [PATCH 04/20] Set robot_names from commandline-args --- .../scripts/Virtual_Speech_Recognition.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py index 7b88484f..f4d2fe18 100755 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py @@ -160,11 +160,11 @@ def event_dispatch(sr): sr.speech_recognized(datetime.now().isoformat(), [""]) -def example_sr(port): +def example_sr(port, robot_name): """example_sr """ c = component() - sr = Speech_Recognition(c) + sr = Speech_Recognition(c, robot_name) # start the timer to dispatch events t = threading.Timer(3, event_dispatch, args=(sr,)) @@ -179,9 +179,9 @@ def example_sr(port): if __name__ == '__main__': - if len(sys.argv) < 2: - print("python3 ") + if len(sys.argv) < 3: + print("python3 ...") raise RuntimeError - port = int(sys.argv[1]) - example_sr(port) + port, robot_names = int(sys.argv[1]), sys.argv[2:] + example_sr(port, robot_names) -- GitLab From f4e73814e22895b6a08072329c150554b8e1aef3 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 15:21:42 +0900 Subject: [PATCH 05/20] Launch Virtual Speech-Recognition component --- route_guidance_ros/scripts/utilities.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 75a600dd..789eeeca 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -78,6 +78,12 @@ def setup_multi_robot(): ] process.append(SubprocessWrapper(command)) print("Virtual Navigation Component Constructed.") + command = [ + "python3", "Virtual_Speech_Recognition.py", + "8042", "robot1", "robot2", "robot3" + ] + process.append(SubprocessWrapper(command)) + print("Virtual Speech-Recognition Component Constructed.") time.sleep(5) process.append(MainEngineWrapper()) print("Main Engine Constructed.") -- GitLab From fb60df20d48004b89cad087d2e769f01141c60ae Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 15:26:55 +0900 Subject: [PATCH 06/20] Remove event_dispatch function from SR components --- .../scripts/Dummy_Speech_Recognition.py | 12 ------------ .../scripts/Virtual_Speech_Recognition.py | 12 ------------ 2 files changed, 24 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 8ab1ea8e..8c964403 100755 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -145,24 +145,12 @@ class component: self._state = False -def event_dispatch(sr): - """event_dispatch - """ - sr.speech_recognized(datetime.now().isoformat(), [""]) - time.sleep(0.5) - sr.speech_recognized(datetime.now().isoformat(), [""]) - - def example_sr(port): """example_sr """ c = component() sr = Speech_Recognition(c) - # start the timer to dispatch events - t = threading.Timer(3, event_dispatch, args=(sr,)) - t.start() - # start the XML-RPC server server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) server.register_instance(sr) diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py index f4d2fe18..bf43d2fe 100755 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py @@ -152,24 +152,12 @@ class component: self._state = False -def event_dispatch(sr): - """event_dispatch - """ - sr.speech_recognized(datetime.now().isoformat(), [""]) - time.sleep(0.5) - sr.speech_recognized(datetime.now().isoformat(), [""]) - - def example_sr(port, robot_name): """example_sr """ c = component() sr = Speech_Recognition(c, robot_name) - # start the timer to dispatch events - t = threading.Timer(3, event_dispatch, args=(sr,)) - t.start() - # start the XML-RPC server server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) server.register_instance(sr) -- GitLab From a20db45115c29ac16fa2bdaa951e72ff406e18ec Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 16:04:53 +0900 Subject: [PATCH 07/20] Upgrade process to specify robot --- route_guidance_ros/scripts/Virtual_Speech_Recognition.py | 2 +- .../scripts/Virtual_Speech_Recognition_client.py | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py index bf43d2fe..f78f7cb1 100755 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py @@ -110,7 +110,7 @@ class Speech_Recognition(Event, Command, Query): def __init__(self, c, robot_names): super().__init__(c) self._robot_names = robot_names - self._target_robot = None + self._target_robot = "robot1" self._component = c self._component.Recognizable_Languages = set("") self._component.Languages = set("English") diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py index d1280d88..f7ba45da 100644 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py @@ -123,3 +123,6 @@ class Speech_Recognition_Client(Command, Query, Event): print("[Speech_Recognition_Client::poll_event]", params, methodname) msg = xmlrpc.client.dumps((methodname, params), 'completed') self.engine_event_queue.put(msg) + + def set_target_robot(self, robot_name): + self._e_proxy.set_target_robot(robot_name) -- GitLab From 14cf5f69e9b317de5f3684f6cf3f05364bf49475 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 16:05:32 +0900 Subject: [PATCH 08/20] Upgrade main_engine to handle SpeechRecognition component --- route_guidance_ros/scripts/main_engine.py | 37 +++++++++++++---------- 1 file changed, 21 insertions(+), 16 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index eb2f3f9d..b10c12b0 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -216,7 +216,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.command_id_result_table[str(self.command_id)].append(results) status = RoIS_HRI.ReturnCode_t.OK.value elif component_ref == 'Speech_Recognition': - pass + target_component = self.component_clients[component_ref] + status = target_component.set_parameter(*parameters).value return (status, str(self.command_id)) @@ -227,21 +228,25 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def analyze_c_status(self): while True: msg = self.component_event_queue.get() - (target_pos, robot_name, status), methodname = xmlrpc.client.loads(msg) - command_id = self.goal_command_table[str(target_pos)] - status = Component_Status(status) - if status == Component_Status.READY: - print(colored("Command (id: {}) was succeeded by {} which reached goal {}."\ - .format(command_id, robot_name, target_pos), "green")) - self.completed(command_id, Completed_Status.OK.value) - del self.goal_command_table[str(target_pos)] - time.sleep(3) - elif status == Component_Status.ERROR: - print(colored("Command (id: {}) was aborted by {} which couldn't reach goal {}."\ - .format(command_id, robot_name, target_pos), "red")) - self.completed(command_id, Completed_Status.ABORT.value) - else: - raise RuntimeError("Unhandled kind of vnav-status.") + params, methodname = xmlrpc.client.loads(msg) + if methodname == 'nav_finished': + target_pos, robot_name, status = params + command_id = self.goal_command_table[str(target_pos)] + status = Component_Status(status) + if status == Component_Status.READY: + print(colored("Command (id: {}) was succeeded by {} which reached goal {}."\ + .format(command_id, robot_name, target_pos), "green")) + self.completed(command_id, Completed_Status.OK.value) + del self.goal_command_table[str(target_pos)] + time.sleep(3) + elif status == Component_Status.ERROR: + print(colored("Command (id: {}) was aborted by {} which couldn't reach goal {}."\ + .format(command_id, robot_name, target_pos), "red")) + self.completed(command_id, Completed_Status.ABORT.value) + else: + raise RuntimeError("Unhandled kind of vnav-status.") + elif methodname == 'speech_recognized': + print("Speech_Recognition EVENT!!!") time.sleep(3) -- GitLab From 0539202dbc4d8821971c11ff01eb8c35079bd311 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 16:06:01 +0900 Subject: [PATCH 09/20] Upgrade test script to test VirtualSpeechRecognition --- .../scripts/test_main_engine_with_service.py | 10 ++++++++++ 1 file changed, 10 insertions(+) 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 73e97a1c..bd0d3c6e 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -38,6 +38,8 @@ class Service(Service_Application_IF): def update_robot_positions(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) + self._proxy.set_target_robot("robot3") + time.sleep(3) (return_code, command_id) = self._proxy.set_parameter('RobotPosition') self.commandid_task_table[command_id] = 'RobotPosition' time.sleep(2) @@ -47,10 +49,18 @@ class Service(Service_Application_IF): def get_robot_positions(self): return self.robot_positions + def speech_recognition(self, robot_name): + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + (return_code, command_id) = self._proxy.set_parameter('Speech_Recognition', "English", "", "") + time.sleep(15) + + def run(self): destinations = [ "point_b", "point_h", [5, 0, 0], "point_e", "point_a", [0, 5, 0] ] + self.speech_recognition("robot3") for dest in destinations: self.go_robot_to(dest) while self.is_running: -- GitLab From 378e26940ce5cd85a4458dbbe847bc7208646cb8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sat, 25 Jan 2020 16:58:28 +0900 Subject: [PATCH 10/20] Add debug code --- route_guidance_ros/scripts/Virtual_Speech_Recognition.py | 1 + route_guidance_ros/scripts/test_main_engine_with_service.py | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py index f78f7cb1..245c21a4 100755 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py @@ -121,6 +121,7 @@ class Speech_Recognition(Event, Command, Query): if not robot_name in self._robot_names: raise RuntimeError("[SR-server] Invalid robot name.") self._target_robot = robot_name + print("target robot is set as ", self._target_robot) def set_parameter(self, languages, grammer, rule): print("Speech_Recognition::set_parameter called.") 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 bd0d3c6e..cecacd26 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -38,8 +38,6 @@ class Service(Service_Application_IF): def update_robot_positions(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) - self._proxy.set_target_robot("robot3") - time.sleep(3) (return_code, command_id) = self._proxy.set_parameter('RobotPosition') self.commandid_task_table[command_id] = 'RobotPosition' time.sleep(2) -- GitLab From f67462808845f2bf83c6ed8758f08d7feee35b90 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 12:13:14 +0900 Subject: [PATCH 11/20] Fix methodname of navigation finished event --- route_guidance_ros/scripts/VirtualNavigation.py | 2 +- route_guidance_ros/scripts/main_engine.py | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index bf7a35a8..1b9cb0c2 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -126,7 +126,7 @@ class VirtualNavigation(Event, Command, Query): status = self._component.send_goal_to_robot_async(goal_coord, robot_name) self.nav_state_table[robot_name] = status print(self.nav_state_table) - msg = xmlrpclib.dumps((goal, robot_name, status.value), 'navi_finished') + msg = xmlrpclib.dumps((goal, robot_name, status.value), 'nav_finished') self.event_queue.put(msg) def back_stucked_robots_to_init_pos(self): diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index b10c12b0..e6e9c771 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -247,6 +247,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): raise RuntimeError("Unhandled kind of vnav-status.") elif methodname == 'speech_recognized': print("Speech_Recognition EVENT!!!") + else: + raise RuntimeError("[main_engine]Unknown method-name of event.") time.sleep(3) -- GitLab From 970d0e1413681ba1e087f83b2ba3879e866afb10 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 16:32:07 +0900 Subject: [PATCH 12/20] Add ListenDest task on service_app_lv2 --- route_guidance_ros/scripts/service_app_lv2.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index f5b54273..bf065c14 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -17,6 +17,7 @@ from transitions import Machine class Task(enum.Enum): Init = 'Init' NaviToUser = 'NaviToUser' + ListenDest = 'ListenDest' NaviToGuide = 'NaviToGuide' End = 'End' @@ -25,10 +26,11 @@ class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) # buid state machine - states = [Task.Init, Task.NaviToUser, Task.NaviToGuide, Task.End] + states = [Task.Init, Task.NaviToUser, Task.ListenDest, Task.NaviToGuide, Task.End] transitions = [ {'trigger': 'StartService', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, - {'trigger': 'NaviSucceeded', 'source': Task.NaviToUser, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'NaviSucceeded', 'source': Task.NaviToUser, 'dest': Task.ListenDest, 'after': 'listen_destination' }, + {'trigger': 'NaviSucceeded', 'source': Task.ListenDest, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, {'trigger': 'NaviFailed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, {'trigger': 'NaviSucceeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, ] @@ -57,6 +59,10 @@ class Service(Service_Application_IF): (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) + def listen_destination(self, event): + print(colored("ListenDest started.", 'cyan')) + (return_code, command_id) = self._proxy.set_parameter('Speech_Recognition', "", "", "") + def start_navi_to_guide(self, event): dest = event.args[0] print(colored("NaviToGuide started.", 'cyan')) -- GitLab From f6cd61e26fec411d8f2711d7d6e39c7918411bc1 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 16:40:46 +0900 Subject: [PATCH 13/20] Fix using Virtual_Speech_Recognition_client on main_engine --- route_guidance_ros/scripts/main_engine.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index e6e9c771..527bcb1d 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -169,7 +169,7 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) from VirtualNavigation_client import VirtualNavigation_Client as VNavClient -from Dummy_Speech_Recognition_client import Speech_Recognition_Client as SRecogClient +from Virtual_Speech_Recognition_client import Speech_Recognition_Client as SRecogClient from sub_engine_client import IF as EngineClient from pyrois.RoIS_Common import Component_Status from pyrois.RoIS_Service import Completed_Status -- GitLab From 09e345a9fae5c3345504726fc3cb06ea5a7d0fcd Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 16:42:50 +0900 Subject: [PATCH 14/20] Remove unnecessary code on Virtual_Speech_Recognition_client --- .../scripts/Virtual_Speech_Recognition_client.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py index f7ba45da..4fd66089 100644 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py +++ b/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py @@ -119,9 +119,6 @@ class Speech_Recognition_Client(Command, Query, Event): """poll_event """ msg = self._e_proxy.poll_event() - (params, methodname) = xmlrpc.client.loads(msg) - print("[Speech_Recognition_Client::poll_event]", params, methodname) - msg = xmlrpc.client.dumps((methodname, params), 'completed') self.engine_event_queue.put(msg) def set_target_robot(self, robot_name): -- GitLab From 1f092706607afd9579ea5ac13c0a185a10ff6983 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 17:17:06 +0900 Subject: [PATCH 15/20] Upgrade main_engine to send SR-event to service --- route_guidance_ros/scripts/main_engine.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 527bcb1d..e38d9c19 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -197,6 +197,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): for engine_client in self.engine_clients.values(): engine_client.connect() self.command_id_result_table = {} + self.next_sr_robot = "robot1" + self.sr_robot_command_table = {} + th = threading.Thread(target=self.analyze_c_status, daemon=True) th.start() @@ -217,6 +220,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): status = RoIS_HRI.ReturnCode_t.OK.value elif component_ref == 'Speech_Recognition': target_component = self.component_clients[component_ref] + self.sr_robot_command_table[self.next_sr_robot] = self.command_id status = target_component.set_parameter(*parameters).value return (status, str(self.command_id)) @@ -229,6 +233,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): while True: msg = self.component_event_queue.get() params, methodname = xmlrpc.client.loads(msg) + print(f"Received event: methodname => {methodname}, params => {params}") if methodname == 'nav_finished': target_pos, robot_name, status = params command_id = self.goal_command_table[str(target_pos)] @@ -236,6 +241,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): if status == Component_Status.READY: print(colored("Command (id: {}) was succeeded by {} which reached goal {}."\ .format(command_id, robot_name, target_pos), "green")) + # sr_component = self.component_clients['Speech_Recognition'] + # sr_component.set_target_robot(robot_name) self.completed(command_id, Completed_Status.OK.value) del self.goal_command_table[str(target_pos)] time.sleep(3) @@ -247,6 +254,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): raise RuntimeError("Unhandled kind of vnav-status.") elif methodname == 'speech_recognized': print("Speech_Recognition EVENT!!!") + timestamp, recognized_text, target_robot = params + command_id = self.sr_robot_command_table[target_robot] + self.completed(command_id, Completed_Status.OK.value) else: raise RuntimeError("[main_engine]Unknown method-name of event.") time.sleep(3) -- GitLab From 5f811f573f8cfeabe238f2a5af31c90bfa4db4ae Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 17:45:43 +0900 Subject: [PATCH 16/20] Add variable to stock command result --- route_guidance_ros/scripts/main_engine.py | 7 +++++++ route_guidance_ros/scripts/service_app_lv2.py | 1 + 2 files changed, 8 insertions(+) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index e38d9c19..1f0139cc 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -199,6 +199,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.command_id_result_table = {} self.next_sr_robot = "robot1" self.sr_robot_command_table = {} + self.command_results = {} th = threading.Thread(target=self.analyze_c_status, daemon=True) th.start() @@ -256,11 +257,17 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): print("Speech_Recognition EVENT!!!") timestamp, recognized_text, target_robot = params command_id = self.sr_robot_command_table[target_robot] + self.command_results[command_id] = recognized_text[0] self.completed(command_id, Completed_Status.OK.value) else: raise RuntimeError("[main_engine]Unknown method-name of event.") time.sleep(3) + def get_command_result(self, command_id, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + results = self.command_results[command_id] + return (status, results) + class IF_server: """IF_Server diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index bf065c14..5cfd568d 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -39,6 +39,7 @@ class Service(Service_Application_IF): self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) def completed(self, command_id, completed_status): + print("command_id: ", command_id, " completed.") completed_status = Completed_Status(completed_status) if completed_status == Completed_Status.OK: self.NaviSucceeded(self.dest_pos) -- GitLab From d75154e309eb33426477e40999aef2f18a2aa060 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 17:50:49 +0900 Subject: [PATCH 17/20] Change trigger name on service-state-machine --- route_guidance_ros/scripts/service_app_lv2.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 5cfd568d..356c26a6 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -28,11 +28,11 @@ class Service(Service_Application_IF): # buid state machine states = [Task.Init, Task.NaviToUser, Task.ListenDest, Task.NaviToGuide, Task.End] transitions = [ - {'trigger': 'StartService', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, - {'trigger': 'NaviSucceeded', 'source': Task.NaviToUser, 'dest': Task.ListenDest, 'after': 'listen_destination' }, - {'trigger': 'NaviSucceeded', 'source': Task.ListenDest, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, - {'trigger': 'NaviFailed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, - {'trigger': 'NaviSucceeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, + {'trigger': 'Start', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'Succeeded', 'source': Task.NaviToUser, 'dest': Task.ListenDest, 'after': 'listen_destination' }, + {'trigger': 'Succeeded', 'source': Task.ListenDest, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'Failed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'Succeeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, ] self.target_pos = "point_c" self.dest_pos = "point_a" @@ -42,9 +42,9 @@ class Service(Service_Application_IF): print("command_id: ", command_id, " completed.") completed_status = Completed_Status(completed_status) if completed_status == Completed_Status.OK: - self.NaviSucceeded(self.dest_pos) + self.Succeeded(self.dest_pos) elif completed_status == Completed_Status.ABORT: - self.NaviFailed(self.target_pos) + self.Failed(self.target_pos) def notify_error(self, error_id, error_type): print('received error event {}({}) '.format( \ @@ -76,7 +76,7 @@ class Service(Service_Application_IF): def run(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) - self.StartService(self.target_pos) + self.Start(self.target_pos) if __name__ == '__main__': -- GitLab From 49ad461841e2605df0f4e24081ee116944e226ba Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 19:15:36 +0900 Subject: [PATCH 18/20] Modify service app to use result of speech recognition --- route_guidance_ros/scripts/service_app_lv2.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 356c26a6..6c43552e 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -37,6 +37,7 @@ class Service(Service_Application_IF): self.target_pos = "point_c" self.dest_pos = "point_a" self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) + self.command_task_table = {} def completed(self, command_id, completed_status): print("command_id: ", command_id, " completed.") @@ -58,16 +59,23 @@ class Service(Service_Application_IF): dest = event.args[0] print(colored("NaviToUser started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") + self.command_task_table[command_id] = Task.NaviToUser stautus = RoIS_HRI.ReturnCode_t(return_code) def listen_destination(self, event): print(colored("ListenDest started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Speech_Recognition', "", "", "") + self.command_task_table[command_id] = Task.ListenDest def start_navi_to_guide(self, event): dest = event.args[0] + latest_listen_command_id = [command_id for command_id, task in self.command_task_table.items() if task == Task.ListenDest][-1] + status, results = self._proxy.get_command_result(latest_listen_command_id, "") + dest = results + print("dest is ", dest) print(colored("NaviToGuide started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") + self.command_task_table[command_id] = Task.NaviToGuide stautus = RoIS_HRI.ReturnCode_t(return_code) def end_service(self, event): -- GitLab From 37a2a1ec48d15d192f0254c0341f15d36631d3b7 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 19:23:07 +0900 Subject: [PATCH 19/20] Fix command_id key type --- route_guidance_ros/scripts/main_engine.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 1f0139cc..a14ae7db 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): self.command_id_result_table = {} self.next_sr_robot = "robot1" self.sr_robot_command_table = {} - self.command_results = {} th = threading.Thread(target=self.analyze_c_status, daemon=True) th.start() @@ -226,10 +225,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): return (status, str(self.command_id)) - def get_command_result(self, command_id, condition): - status = RoIS_HRI.ReturnCode_t.OK.value - return status, self.command_id_result_table[command_id] - def analyze_c_status(self): while True: msg = self.component_event_queue.get() @@ -257,7 +252,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): print("Speech_Recognition EVENT!!!") timestamp, recognized_text, target_robot = params command_id = self.sr_robot_command_table[target_robot] - self.command_results[command_id] = recognized_text[0] + self.command_id_result_table[str(command_id)] = recognized_text[0] self.completed(command_id, Completed_Status.OK.value) else: raise RuntimeError("[main_engine]Unknown method-name of event.") @@ -265,7 +260,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def get_command_result(self, command_id, condition): status = RoIS_HRI.ReturnCode_t.OK.value - results = self.command_results[command_id] + results = self.command_id_result_table[command_id] return (status, results) -- GitLab From ffbc3dd0ea88ad7aaefc0c8d19dac3cde668368d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 26 Jan 2020 19:26:50 +0900 Subject: [PATCH 20/20] Remove unnecessary code on service_app_lv2 --- route_guidance_ros/scripts/service_app_lv2.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 6c43552e..8cf8f76c 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -35,7 +35,6 @@ class Service(Service_Application_IF): {'trigger': 'Succeeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, ] self.target_pos = "point_c" - self.dest_pos = "point_a" self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) self.command_task_table = {} @@ -43,7 +42,7 @@ class Service(Service_Application_IF): print("command_id: ", command_id, " completed.") completed_status = Completed_Status(completed_status) if completed_status == Completed_Status.OK: - self.Succeeded(self.dest_pos) + self.Succeeded() elif completed_status == Completed_Status.ABORT: self.Failed(self.target_pos) @@ -68,7 +67,6 @@ class Service(Service_Application_IF): self.command_task_table[command_id] = Task.ListenDest def start_navi_to_guide(self, event): - dest = event.args[0] latest_listen_command_id = [command_id for command_id, task in self.command_task_table.items() if task == Task.ListenDest][-1] status, results = self._proxy.get_command_result(latest_listen_command_id, "") dest = results -- GitLab