From 70633b53baa10d20dcd0a2aff8bf7ea105a9f024 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 17:08:52 +0900 Subject: [PATCH 01/26] Use non virtual speech-recognition on main_engine --- route_guidance_ros/scripts/main_engine.py | 6 ++++-- route_guidance_ros/scripts/utilities.py | 6 ------ 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index a14ae7db..a7cb01fb 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 Virtual_Speech_Recognition_client import Speech_Recognition_Client as SRecogClient +from Dummy_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 @@ -186,7 +186,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.component_event_queue = queue.Queue() self.component_clients = { 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue), - 'Speech_Recognition': SRecogClient('http://localhost:8042', self.component_event_queue) + 'Speech_Recognition1': SRecogClient('http://localhost:8013', self.component_event_queue) + 'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue) + 'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue) } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 789eeeca..75a600dd 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -78,12 +78,6 @@ 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 4ed41c1240eb1ecb6a47fcdbfd99e39d567947c8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 17:11:27 +0900 Subject: [PATCH 02/26] Avoid to use SpeechRecognition by set_parameter --- 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 a7cb01fb..5278c9e3 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -221,9 +221,10 @@ 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': - 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 + print("Speech_Recognition was called by set_parameter") + # 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)) -- GitLab From 4bbb1a69bcaeb945614351440f83e91e388722cb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 17:31:59 +0900 Subject: [PATCH 03/26] Modify Service and MainEngine to subscribe event --- route_guidance_ros/scripts/main_engine.py | 18 ++++++++++++++++++ route_guidance_ros/scripts/service_app_lv2.py | 1 + 2 files changed, 19 insertions(+) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 5278c9e3..747b4d7d 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -182,6 +182,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def __init__(self, Engine): super().__init__(Engine) self.command_id = 0 + self.event_id = 0 + self.subscribe_id = 0 self.event_queue = queue.Queue() self.component_event_queue = queue.Queue() self.component_clients = { @@ -199,6 +201,8 @@ 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.subscribe_id_type_table = {} + self.event_id_result_table = {} self.next_sr_robot = "robot1" self.sr_robot_command_table = {} @@ -266,6 +270,20 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): results = self.command_id_result_table[command_id] return (status, results) + def subscribe(self, event_type, condition): + print("MainEngine::subscribe called.") + status = RoIS_HRI.ReturnCode_t.OK.value + self.subscribe_id += 1 + subscribe_id = str(self.subscribe_id) + self.subscribe_id_type_table[subscribe_id] = event_type + return (status, subscribe_id) + + def get_event_detail(self, event_id, condition): + print("MainEngine::get_event_detail called.") + status = RoIS_HRI.ReturnCode_t.OK.value + results = self.event_id_result_table[event_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 ce039c5a..54bb5ddf 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -82,6 +82,7 @@ class Service(Service_Application_IF): def run(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) + return_code, subscribe_id = self._proxy.subscribe('speech_recognized', "") self.Start(self.target_pos) -- GitLab From 8e871023f9fb7804573d97cf23333ede4b978ab0 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 17:48:25 +0900 Subject: [PATCH 04/26] Make Dummy_Speech_Recognition ROS node --- .../scripts/Dummy_Speech_Recognition.py | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 8c964403..33f8892a 100755 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python + # (Dummy) Speech_Recognition.py # @@ -5,7 +7,8 @@ """ import sys -import queue +from six.moves import queue +from six.moves import xmlrpc_client import time from datetime import datetime import threading @@ -91,36 +94,39 @@ class Event(RoIS_Common.Event): def speech_recognized(self, timestamp, recognized_text): """speech_recognition """ - msg = xmlrpc.client.dumps((timestamp, recognized_text), 'speech_recognized') + 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') + 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') + msg = xmlrpc_client.dumps((timestamp), 'speech_input_finished') self.event_queue.put(msg) import random +import rospy +from std_msgs.msg import String class Speech_Recognition(Event, Command, Query): """Speech_Recognition """ def __init__(self, c): - super().__init__(c) + super(Speech_Recognition, self).__init__(c) self._component = c self._component.Recognizable_Languages = set("") self._component.Languages = set("English") self._component.Grammer = "" #??? self._component.Rule = "" #??? + rospy.Subscriber("detected_text", String, self.sr_callback) 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() + # th = threading.Thread( + # target=self.wait_and_send_speech_recognized_event, + # daemon=True) + # th.start() status = RoIS_HRI.ReturnCode_t.OK.value return status @@ -130,11 +136,16 @@ class Speech_Recognition(Event, Command, Query): time.sleep(delay_duration) self.speech_recognized(datetime.now(), [detected_text]) + def sr_callback(self, text): + detected_text = text.data + print("SR: detected_text => ", detected_text) + 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) + msg = xmlrpc_client.dumps(*msg) self.event_queue.put(msg) @@ -165,4 +176,5 @@ if __name__ == '__main__': raise RuntimeError port = int(sys.argv[1]) + rospy.init_node('speech_recognition_component', anonymous=True) example_sr(port) -- GitLab From 31899d5d65f1e2294d9beb39165abcad9b8aa19f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 18:05:00 +0900 Subject: [PATCH 05/26] Fix launching error --- route_guidance_ros/scripts/main_engine.py | 6 +++--- route_guidance_ros/scripts/utilities.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 747b4d7d..735d9c10 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -188,9 +188,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.component_event_queue = queue.Queue() self.component_clients = { 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue), - 'Speech_Recognition1': SRecogClient('http://localhost:8013', self.component_event_queue) - 'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue) - 'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue) + 'Speech_Recognition1': SRecogClient('http://localhost:8013', self.component_event_queue), + 'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue), + 'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue), } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 75a600dd..d4517e3f 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -36,7 +36,7 @@ class SysInfoComponent(ComponentWrapper): class SpeechRecogComponent(SubprocessWrapper): def __init__(self, port): - super().__init__(["python3", "Dummy_Speech_Recognition.py", str(port)]) + super().__init__(["rosrun", "route_guidance_ros", "Dummy_Speech_Recognition.py", str(port)]) class SubEngineWrapper(SubprocessWrapper): -- GitLab From 5a469b176e6c664fb60124921cd17e1ab61b1402 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 18:24:44 +0900 Subject: [PATCH 06/26] Split event analyzer for VNav and Other components --- route_guidance_ros/scripts/main_engine.py | 54 ++++++++++++++--------- 1 file changed, 32 insertions(+), 22 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 735d9c10..6e455e40 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -184,10 +184,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.command_id = 0 self.event_id = 0 self.subscribe_id = 0 + self.vnav_event_queue = queue.Queue() self.event_queue = queue.Queue() self.component_event_queue = queue.Queue() self.component_clients = { - 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue), + 'Navigation': VNavClient('http://localhost:8041', self.vnav_event_queue), 'Speech_Recognition1': SRecogClient('http://localhost:8013', self.component_event_queue), 'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue), 'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue), @@ -206,7 +207,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.next_sr_robot = "robot1" self.sr_robot_command_table = {} - th = threading.Thread(target=self.analyze_c_status, daemon=True) + th = threading.Thread(target=self.analyze_vnav_status, daemon=True) + th.start() + th = threading.Thread(target=self.monitor_event, daemon=True) th.start() def set_parameter(self, component_ref, *parameters): @@ -232,30 +235,35 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): return (status, str(self.command_id)) - def analyze_c_status(self): + def analyze_vnav_status(self): + while True: + msg = self.vnav_event_queue.get() + params, methodname = xmlrpc.client.loads(msg) + 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")) + # 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) + 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.") + time.sleep(3) + + def monitor_event(self): 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)] - 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")) - # 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) - 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': + if methodname == 'speech_recognized': print("Speech_Recognition EVENT!!!") timestamp, recognized_text, target_robot = params command_id = self.sr_robot_command_table[target_robot] @@ -265,6 +273,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 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_id_result_table[command_id] -- GitLab From 401910a8931b93a86871efe18f6747f24e0ba23e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 18:33:17 +0900 Subject: [PATCH 07/26] Modify MainEngine::subscribe function --- route_guidance_ros/scripts/main_engine.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 6e455e40..8a4e6b26 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -202,7 +202,7 @@ 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.subscribe_id_type_table = {} + self.event_ref_subscribe_id_table = {} self.event_id_result_table = {} self.next_sr_robot = "robot1" self.sr_robot_command_table = {} @@ -274,7 +274,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): time.sleep(3) - def get_command_result(self, command_id, condition): status = RoIS_HRI.ReturnCode_t.OK.value results = self.command_id_result_table[command_id] @@ -285,7 +284,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): status = RoIS_HRI.ReturnCode_t.OK.value self.subscribe_id += 1 subscribe_id = str(self.subscribe_id) - self.subscribe_id_type_table[subscribe_id] = event_type + self.event_ref_subscribe_id_table[event_type+condition] = subscribe_id return (status, subscribe_id) def get_event_detail(self, event_id, condition): -- GitLab From 881544e06b80b11048b6f5775ce6a3fe54e69f7d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 18:42:06 +0900 Subject: [PATCH 08/26] Modify to create event notiyfcation for SR --- route_guidance_ros/scripts/main_engine.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 8a4e6b26..3ce2180b 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -205,7 +205,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.event_ref_subscribe_id_table = {} self.event_id_result_table = {} self.next_sr_robot = "robot1" - self.sr_robot_command_table = {} th = threading.Thread(target=self.analyze_vnav_status, daemon=True) th.start() @@ -230,7 +229,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): elif component_ref == 'Speech_Recognition': print("Speech_Recognition was called by set_parameter") # 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)) @@ -261,14 +259,16 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def monitor_event(self): while True: msg = self.component_event_queue.get() + self.event_id += 1 + event_id = str(self.event_id) params, methodname = xmlrpc.client.loads(msg) print(f"Received event: methodname => {methodname}, params => {params}") if methodname == 'speech_recognized': print("Speech_Recognition EVENT!!!") timestamp, recognized_text, target_robot = params - command_id = self.sr_robot_command_table[target_robot] - self.command_id_result_table[str(command_id)] = recognized_text[0] self.completed(command_id, Completed_Status.OK.value) + subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'] + self.notify_event(event_id, 'speech_recognized', subscribe_id, "") else: raise RuntimeError("[main_engine]Unknown method-name of event.") time.sleep(3) -- GitLab From 5c33b2418987646ab6bd714d2fa1599dcce72c04 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 18:57:52 +0900 Subject: [PATCH 09/26] [TMP] Use one Speech_Recognition component --- route_guidance_ros/scripts/main_engine.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 3ce2180b..6879d033 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -190,8 +190,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.component_clients = { 'Navigation': VNavClient('http://localhost:8041', self.vnav_event_queue), 'Speech_Recognition1': SRecogClient('http://localhost:8013', self.component_event_queue), - 'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue), - 'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue), + # 'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue), + # 'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue), } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), @@ -266,7 +266,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): if methodname == 'speech_recognized': print("Speech_Recognition EVENT!!!") timestamp, recognized_text, target_robot = params - self.completed(command_id, Completed_Status.OK.value) subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'] self.notify_event(event_id, 'speech_recognized', subscribe_id, "") else: -- GitLab From 4e8f3b0416e20050a24192568a1bbc166f1de6a5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 19:24:54 +0900 Subject: [PATCH 10/26] Add robot_name property on Speech_Recognition server --- route_guidance_ros/scripts/Dummy_Speech_Recognition.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 33f8892a..17b6e63c 100755 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -120,6 +120,7 @@ class Speech_Recognition(Event, Command, Query): self._component.Grammer = "" #??? self._component.Rule = "" #??? rospy.Subscriber("detected_text", String, self.sr_callback) + self.robot_name = "robot1" def set_parameter(self, languages, grammer, rule): print("Speech_Recognition::set_parameter called.") @@ -144,7 +145,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.robot_name), 'speech_recognized' msg = xmlrpc_client.dumps(*msg) self.event_queue.put(msg) -- GitLab From c42bfaab11801d15774bebb8ddc6487ec03305aa Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 19:25:40 +0900 Subject: [PATCH 11/26] Fix event parameter format --- route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py | 1 - 1 file changed, 1 deletion(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py index d1280d88..38151fab 100644 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py @@ -121,5 +121,4 @@ class Speech_Recognition_Client(Command, Query, 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 a39fad2ed5e9aeb2c4b8e863f9f9daab7c7ea960 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 19:30:54 +0900 Subject: [PATCH 12/26] Fix typo on MainEngine event-handler --- route_guidance_ros/scripts/main_engine.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 6879d033..6a18d638 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -267,12 +267,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): print("Speech_Recognition EVENT!!!") timestamp, recognized_text, target_robot = params subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'] - self.notify_event(event_id, 'speech_recognized', subscribe_id, "") + self.notify_event(event_id, 'speech_recognized', subscribed_id, "") 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_id_result_table[command_id] -- GitLab From f08d8c94e0904dff23b1535f912e4f090085dc21 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 20:02:16 +0900 Subject: [PATCH 13/26] Comment-out unnecessary print code for debug --- route_guidance_ros/scripts/Dummy_Speech_Recognition.py | 2 +- route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 17b6e63c..dac5986d 100755 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -139,7 +139,7 @@ class Speech_Recognition(Event, Command, Query): def sr_callback(self, text): detected_text = text.data - print("SR: detected_text => ", detected_text) + # print("SR: detected_text => ", detected_text) self.speech_recognized(datetime.now(), [detected_text]) def speech_recognized(self, timestamp, recognized_text): diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py index 38151fab..a4e526d8 100644 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py @@ -120,5 +120,5 @@ class Speech_Recognition_Client(Command, Query, Event): """ msg = self._e_proxy.poll_event() (params, methodname) = xmlrpc.client.loads(msg) - print("[Speech_Recognition_Client::poll_event]", params, methodname) + # print("[Speech_Recognition_Client::poll_event]", params, methodname) self.engine_event_queue.put(msg) -- GitLab From 927f4a4dd610d6dd2273180afa46b9a8fb99e639 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 20:02:48 +0900 Subject: [PATCH 14/26] Modify service_app to wait for event from SR MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit インタプリタからの操作じゃないとできない (Service の notify_event が呼ばれない) --- route_guidance_ros/scripts/service_app_lv2.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 54bb5ddf..0b67fa43 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -51,8 +51,10 @@ class Service(Service_Application_IF): error_id, RoIS_Service.ErrorType(error_type).name)) def notify_event(self, event_id, event_type, subscribe_id, expire): - print('received event {} {} {} {}'.format( \ - event_id, event_type, subscribe_id, expire)) + print("Service::notify_event called.") + if event_type == 'speech_recognized' and self.state == Task.ListenDest: + print(colored("speech recognized successfully.", 'green')) + self.Succeeded() def start_navi_to_user(self, event): dest = self.target_pos @@ -63,8 +65,7 @@ class Service(Service_Application_IF): 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): latest_listen_command_id = [command_id for command_id, task in self.command_task_table.items() if task == Task.ListenDest][-1] -- GitLab From 25997f97611093760e1703b2351eb7df5e048357 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 28 Jan 2020 20:23:22 +0900 Subject: [PATCH 15/26] Use result of SR for NaviToGuide MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit /detected_text を publish しても 1/2 回しかイベントが発生しない --- route_guidance_ros/scripts/main_engine.py | 1 + route_guidance_ros/scripts/service_app_lv2.py | 8 +++++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 6a18d638..5ad81573 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -267,6 +267,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): print("Speech_Recognition EVENT!!!") timestamp, recognized_text, target_robot = params subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'] + self.event_id_result_table[event_id] = params self.notify_event(event_id, 'speech_recognized', subscribed_id, "") else: raise RuntimeError("[main_engine]Unknown method-name of event.") diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 0b67fa43..0bde1efa 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.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) self.command_task_table = {} + self.latest_event_id_table = {} def completed(self, command_id, completed_status): print("command_id: ", command_id, " completed.") @@ -53,6 +54,7 @@ class Service(Service_Application_IF): def notify_event(self, event_id, event_type, subscribe_id, expire): print("Service::notify_event called.") if event_type == 'speech_recognized' and self.state == Task.ListenDest: + self.latest_event_id_table['speech_recognized'] = event_id print(colored("speech recognized successfully.", 'green')) self.Succeeded() @@ -68,9 +70,9 @@ class Service(Service_Application_IF): def start_navi_to_guide(self, event): - 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 + sr_event_id = self.latest_event_id_table['speech_recognized'] + status, results = self._proxy.get_event_detail(sr_event_id, "") + dest = results[1][0] print("dest is ", dest) print(colored("NaviToGuide started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") -- GitLab From d17295d654180192356c6712e674500aff37de74 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 13:34:09 +0900 Subject: [PATCH 16/26] Add robot_name argument to SR server --- .../scripts/Dummy_Speech_Recognition.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index dac5986d..e7e65491 100755 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -112,7 +112,7 @@ from std_msgs.msg import String class Speech_Recognition(Event, Command, Query): """Speech_Recognition """ - def __init__(self, c): + def __init__(self, c, robot_name): super(Speech_Recognition, self).__init__(c) self._component = c self._component.Recognizable_Languages = set("") @@ -120,7 +120,7 @@ class Speech_Recognition(Event, Command, Query): self._component.Grammer = "" #??? self._component.Rule = "" #??? rospy.Subscriber("detected_text", String, self.sr_callback) - self.robot_name = "robot1" + self.robot_name = robot_name def set_parameter(self, languages, grammer, rule): print("Speech_Recognition::set_parameter called.") @@ -157,11 +157,11 @@ class component: self._state = False -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 XML-RPC server server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) @@ -173,9 +173,9 @@ def example_sr(port): if __name__ == '__main__': if len(sys.argv) < 2: - print("python3 ") + print("rosrun ") raise RuntimeError - port = int(sys.argv[1]) + port, robot_name = int(sys.argv[1]), sys.argv[2] rospy.init_node('speech_recognition_component', anonymous=True) - example_sr(port) + example_sr(port, robot_name) -- GitLab From a8220f1409b1141be288e49ecda86d56ead7e14c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 13:39:08 +0900 Subject: [PATCH 17/26] Modify SR component launch process --- route_guidance_ros/scripts/utilities.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index d4517e3f..8554eedb 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -34,9 +34,9 @@ class SysInfoComponent(ComponentWrapper): super().__init__("System_Information.py", robot_name, port) -class SpeechRecogComponent(SubprocessWrapper): - def __init__(self, port): - super().__init__(["rosrun", "route_guidance_ros", "Dummy_Speech_Recognition.py", str(port)]) +class SpeechRecogComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("Dummy_Speech_Recognition.py", robot_name, port) class SubEngineWrapper(SubprocessWrapper): @@ -54,7 +54,7 @@ def launch_components_and_subengines(**robot_port_table): for robot_name, engine_port in robot_port_table.items(): process.append(NavigationComponent(robot_name, engine_port+1)) process.append(SysInfoComponent(robot_name, engine_port+2)) - process.append(SpeechRecogComponent(engine_port+3)) + process.append(SpeechRecogComponent(robot_name, engine_port+3)) print("Components Constructed.") time.sleep(5) -- GitLab From 1d652d1187f2b27658757b1d77272ad6d5d28b6a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 13:52:05 +0900 Subject: [PATCH 18/26] Add robot_name to subscribed topic name --- route_guidance_ros/scripts/Dummy_Speech_Recognition.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index e7e65491..2879112c 100755 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -119,7 +119,7 @@ class Speech_Recognition(Event, Command, Query): self._component.Languages = set("English") self._component.Grammer = "" #??? self._component.Rule = "" #??? - rospy.Subscriber("detected_text", String, self.sr_callback) + rospy.Subscriber(robot_name+"/detected_text", String, self.sr_callback) self.robot_name = robot_name def set_parameter(self, languages, grammer, rule): -- GitLab From f49fdf906a299daa8080db736709f92b12f20097 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 14:09:29 +0900 Subject: [PATCH 19/26] Remove unnecessary lines --- route_guidance_ros/scripts/Dummy_Speech_Recognition.py | 1 - .../scripts/Dummy_Speech_Recognition_client.py | 6 ------ route_guidance_ros/scripts/main_engine.py | 5 ----- route_guidance_ros/scripts/service_app_lv2.py | 5 ----- 4 files changed, 17 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 2879112c..05d019a3 100755 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -139,7 +139,6 @@ class Speech_Recognition(Event, Command, Query): def sr_callback(self, text): detected_text = text.data - # print("SR: detected_text => ", detected_text) self.speech_recognized(datetime.now(), [detected_text]) def speech_recognized(self, timestamp, recognized_text): diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py index a4e526d8..5801e977 100644 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py @@ -83,17 +83,12 @@ class Event(RoIS_Common.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)) @@ -120,5 +115,4 @@ class Speech_Recognition_Client(Command, Query, Event): """ msg = self._e_proxy.poll_event() (params, methodname) = xmlrpc.client.loads(msg) - # print("[Speech_Recognition_Client::poll_event]", params, methodname) self.engine_event_queue.put(msg) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 5ad81573..1f6530eb 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -243,8 +243,6 @@ 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) @@ -264,7 +262,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): params, methodname = xmlrpc.client.loads(msg) print(f"Received event: methodname => {methodname}, params => {params}") if methodname == 'speech_recognized': - print("Speech_Recognition EVENT!!!") timestamp, recognized_text, target_robot = params subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'] self.event_id_result_table[event_id] = params @@ -279,7 +276,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): return (status, results) def subscribe(self, event_type, condition): - print("MainEngine::subscribe called.") status = RoIS_HRI.ReturnCode_t.OK.value self.subscribe_id += 1 subscribe_id = str(self.subscribe_id) @@ -287,7 +283,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): return (status, subscribe_id) def get_event_detail(self, event_id, condition): - print("MainEngine::get_event_detail called.") status = RoIS_HRI.ReturnCode_t.OK.value results = self.event_id_result_table[event_id] return (status, results) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 0bde1efa..d652de29 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -36,7 +36,6 @@ class Service(Service_Application_IF): ] self.target_pos = "point_c" self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) - self.command_task_table = {} self.latest_event_id_table = {} def completed(self, command_id, completed_status): @@ -52,7 +51,6 @@ class Service(Service_Application_IF): error_id, RoIS_Service.ErrorType(error_type).name)) def notify_event(self, event_id, event_type, subscribe_id, expire): - print("Service::notify_event called.") if event_type == 'speech_recognized' and self.state == Task.ListenDest: self.latest_event_id_table['speech_recognized'] = event_id print(colored("speech recognized successfully.", 'green')) @@ -62,13 +60,11 @@ class Service(Service_Application_IF): dest = self.target_pos 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')) - def start_navi_to_guide(self, event): sr_event_id = self.latest_event_id_table['speech_recognized'] status, results = self._proxy.get_event_detail(sr_event_id, "") @@ -76,7 +72,6 @@ class Service(Service_Application_IF): 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 9dab69c6d41921de6222e4c24c01f8cc74b5fd33 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 14:40:32 +0900 Subject: [PATCH 20/26] Catch KeyError exception on VirtualNavigation::set_parameter --- route_guidance_ros/scripts/VirtualNavigation.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index b307260c..9b1ddc74 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -107,7 +107,7 @@ class VirtualNavigation(Event, Command, Query): self.latest_target_robot = None def position_to_coord(self, position): - if isinstance(position, str) and position in self.pointname_coord_table: + if isinstance(position, str): return self.pointname_coord_table[position] else: return position @@ -116,7 +116,10 @@ class VirtualNavigation(Event, Command, Query): # ERRORなロボットは初期位置に戻す self.back_stucked_robots_to_init_pos() - target_coord = self.position_to_coord(target_position) + try: + target_coord = self.position_to_coord(target_position) + except KeyError: + return RoIS_HRI.ReturnCode_t.BAD_PARAMETER.value if self.mode == Mode.NaviToUser: # READYなロボットの中でゴールに最も近い位置にいるものにNavigationさせる target_robot = self._component.get_nearest_robotname(target_coord, self.nav_state_table) -- GitLab From fb42c42b80c4e29ca172598e5434c31d32c6ece1 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 15:08:00 +0900 Subject: [PATCH 21/26] Add error-handling process on service_app --- 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 d652de29..d051b5d3 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -70,9 +70,15 @@ class Service(Service_Application_IF): status, results = self._proxy.get_event_detail(sr_event_id, "") dest = results[1][0] print("dest is ", dest) - print(colored("NaviToGuide started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") - stautus = RoIS_HRI.ReturnCode_t(return_code) + status = RoIS_HRI.ReturnCode_t(return_code) + if status == RoIS_HRI.ReturnCode_t.BAD_PARAMETER: + print(colored("Bad destination detected.", 'yellow')) + print(colored("Trying to ask again...", 'yellow')) + self.state = Task.ListenDest + self.listen_destination(self, None) + else: + print(colored("NaviToGuide started.", 'cyan')) def end_service(self, event): print(colored("Route Guidance Completed !", 'cyan')) -- GitLab From 011861ac8cde9295c704a94fc4113ff3763a29a0 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 15:12:47 +0900 Subject: [PATCH 22/26] Remove extra arguments of event-driven-function --- 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 d051b5d3..9ba746ab 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -35,7 +35,7 @@ class Service(Service_Application_IF): {'trigger': 'Succeeded', 'source': Task.NaviToGuide, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, ] self.target_pos = "point_c" - self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) + self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init) self.latest_event_id_table = {} def completed(self, command_id, completed_status): @@ -44,7 +44,7 @@ class Service(Service_Application_IF): if completed_status == Completed_Status.OK: self.Succeeded() elif completed_status == Completed_Status.ABORT: - self.Failed(self.target_pos) + self.Failed() def notify_error(self, error_id, error_type): print('received error event {}({}) '.format( \ @@ -56,16 +56,16 @@ class Service(Service_Application_IF): print(colored("speech recognized successfully.", 'green')) self.Succeeded() - def start_navi_to_user(self, event): + def start_navi_to_user(self): dest = self.target_pos print(colored("NaviToUser started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) - def listen_destination(self, event): + def listen_destination(self): print(colored("ListenDest started.", 'cyan')) - def start_navi_to_guide(self, event): + def start_navi_to_guide(self): sr_event_id = self.latest_event_id_table['speech_recognized'] status, results = self._proxy.get_event_detail(sr_event_id, "") dest = results[1][0] @@ -76,18 +76,18 @@ class Service(Service_Application_IF): print(colored("Bad destination detected.", 'yellow')) print(colored("Trying to ask again...", 'yellow')) self.state = Task.ListenDest - self.listen_destination(self, None) + self.listen_destination() else: print(colored("NaviToGuide started.", 'cyan')) - def end_service(self, event): + def end_service(self): print(colored("Route Guidance Completed !", 'cyan')) def run(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) return_code, subscribe_id = self._proxy.subscribe('speech_recognized', "") - self.Start(self.target_pos) + self.Start() if __name__ == '__main__': -- GitLab From a6b3f035b64cc5e57ee98e7813387e3133f8e3c9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 16:30:57 +0900 Subject: [PATCH 23/26] Modify to filter notified events by robot_name --- route_guidance_ros/scripts/main_engine.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 1f6530eb..28e9821d 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -190,8 +190,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.component_clients = { 'Navigation': VNavClient('http://localhost:8041', self.vnav_event_queue), 'Speech_Recognition1': SRecogClient('http://localhost:8013', self.component_event_queue), - # 'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue), - # 'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue), + 'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue), + 'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue), } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), @@ -263,7 +263,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): print(f"Received event: methodname => {methodname}, params => {params}") if methodname == 'speech_recognized': timestamp, recognized_text, target_robot = params - subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'] + try: + subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'+target_robot] + except KeyError: + print("This is un-subscribed event.") + continue self.event_id_result_table[event_id] = params self.notify_event(event_id, 'speech_recognized', subscribed_id, "") else: @@ -279,6 +283,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): status = RoIS_HRI.ReturnCode_t.OK.value self.subscribe_id += 1 subscribe_id = str(self.subscribe_id) + if event_type == 'speech_recognized': + condition += self.next_sr_robot self.event_ref_subscribe_id_table[event_type+condition] = subscribe_id return (status, subscribe_id) -- GitLab From d2bddff376c44ec24cfda157dd56ab49f919bcd9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 16:33:38 +0900 Subject: [PATCH 24/26] Subscribe speech_recognized event for each ListenDest --- route_guidance_ros/scripts/main_engine.py | 1 + route_guidance_ros/scripts/service_app_lv2.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 28e9821d..aeacbcbd 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -243,6 +243,7 @@ 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")) + self.next_sr_robot = robot_name self.completed(command_id, Completed_Status.OK.value) del self.goal_command_table[str(target_pos)] time.sleep(3) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 9ba746ab..553923bb 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -63,6 +63,7 @@ class Service(Service_Application_IF): stautus = RoIS_HRI.ReturnCode_t(return_code) def listen_destination(self): + return_code, subscribe_id = self._proxy.subscribe('speech_recognized', "") print(colored("ListenDest started.", 'cyan')) def start_navi_to_guide(self): @@ -86,7 +87,6 @@ class Service(Service_Application_IF): def run(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) - return_code, subscribe_id = self._proxy.subscribe('speech_recognized', "") self.Start() -- GitLab From 3a15be44f1ecce56fabc818fc48d579643080752 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 16:54:45 +0900 Subject: [PATCH 25/26] Implement MainEngine::unsubscribe function --- route_guidance_ros/scripts/main_engine.py | 13 ++++++++++--- route_guidance_ros/scripts/service_app_lv2.py | 1 + 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index aeacbcbd..09b639c0 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -204,7 +204,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.command_id_result_table = {} self.event_ref_subscribe_id_table = {} self.event_id_result_table = {} - self.next_sr_robot = "robot1" + self.sr_robot = "robot1" th = threading.Thread(target=self.analyze_vnav_status, daemon=True) th.start() @@ -243,7 +243,7 @@ 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")) - self.next_sr_robot = robot_name + self.sr_robot = robot_name self.completed(command_id, Completed_Status.OK.value) del self.goal_command_table[str(target_pos)] time.sleep(3) @@ -285,10 +285,17 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.subscribe_id += 1 subscribe_id = str(self.subscribe_id) if event_type == 'speech_recognized': - condition += self.next_sr_robot + condition += self.sr_robot self.event_ref_subscribe_id_table[event_type+condition] = subscribe_id return (status, subscribe_id) + def unsubscribe(self, subscribe_id): + status = RoIS_HRI.ReturnCode_t.OK.value + for key, value in self.event_ref_subscribe_id_table.items(): + if value == subscribe_id: event_ref = key + del self.event_ref_subscribe_id_table[event_ref] + return status + def get_event_detail(self, event_id, condition): status = RoIS_HRI.ReturnCode_t.OK.value results = self.event_id_result_table[event_id] diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 553923bb..05ed24cb 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -54,6 +54,7 @@ class Service(Service_Application_IF): if event_type == 'speech_recognized' and self.state == Task.ListenDest: self.latest_event_id_table['speech_recognized'] = event_id print(colored("speech recognized successfully.", 'green')) + return_code = self._proxy.unsubscribe(subscribe_id) self.Succeeded() def start_navi_to_user(self): -- GitLab From d7c6707d8758ba0b580bdfa7e977f31a3805d42b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 17:12:43 +0900 Subject: [PATCH 26/26] Remove unused file (Virtual_Speech_Recognition series) --- .../scripts/Virtual_Speech_Recognition.py | 176 ------------------ .../Virtual_Speech_Recognition_client.py | 125 ------------- 2 files changed, 301 deletions(-) delete mode 100755 route_guidance_ros/scripts/Virtual_Speech_Recognition.py delete mode 100644 route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition.py deleted file mode 100755 index 245c21a4..00000000 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition.py +++ /dev/null @@ -1,176 +0,0 @@ -# (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, robot_names): - super().__init__(c) - self._robot_names = robot_names - self._target_robot = "robot1" - 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 - print("target robot is set as ", self._target_robot) - - 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 = input("SR >>> ") - delay_duration = random.randint(0, 5) - time.sleep(delay_duration) - self.speech_recognized(datetime.now(), [detected_text]) - - def speech_recognized(self, timestamp, recognized_text): - """speech_recognition - """ - msg = (timestamp, recognized_text, self._target_robot), 'speech_recognized' - msg = xmlrpc.client.dumps(*msg) - self.event_queue.put(msg) - - -class component: - """component - """ - def __init__(self): - self._state = False - - -def example_sr(port, robot_name): - """example_sr - """ - c = component() - sr = Speech_Recognition(c, robot_name) - - # 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) < 3: - print("python3 ...") - raise RuntimeError - - port, robot_names = int(sys.argv[1]), sys.argv[2:] - example_sr(port, robot_names) diff --git a/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py b/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py deleted file mode 100644 index 4fd66089..00000000 --- a/route_guidance_ros/scripts/Virtual_Speech_Recognition_client.py +++ /dev/null @@ -1,125 +0,0 @@ -# (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() - self.engine_event_queue.put(msg) - - def set_target_robot(self, robot_name): - self._e_proxy.set_target_robot(robot_name) -- GitLab