From 2a3f5e6acff130c0b5d0a8fb6bb8f39f5bae50ae Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 27 Jan 2020 13:25:05 +0900 Subject: [PATCH 1/3] Upgrade NaviToGuide of VirtualNavigation --- .../scripts/VirtualNavigation.py | 23 +++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 1b9cb0c2..b307260c 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -77,9 +77,15 @@ class Event(RoIS_Common.Event): from goal_sender_ros import GoalSenderROS from pyrois.RoIS_Common import Component_Status import yaml - +import enum import xmlrpclib + +class Mode(enum.Enum): + NaviToUser = 'NaviToUser' + NaviToGuide = 'NaviToGuide' + + class VirtualNavigation(Event, Command, Query): def __init__(self, c, robot_names): super(VirtualNavigation, self).__init__(c) @@ -97,6 +103,8 @@ class VirtualNavigation(Event, Command, Query): {robot_name : Component_Status.READY for robot_name in robot_names} # {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} self.pointname_coord_table = yaml.load(open("field_points.yaml")) + self.mode = Mode.NaviToUser + self.latest_target_robot = None def position_to_coord(self, position): if isinstance(position, str) and position in self.pointname_coord_table: @@ -108,12 +116,17 @@ class VirtualNavigation(Event, Command, Query): # 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) + if self.mode == Mode.NaviToUser: + # READYなロボットの中でゴールに最も近い位置にいるものにNavigationさせる + target_robot = self._component.get_nearest_robotname(target_coord, self.nav_state_table) + else: # self.mode == Mode.NaviToGuide: + # ユーザのもとに辿り着いたロボットに Navigation させる + target_robot = self.latest_target_robot + th = threading.Thread( target=self.send_goal_and_event, - args=(target_position, nearest_robot)) + args=(target_position, target_robot)) th.daemon = True th.start() status = RoIS_HRI.ReturnCode_t.OK.value @@ -126,6 +139,8 @@ class VirtualNavigation(Event, Command, Query): status = self._component.send_goal_to_robot_async(goal_coord, robot_name) self.nav_state_table[robot_name] = status print(self.nav_state_table) + self.mode = Mode.NaviToGuide if self.mode == Mode.NaviToUser else Mode.NaviToUser + self.latest_target_robot = robot_name msg = xmlrpclib.dumps((goal, robot_name, status.value), 'nav_finished') self.event_queue.put(msg) -- GitLab From 323281244c933aaf926a31a3ca544945425ce683 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 27 Jan 2020 13:27:10 +0900 Subject: [PATCH 2/3] Make service endless (Task.End => Task.NaviToUser) --- 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 8cf8f76c..327dd80a 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -32,7 +32,7 @@ class Service(Service_Application_IF): {'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.End, 'after': 'end_service' }, + {'trigger': 'Succeeded', 'source': Task.NaviToGuide, 'dest': Task.NaviToUser, 'after': 'end_service' }, ] self.target_pos = "point_c" self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) -- GitLab From 84aa7b215552e561ff405b3ad72b03289e396023 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 27 Jan 2020 13:33:34 +0900 Subject: [PATCH 3/3] Change triggered function on end of service --- 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 327dd80a..ce039c5a 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -32,7 +32,7 @@ class Service(Service_Application_IF): {'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': 'end_service' }, + {'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) @@ -55,7 +55,7 @@ class Service(Service_Application_IF): event_id, event_type, subscribe_id, expire)) def start_navi_to_user(self, event): - dest = event.args[0] + dest = self.target_pos print(colored("NaviToUser started.", 'cyan')) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") self.command_task_table[command_id] = Task.NaviToUser -- GitLab