From 9dab69c6d41921de6222e4c24c01f8cc74b5fd33 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 29 Jan 2020 14:40:32 +0900 Subject: [PATCH 1/3] 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 2/3] 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 3/3] 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