From 8b0c13b7d6ca610bc2f44d04a8fc762a351ba8d6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 16:11:59 +0900 Subject: [PATCH 01/12] Add ignore_invalid_triggers option to state-machine --- route_guidance_ros/scripts/service_app_lv2.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 344d0967..8ab38f8e 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -29,14 +29,18 @@ 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': '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': 'Succeeded', '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): -- GitLab From fc46d3a6b50cdffb557f4d81190072816988ce04 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 17:00:13 +0900 Subject: [PATCH 02/12] Add trigger for speech_recognized and person_localized --- route_guidance_ros/scripts/service_app_lv2.py | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 8ab38f8e..440b18a2 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -29,12 +29,12 @@ 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': 'PLSucess', '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, @@ -47,25 +47,24 @@ class Service(Service_Application_IF): print("command_id: ", command_id, " completed.") completed_status = Completed_Status(completed_status) if completed_status == Completed_Status.OK: - self.Succeeded() + self.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', "") @@ -114,4 +113,3 @@ if __name__ == '__main__': service = Service() print("Starting service..") service.run() - print("Finish.") -- GitLab From d178fcae1a86982769d7931d9310ae945fe5415a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 17:26:32 +0900 Subject: [PATCH 03/12] Remove unnecessary functions on sub_engine --- route_guidance_ros/scripts/sub_engine.py | 11 ----------- route_guidance_ros/scripts/sub_engine_client.py | 13 ------------- 2 files changed, 24 deletions(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 28ddb009..6e5a0657 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -221,17 +221,6 @@ 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) diff --git a/route_guidance_ros/scripts/sub_engine_client.py b/route_guidance_ros/scripts/sub_engine_client.py index 04f0b12a..de834494 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 - ############## -- GitLab From 9b7441473f181992c8b5ccfd8df4ccea8e63cf89 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 17:27:17 +0900 Subject: [PATCH 04/12] Remove robot system launcher on service_app --- route_guidance_ros/scripts/service_app_lv1.py | 3 --- route_guidance_ros/scripts/service_app_lv2.py | 3 --- 2 files changed, 6 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index 56abfeff..72655966 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 440b18a2..2255293e 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 @@ -108,8 +107,6 @@ class Service(Service_Application_IF): if __name__ == '__main__': - process = setup_multi_robot() - time.sleep(5) service = Service() print("Starting service..") service.run() -- GitLab From 9a059d469cbb3d87a90eba931fe8a3e4428eaa0c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 17:38:31 +0900 Subject: [PATCH 05/12] Modify engine server port on setup_single_robot --- route_guidance_ros/scripts/utilities.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index cbfd97b0..b9096b87 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -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(): -- GitLab From 5e7be41ed85317efdf582ef2c28b6f95cf7f46ab Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 19:37:21 +0900 Subject: [PATCH 06/12] Add event-subscribe functions on sub_engine --- route_guidance_ros/scripts/sub_engine.py | 70 ++++++++++++++++++++---- 1 file changed, 60 insertions(+), 10 deletions(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 6e5a0657..d711548a 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,21 @@ 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.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.event_queue), + 'Person_Localization': PLClient('http://localhost:8043', self.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 +216,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): @@ -230,6 +232,54 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.completed(command_id, c_status.value) return + def monitor_event(self): + while True: + msg = self.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'+target_robot] + 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 -- GitLab From 870ccc538a745451ab4673e3751859d29aed2420 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 20:13:38 +0900 Subject: [PATCH 07/12] Wait a minutes after PL launched --- route_guidance_ros/scripts/utilities.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index b9096b87..0bb5210d 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)) -- GitLab From 69b4b5ab961bb0f034474bc7b2660a26a472a719 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 30 Jan 2020 20:32:43 +0900 Subject: [PATCH 08/12] Fix typo on service_app_lv2 --- route_guidance_ros/scripts/service_app_lv2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 2255293e..9d8b9744 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -29,7 +29,7 @@ class Service(Service_Application_IF): 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': 'PLSucess', 'source': Task.GetUserPos, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'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' }, -- GitLab From 382ca8785e0c1f0f348e593c6c6f4629b4c16a4d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 31 Jan 2020 15:39:39 +0900 Subject: [PATCH 09/12] Change timing of print message of events --- route_guidance_ros/scripts/service_app_lv2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 9d8b9744..e848bb11 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -57,12 +57,10 @@ class Service(Service_Application_IF): def notify_event(self, event_id, event_type, subscribe_id, expire): 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.SRSuccess() if event_type == 'person_localized': self.latest_event_id_table['person_localized'] = event_id - print(colored("person localized successfully.", 'green')) self.PLSuccess() def get_user_position(self): @@ -72,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')) @@ -85,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, "", "") -- GitLab From 95f7ece5dbe6324d6999074ed447b4ba1443487f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 31 Jan 2020 15:40:21 +0900 Subject: [PATCH 10/12] Fix bug that speech_recognized is never notified --- route_guidance_ros/scripts/sub_engine.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index d711548a..b4c474b5 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -184,11 +184,12 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 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), - 'Person_Localization': PLClient('http://localhost:8043', self.event_queue) + '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 = {} @@ -234,7 +235,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def monitor_event(self): while True: - msg = self.event_queue.get() + msg = self.component_event_queue.get() self.event_id += 1 event_id = str(self.event_id) params, methodname = xmlrpc.client.loads(msg) @@ -242,7 +243,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): if methodname == 'speech_recognized': timestamp, recognized_text, target_robot = params try: - subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'+target_robot] + subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'] except KeyError: print("This is un-subscribed event. (speech_recognized)") continue -- GitLab From b2cb0eb5cb20a6e7948f8bf3cf08973f39acc6a3 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 31 Jan 2020 16:05:51 +0900 Subject: [PATCH 11/12] Add invliad destination handler on sub_engine --- route_guidance_ros/scripts/Navigation.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index 45312515..9e48f5af 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 -- GitLab From c07506c6476a9fabc82b7a0b34ada1933d1051ff Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 31 Jan 2020 16:06:55 +0900 Subject: [PATCH 12/12] Remove unnecessary print code --- route_guidance_ros/scripts/sub_engine.py | 1 - 1 file changed, 1 deletion(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index b4c474b5..0b7f9357 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -228,7 +228,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 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 -- GitLab