diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index 453125156563fd9c6a29ce8acf0e27939b85e30f..9e48f5af5e5aa49321cd7dd3f6d873f187e7dae1 100755 --- a/route_guidance_ros/scripts/Navigation.py +++ b/route_guidance_ros/scripts/Navigation.py @@ -89,8 +89,11 @@ class Navigation(Event, Command, Query): def set_parameter(self, target_position, time_limit, routing_policy): # Put event self.latest_nav_state = Component_Status.BUSY - if isinstance(target_position, str) and target_position in self.pointname_coord_table: - target_position = self.pointname_coord_table[target_position] + if isinstance(target_position, str): + try: + target_position = self.pointname_coord_table[target_position] + except KeyError: + return RoIS_HRI.ReturnCode_t.BAD_PARAMETER.value self._goal_sender.send_goal(target_position) status = RoIS_HRI.ReturnCode_t.OK.value return status diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index 56abfeff78d201943344d24a701e353539f8a68f..72655966aa74c6f714d13a122e1a657dacc62e9d 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -1,6 +1,5 @@ from pyrois.Service_Application_IF import Service_Application_IF from pyrois import RoIS_Service, RoIS_HRI -from utilities import setup_multi_robot from pyrois.RoIS_Common import Component_Status from pyrois.RoIS_Service import Completed_Status @@ -81,8 +80,6 @@ class Service(Service_Application_IF): if __name__ == '__main__': - process = setup_multi_robot() - time.sleep(5) service = Service() print("Starting service..") service.run() diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 344d0967655ec5dad9804293cffaaefada1d839d..e848bb11dd9bdcaa45b75629dbd32b11e63e7344 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -1,6 +1,5 @@ from pyrois.Service_Application_IF import Service_Application_IF from pyrois import RoIS_Service, RoIS_HRI -from utilities import setup_multi_robot from pyrois.RoIS_Common import Component_Status from pyrois.RoIS_Service import Completed_Status @@ -29,39 +28,40 @@ class Service(Service_Application_IF): # buid state machine states = [Task.Init, Task.GetUserPos, Task.NaviToUser, Task.ListenDest, Task.NaviToGuide, Task.End] transitions = [ - {'trigger': 'Start', 'source': Task.Init, 'dest': Task.GetUserPos, 'after': 'get_user_position' }, - {'trigger': 'Succeeded', 'source': Task.GetUserPos, '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.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'Start', 'source': Task.Init, 'dest': Task.GetUserPos, 'after': 'get_user_position' }, + {'trigger': 'PLSuccess', 'source': Task.GetUserPos, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSuccess', 'source': Task.NaviToUser, 'dest': Task.ListenDest, 'after': 'listen_destination' }, + {'trigger': 'SRSuccess', 'source': Task.ListenDest, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'NaviFail', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSuccess', 'source': Task.NaviToGuide, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, ] - self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init) + self.machine = Machine(self, + states=states, + transitions=transitions, + initial=Task.Init, + ignore_invalid_triggers=True) self.latest_event_id_table = {} 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.Succeeded() + self.NaviSuccess() elif completed_status == Completed_Status.ABORT: - self.Failed() + self.NaviFail() 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): - if event_type == 'speech_recognized' and self.state == Task.ListenDest: + if event_type == 'speech_recognized': self.latest_event_id_table['speech_recognized'] = event_id - print(colored("speech recognized successfully.", 'green')) return_code = self._proxy.unsubscribe(subscribe_id) - self.Succeeded() - if event_type == 'person_localized' and self.state == Task.GetUserPos: + self.SRSuccess() + if event_type == 'person_localized': self.latest_event_id_table['person_localized'] = event_id - print(colored("person localized successfully.", 'green')) - return_code = self._proxy.unsubscribe(subscribe_id) - self.Succeeded() + self.PLSuccess() def get_user_position(self): return_code, subscribe_id = self._proxy.subscribe('person_localized', "") @@ -70,6 +70,7 @@ class Service(Service_Application_IF): def start_navi_to_user(self): pl_event_id = self.latest_event_id_table['person_localized'] status, results = self._proxy.get_event_detail(pl_event_id, "") + print(colored("person localized successfully.", 'green')) dest = results[2] print("dest is ", dest) print(colored("NaviToUser started.", 'cyan')) @@ -83,6 +84,7 @@ class Service(Service_Application_IF): 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, "") + print(colored("speech recognized successfully.", 'green')) dest = results[1][0] print("dest is ", dest) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") @@ -105,9 +107,6 @@ class Service(Service_Application_IF): if __name__ == '__main__': - process = setup_multi_robot() - time.sleep(5) service = Service() print("Starting service..") service.run() - print("Finish.") diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 28ddb0094fe6fe6aea5cf7e87268b5f06464f9e8..0b7f9357dfcb27d957eccbe164da6b932c0d85ba 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -170,6 +170,7 @@ from pyrois.Service_Application_Base_example import Service_Application_Base from Navigation_client import Navigation_Client as NavClient from System_Information_client import System_Information_Client as SysClient from Dummy_Speech_Recognition_client import Speech_Recognition_Client as SRecogClient +from Person_Localization_client import Person_Localization_Client as PLClient from pyrois.RoIS_Common import Component_Status import queue import time @@ -180,13 +181,22 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def __init__(self, Engine, engine_port): super().__init__(Engine) self.command_id = 0 + self.subscribe_id = 0 + self.event_id = 0 self.event_queue = queue.Queue() + self.component_event_queue = queue.Queue() self.compoent_clients = { - 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), - 'System_Information': SysClient('http://localhost:' + str(engine_port+2)), - 'Speech_Recognition': SRecogClient('http://localhost:' + str(engine_port+3), self.event_queue) + 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), + 'System_Information': SysClient('http://localhost:' + str(engine_port+2)), + 'Speech_Recognition': SRecogClient('http://localhost:' + str(engine_port+3), self.component_event_queue), + 'Person_Localization': PLClient('http://localhost:8043', self.component_event_queue) } self.command_id_status_table = {} + self.event_ref_subscribe_id_table = {} + self.event_id_result_table = {} + + th = threading.Thread(target=self.monitor_event, daemon=True) + th.start() def set_parameter(self, component_ref, *parameters): status = None @@ -207,13 +217,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): result = target_component_client.robot_position() result = (result[0].value, result[1], result[2], result[3]) # FIXME self.command_id_status_table[str(self.command_id)] = result - elif component_ref == 'Speech_Recognition': - status = target_component_client.set_parameter(*parameters).value - th = threading.Thread( - target=self.analyze_c_status, - daemon=True, - args=(target_component_client, str(self.command_id))) - th.start() return (status, str(self.command_id)) def get_command_result(self, command_id, condition): @@ -221,26 +224,62 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): results = self.command_id_status_table[command_id] return (status, results) - ### Remove ### - def get_navi_status(self): - status, c_status = self.compoent_clients['Navigation'].component_status() - return (status.value, c_status.value) - - def get_position(self): - si_compoent = self.compoent_clients['System_Information'] - status, timestamp, robot_ref, position_data = si_compoent.robot_position() - return (status.value, timestamp, robot_ref, position_data) - ############## - def analyze_c_status(self, component, command_id): while True: time.sleep(5) (_, c_status) = component.component_status() - print("command_id: {}, status: {}".format(command_id, c_status)) if c_status == Component_Status.READY: self.completed(command_id, c_status.value) return + 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': + timestamp, recognized_text, target_robot = params + try: + subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'] + except KeyError: + print("This is un-subscribed event. (speech_recognized)") + continue + self.event_id_result_table[event_id] = params + self.notify_event(event_id, 'speech_recognized', subscribed_id, "") + elif methodname == 'person_localized': + timestamp, person_ref, position_data = params + try: + subscribed_id = self.event_ref_subscribe_id_table['person_localized'] + except KeyError: + print("This is un-subscribed event. (person_localized)") + continue + self.event_id_result_table[event_id] = params + self.notify_event(event_id, 'person_localized', subscribed_id, "") + else: + raise RuntimeError("[main_engine]Unknown method-name of event.") + time.sleep(3) + + def subscribe(self, event_type, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + self.subscribe_id += 1 + subscribe_id = str(self.subscribe_id) + 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/sub_engine_client.py b/route_guidance_ros/scripts/sub_engine_client.py index 04f0b12a9c5193187d8c109f52a9d7d80b1f0595..de8344942679eeceef85b97358f15505660fb644 100644 --- a/route_guidance_ros/scripts/sub_engine_client.py +++ b/route_guidance_ros/scripts/sub_engine_client.py @@ -11,11 +11,9 @@ """ import time import xmlrpc.client -# from pyrois import Service_Application_Base_example from pyrois import RoIS_HRI, RoIS_Common, RoIS_Service from pyrois.Service_Application_IF import Service_Application_IF -# import Service_Application_IF_test class SystemIF(RoIS_HRI.SystemIF): """SystemIF @@ -134,14 +132,3 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF): s = self._proxy.analysis_c_status() status = RoIS_HRI.ReturnCode_t(s) return status - - ### Remove ### - def get_navi_status(self): - status, c_status = self._proxy.get_navi_status() - return RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status) - - def get_position(self): - position = self._proxy.get_position() - position[0] = RoIS_HRI.ReturnCode_t(position[0]) - return position - ############## diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index cbfd97b0e1922137fb4ba398d5bfdc3e5a7e0b9d..0bb5210d128f6dcbbc65ef30d8c075aa74f8e666 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -56,7 +56,6 @@ def launch_components_and_subengines(**robot_port_table): process.append(SysInfoComponent(robot_name, engine_port+2)) process.append(SpeechRecogComponent(robot_name, engine_port+3)) print("Components Constructed.") - time.sleep(5) command = [ "rosrun", "route_guidance_ros", "Person_Localization.py", @@ -64,6 +63,7 @@ def launch_components_and_subengines(**robot_port_table): ] process.append(SubprocessWrapper(command)) print("Person Localization Component Constructed.") + time.sleep(10) for engine_port in robot_port_table.values(): process.append(SubEngineWrapper(engine_port)) @@ -74,7 +74,7 @@ def launch_components_and_subengines(**robot_port_table): def setup_single_robot(): - return launch_components_and_subengines(robot1=8010) + return launch_components_and_subengines(robot1=8000) def setup_multi_robot():