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/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index d652de29cf8070741e76c2be2ceea98c0a7f5f6c..9ba746aba24e3d5dc10e91552c9a33da6e63f55d 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,32 +56,38 @@ 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] 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() + 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__':