diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 8c964403f95ab00a1c9f73094dd708d03540fe16..05d019a3c3977311fb72b4a25ad8bdd86c81daa4 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,40 @@ 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) + def __init__(self, c, robot_name): + 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(robot_name+"/detected_text", String, self.sr_callback) + self.robot_name = robot_name 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 +137,15 @@ 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 + 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 = (timestamp, recognized_text, self.robot_name), 'speech_recognized' + msg = xmlrpc_client.dumps(*msg) self.event_queue.put(msg) @@ -145,11 +156,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) @@ -161,8 +172,9 @@ def example_sr(port): if __name__ == '__main__': if len(sys.argv) < 2: - print("python3 ") + print("rosrun ") raise RuntimeError - port = int(sys.argv[1]) - example_sr(port) + port, robot_name = int(sys.argv[1]), sys.argv[2] + rospy.init_node('speech_recognition_component', anonymous=True) + example_sr(port, robot_name) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py index d1280d8821dc876339ca549508308e93a6ea9422..5801e977f6fda0939072046b348e3021cb46aef4 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,6 +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) - msg = xmlrpc.client.dumps((methodname, params), 'completed') self.engine_event_queue.put(msg) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index b307260ca68f71deee1ff89ae1bd1213398f7bb0..9b1ddc7437df247e3678742d248ed11fc16ca8c5 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) 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 245c21a482912af66f88afcf9d6253749b74ca44..0000000000000000000000000000000000000000 --- 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 4fd660898cc0edc3cee018e925346df0eac04115..0000000000000000000000000000000000000000 --- 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) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index a14ae7dbac25426094ad6af840e9d93e43e176f2..09b639c084705a1547fc0213e0bc790f088b5dff 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 @@ -182,11 +182,16 @@ 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.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), - 'Speech_Recognition': SRecogClient('http://localhost:8042', 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), } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), @@ -197,10 +202,13 @@ 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 = {} + self.event_ref_subscribe_id_table = {} + self.event_id_result_table = {} + self.sr_robot = "robot1" - 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): @@ -219,41 +227,50 @@ 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] + # status = target_component.set_parameter(*parameters).value 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")) + self.sr_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() + 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 == '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': - print("Speech_Recognition EVENT!!!") + if methodname == 'speech_recognized': 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) + 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: raise RuntimeError("[main_engine]Unknown method-name of event.") time.sleep(3) @@ -263,6 +280,27 @@ 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): + status = RoIS_HRI.ReturnCode_t.OK.value + self.subscribe_id += 1 + subscribe_id = str(self.subscribe_id) + if event_type == 'speech_recognized': + 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] + 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 ce039c5a57a1612ed1cccd545de2bd2fd1fb2546..05ed24cb9b8f8b4d7835a6438593a34f759dfe79 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -35,8 +35,8 @@ 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.command_task_table = {} + self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init) + self.latest_event_id_table = {} def completed(self, command_id, completed_status): print("command_id: ", command_id, " completed.") @@ -44,45 +44,51 @@ 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( \ 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)) + 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, 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, "", "") - self.command_task_table[command_id] = Task.NaviToUser stautus = RoIS_HRI.ReturnCode_t(return_code) - def listen_destination(self, event): + def listen_destination(self): + return_code, subscribe_id = self._proxy.subscribe('speech_recognized', "") 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] - status, results = self._proxy.get_command_result(latest_listen_command_id, "") - dest = results + 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] 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) + 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() + 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) - self.Start(self.target_pos) + self.Start() if __name__ == '__main__': diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 789eeeca0e6622325c3705c1e9e5cf6e2a093380..8554eedbdb69bf981af290065e0e7a78469d8cc9 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__(["python3", "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) @@ -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.")