diff --git a/route_guidance_ros/requirements.txt b/route_guidance_ros/requirements.txt index ff3e89cf478ee04a82670afa751248b47ca6945d..5495fb3ee4409c30ef9d04b686c1757f8ddf1bfb 100644 --- a/route_guidance_ros/requirements.txt +++ b/route_guidance_ros/requirements.txt @@ -1,3 +1,4 @@ pyrois numpy termcolor +transitions diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 0a19ecaded640f160c6447f276a5229ef7818dfa..2cd3a62f4e557f51214421a541c2c6dfd3523186 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# -*- coding: utf-8 -*- # VirtualNavigation.py # @@ -86,6 +87,11 @@ class VirtualNavigation(Event, Command, Query): self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" + self.init_pos_table = { + 'robot1': [0.0, 0.0, 0.0], + 'robot2': [10.0, 0.0, 0.0], + 'robot3': [0.0, 10.0, 0.0] + } self.robot_goal_table = {} self.nav_state_table = \ {robot_name : Component_Status.READY for robot_name in robot_names} @@ -99,6 +105,10 @@ class VirtualNavigation(Event, Command, Query): return position def set_parameter(self, target_position, time_limit, routing_policy): + # ERRORなロボットは初期位置に戻す + self.back_stucked_robots_to_init_pos() + + # READYなロボットの中でゴールに最も近い位置にいるものにNavigationさせる target_coord = self.position_to_coord(target_position) nearest_robot = self._component.get_nearest_robotname(target_coord, self.nav_state_table) th = threading.Thread( @@ -119,6 +129,21 @@ class VirtualNavigation(Event, Command, Query): msg = xmlrpclib.dumps((goal, robot_name, status.value), 'completed') self.event_queue.put(msg) + def back_stucked_robots_to_init_pos(self): + def reset_robot_pos_and_state(robot_name): + init_pos = self.init_pos_table[robot_name] + init_pos = self.position_to_coord(init_pos) + self._component.send_goal_to_robot_async(init_pos, robot_name) + self.nav_state_table[robot_name] = Component_Status.READY + + for robot_name, state in self.nav_state_table.items(): + if state == Component_Status.ERROR: + th = threading.Thread( + target=reset_robot_pos_and_state, + args=(robot_name, )) + th.daemon = True + th.start() + def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 283f41b90826a8e3a55bde40ba57f61cde4e5abd..a80f77ed020815bfe7f2fb6db66a82ad7d26178a 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -191,11 +191,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 'robot2': EngineClient('http://localhost:8020'), 'robot3': EngineClient('http://localhost:8030') } - self.init_pos_table = { - 'robot1': (0.0, 0.0 ), - 'robot2': (10.0, 0.0 ), - 'robot3': (0.0, 10.0) - } self.goal_command_table = {} for engine_client in self.engine_clients.values(): engine_client.connect() diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 56abfeff78d201943344d24a701e353539f8a68f..f5b54273711a088cddf7e37f0d967288792d89cc 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -10,50 +10,38 @@ import threading import time import enum import xmlrpc +from termcolor import colored +from transitions import Machine -class TaskKind(enum.Enum): - NaviToUser = 1 - NaviToGuide = 2 - End = 3 - - -class TaskState(object): - __task_seq_table = { - TaskKind.NaviToUser: TaskKind.NaviToGuide, - TaskKind.NaviToGuide: TaskKind.End, - TaskKind.End: TaskKind.End - } - - def __init__(self, task_kind, status=None): - self.task_kind = task_kind - self.status = status - - def __repr__(self): - return "".format(self.task_kind, self.status) - - def next(self): - return TaskState.__task_seq_table[self.task_kind] +class Task(enum.Enum): + Init = 'Init' + NaviToUser = 'NaviToUser' + NaviToGuide = 'NaviToGuide' + End = 'End' class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) + # buid state machine + states = [Task.Init, Task.NaviToUser, Task.NaviToGuide, Task.End] + transitions = [ + {'trigger': 'StartService', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSucceeded', 'source': Task.NaviToUser, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'NaviFailed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviSucceeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, + ] self.target_pos = "point_c" self.dest_pos = "point_a" - self.commandid_taskstate_table = {} + self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) - def completed(self, command_id, status): - task = self.commandid_taskstate_table[command_id] - status = Completed_Status(status) - task.status = status - next_task = task.next() - if next_task == TaskKind.NaviToGuide: - self.start_navi_to_guide(self.dest_pos) - elif next_task == TaskKind.End: - print("Route Guide Finished !!") - self.commandid_taskstate_table[command_id] = task - print(self.commandid_taskstate_table) + def completed(self, command_id, completed_status): + completed_status = Completed_Status(completed_status) + if completed_status == Completed_Status.OK: + self.NaviSucceeded(self.dest_pos) + elif completed_status == Completed_Status.ABORT: + self.NaviFailed(self.target_pos) def notify_error(self, error_id, error_type): print('received error event {}({}) '.format( \ @@ -63,21 +51,25 @@ class Service(Service_Application_IF): print('received event {} {} {} {}'.format( \ event_id, event_type, subscribe_id, expire)) - def start_navi_to_user(self, dest): + def start_navi_to_user(self, event): + dest = event.args[0] + print(colored("NaviToUser started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) - self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToUser) - def start_navi_to_guide(self, dest): + def start_navi_to_guide(self, event): + dest = event.args[0] + print(colored("NaviToGuide started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) - self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToGuide) + + def end_service(self, event): + print(colored("Route Guidance Completed !", 'cyan')) def run(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) - target_point = "point_c" - self.start_navi_to_user(target_point) + self.StartService(self.target_pos) if __name__ == '__main__':