From e8c0446547063904faa54a61d9af0c921bdf4373 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 8 Dec 2019 16:47:47 +0900 Subject: [PATCH 01/36] Add level2 service_app script --- route_guidance_ros/scripts/service_app_lv2.py | 88 +++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 route_guidance_ros/scripts/service_app_lv2.py diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py new file mode 100644 index 00000000..5a220f81 --- /dev/null +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -0,0 +1,88 @@ +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 + +import logging +import queue +import threading +import time +import enum +import xmlrpc + + +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=Component_Status.BUSY): + 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 Service(Service_Application_IF): + def __init__(self, uri="http://localhost:8000", logger=None): + super().__init__(uri) + self.target_pos = "point_c" + self.dest_pos = "point_a" + self.commandid_taskstate_table = {} + + def completed(self, command_id, status): + task = self.commandid_taskstate_table[command_id] + status = Component_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 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): + print('received event {} {} {} {}'.format( \ + event_id, event_type, subscribe_id, expire)) + + def start_navi_to_user(self, dest): + (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): + (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 run(self): + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + target_point = "point_c" + self.start_navi_to_user(target_point) + + +if __name__ == '__main__': + process = setup_multi_robot() + time.sleep(5) + service = Service() + print("Starting service..") + service.run() + print("Finish.") -- GitLab From 83e992ec17037e3a3bccdfc6a804f1afd801733b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 8 Dec 2019 17:20:37 +0900 Subject: [PATCH 02/36] Upgrade send_goal_and_wait to return Component_status --- route_guidance_ros/scripts/goal_sender_ros.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/goal_sender_ros.py b/route_guidance_ros/scripts/goal_sender_ros.py index 13bec919..df0592a3 100644 --- a/route_guidance_ros/scripts/goal_sender_ros.py +++ b/route_guidance_ros/scripts/goal_sender_ros.py @@ -50,11 +50,12 @@ class GoalSenderROS(object): def send_goal_and_wait(self, pose_xy): pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)] goal = pose_to_goal(pose) - return self.action_client.send_goal_and_wait(goal) + state = self.action_client.send_goal_and_wait(goal) + return GoalSenderROS.GoalSenderState[state] def get_goalsender_state(self): - state = self.action_client.get_state() - return GoalSenderROS.GoalSenderState[state] + state = self.action_client.get_state() + return GoalSenderROS.GoalSenderState[state] # If you remove this comment out, it does not work. # rospy.init_node('goal_sender') -- GitLab From 4f06d6c7c8e454b731a83942bfac142758e72e82 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 8 Dec 2019 17:23:43 +0900 Subject: [PATCH 03/36] Print robot status for debug --- route_guidance_ros/scripts/VirtualNavigation.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 3d7d651b..7246af21 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -110,7 +110,11 @@ class VirtualNavigation(Event, Command, Query): def send_goal_and_event(self, goal, robot_name): goal_coord = self.position_to_coord(goal) + self.nav_state_table[robot_name] = Component_Status.BUSY + print(self.nav_state_table) status = self._component.send_goal_to_robot_async(goal_coord, robot_name) + self.nav_state_table[robot_name] = status + print(self.nav_state_table) msg = xmlrpclib.dumps((goal, robot_name), 'completed') self.event_queue.put(msg) -- GitLab From 31aa15e4fed5031a5101925a34d439629c27d4c8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 02:09:29 +0900 Subject: [PATCH 04/36] Modify event handler with event-type --- route_guidance_ros/scripts/VirtualNavigation.py | 6 +++++- route_guidance_ros/scripts/main_engine.py | 14 ++++++++++---- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 7246af21..aa79edfe 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -115,7 +115,11 @@ 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) - msg = xmlrpclib.dumps((goal, robot_name), 'completed') + print("status: ", status) + if status == Component_Status.ERROR: + msg = xmlrpclib.dumps((goal, robot_name), 'notify_error') + else: + msg = xmlrpclib.dumps((goal, robot_name), 'completed') self.event_queue.put(msg) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 63fbcee9..79fe8aa1 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -227,10 +227,16 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): msg = self.component_event_queue.get() (target_pos, robot_name), methodname = xmlrpc.client.loads(msg) command_id = self.goal_command_table[str(target_pos)] - print("Command (id: {}) was completed by {} which reached goal {}."\ - .format(command_id, robot_name, target_pos)) - self.completed(command_id, Component_Status.READY.value) - del self.goal_command_table[str(target_pos)] + if methodname == 'completed': + print("Command (id: {}) was completed by {} which reached goal {}."\ + .format(command_id, robot_name, target_pos)) + self.completed(command_id, Component_Status.READY.value) + del self.goal_command_table[str(target_pos)] + elif methodname == 'notify_error': + print("Command (id: {}) was failed (robot: {}, goal: {})"\ + .format(command_id, robot_name, target_pos)) + self.notify_error(command_id, Component_Status.ERROR.value) + time.sleep(3) -- GitLab From 52bdbf2df49d266cc9a6f13663bf638a2a46a955 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 02:21:00 +0900 Subject: [PATCH 05/36] Upgrade debug message with colored test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Windows 環境だと動かない可能性があるので注意 https://gkalumnium.com/devlog/termcolorでpythonのprint文に色を付けよう！/ --- route_guidance_ros/requirements.txt | 1 + route_guidance_ros/scripts/main_engine.py | 9 +++++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/requirements.txt b/route_guidance_ros/requirements.txt index 5221619b..ff3e89cf 100644 --- a/route_guidance_ros/requirements.txt +++ b/route_guidance_ros/requirements.txt @@ -1,2 +1,3 @@ pyrois numpy +termcolor diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 79fe8aa1..0dfc09ce 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -172,6 +172,7 @@ from VirtualNavigation_client import VirtualNavigation_Client as VNavClient from sub_engine_client import IF as EngineClient from pyrois.RoIS_Common import Component_Status import time +from termcolor import colored class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): """IF @@ -228,13 +229,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): (target_pos, robot_name), methodname = xmlrpc.client.loads(msg) command_id = self.goal_command_table[str(target_pos)] if methodname == 'completed': - print("Command (id: {}) was completed by {} which reached goal {}."\ - .format(command_id, robot_name, target_pos)) + print(colored("Command (id: {}) was completed by {} which reached goal {}."\ + .format(command_id, robot_name, target_pos), "green")) self.completed(command_id, Component_Status.READY.value) del self.goal_command_table[str(target_pos)] elif methodname == 'notify_error': - print("Command (id: {}) was failed (robot: {}, goal: {})"\ - .format(command_id, robot_name, target_pos)) + print(colored("Command (id: {}) was failed (robot: {}, goal: {})"\ + .format(command_id, robot_name, target_pos), "red")) self.notify_error(command_id, Component_Status.ERROR.value) time.sleep(3) -- GitLab From bb443875b5a078b84c6e9cc03825d726d4d5e891 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 02:32:00 +0900 Subject: [PATCH 06/36] Remove unused test script --- route_guidance_ros/scripts/test_app.py | 23 ----------------------- 1 file changed, 23 deletions(-) delete mode 100644 route_guidance_ros/scripts/test_app.py diff --git a/route_guidance_ros/scripts/test_app.py b/route_guidance_ros/scripts/test_app.py deleted file mode 100644 index 4e33277f..00000000 --- a/route_guidance_ros/scripts/test_app.py +++ /dev/null @@ -1,23 +0,0 @@ -# 1. launch multi navigation. -# 2. run this test. - - -from service_app import Service_Application_IF as Service -import time -from pyrois.RoIS_Common import Component_Status -from utilities import setup_multi_robot - -process = setup_multi_robot() - -time.sleep(10) -print("launching Service...") -service = Service('http://localhost:8000') -print("Service was established.") -for dest in [[25, 0, 0], [0, 20, 0], [5, 0, 0]]: - status = service.go_robot_to(dest) - print(status) - time.sleep(5) -# service.all_finished() - -time.sleep(30) -print("finish") -- GitLab From fdd257e0ca6e4514a290117b3d5239fe7ae821dd Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 13:22:31 +0900 Subject: [PATCH 07/36] Move service_apps from \(route_guidance\)_ros to \1_service --- .../scripts => route_guidance_service}/service_app_lv1.py | 0 .../scripts => route_guidance_service}/service_app_lv2.py | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename {route_guidance_ros/scripts => route_guidance_service}/service_app_lv1.py (100%) rename {route_guidance_ros/scripts => route_guidance_service}/service_app_lv2.py (100%) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_service/service_app_lv1.py similarity index 100% rename from route_guidance_ros/scripts/service_app_lv1.py rename to route_guidance_service/service_app_lv1.py diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_service/service_app_lv2.py similarity index 100% rename from route_guidance_ros/scripts/service_app_lv2.py rename to route_guidance_service/service_app_lv2.py -- GitLab From 2a167a86d7383315b77e71fad2eff4d656beaf2f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 15:29:13 +0900 Subject: [PATCH 08/36] Revert "Modify event handler with event-type" This reverts commit 31aa15e4fed5031a5101925a34d439629c27d4c8. --- route_guidance_ros/scripts/VirtualNavigation.py | 6 +----- route_guidance_ros/scripts/main_engine.py | 14 ++++---------- 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index aa79edfe..7246af21 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -115,11 +115,7 @@ 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) - print("status: ", status) - if status == Component_Status.ERROR: - msg = xmlrpclib.dumps((goal, robot_name), 'notify_error') - else: - msg = xmlrpclib.dumps((goal, robot_name), 'completed') + msg = xmlrpclib.dumps((goal, robot_name), 'completed') self.event_queue.put(msg) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 0dfc09ce..042d1478 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -228,16 +228,10 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): msg = self.component_event_queue.get() (target_pos, robot_name), methodname = xmlrpc.client.loads(msg) command_id = self.goal_command_table[str(target_pos)] - if methodname == 'completed': - print(colored("Command (id: {}) was completed by {} which reached goal {}."\ - .format(command_id, robot_name, target_pos), "green")) - self.completed(command_id, Component_Status.READY.value) - del self.goal_command_table[str(target_pos)] - elif methodname == 'notify_error': - print(colored("Command (id: {}) was failed (robot: {}, goal: {})"\ - .format(command_id, robot_name, target_pos), "red")) - self.notify_error(command_id, Component_Status.ERROR.value) - + print("Command (id: {}) was completed by {} which reached goal {}."\ + .format(command_id, robot_name, target_pos)) + self.completed(command_id, Component_Status.READY.value) + del self.goal_command_table[str(target_pos)] time.sleep(3) -- GitLab From 4bfdf702b8ce2ff62ce9f65cdede173323b3b972 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 15:47:21 +0900 Subject: [PATCH 09/36] Add symlink to utilities script --- route_guidance_service/utilities.py | 1 + 1 file changed, 1 insertion(+) create mode 120000 route_guidance_service/utilities.py diff --git a/route_guidance_service/utilities.py b/route_guidance_service/utilities.py new file mode 120000 index 00000000..af137a3c --- /dev/null +++ b/route_guidance_service/utilities.py @@ -0,0 +1 @@ +../route_guidance_ros/scripts/utilities.py \ No newline at end of file -- GitLab From 8437663f733c77349b28cfac820e8924f928609c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 15:51:01 +0900 Subject: [PATCH 10/36] Modify way to send event of command completed and failed --- .../scripts/VirtualNavigation.py | 2 +- .../scripts/VirtualNavigation_client.py | 2 +- route_guidance_ros/scripts/main_engine.py | 20 ++++++++++++++----- 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 7246af21..0441a611 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -115,7 +115,7 @@ 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) - msg = xmlrpclib.dumps((goal, robot_name), 'completed') + msg = xmlrpclib.dumps((goal, robot_name, status.value), 'completed') self.event_queue.put(msg) diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index 54d44369..4451bd22 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -113,5 +113,5 @@ class VirtualNavigation_Client(Command, Query, Event): """poll_event """ msg = self._e_proxy.poll_event() - (params, methodname) = xmlrpc.client.loads(msg) + (params, methodname, status) = xmlrpc.client.loads(msg) self.engine_event_queue.put(msg) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 042d1478..d1546338 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -171,6 +171,7 @@ class EventIF(RoIS_HRI.EventIF): from VirtualNavigation_client import VirtualNavigation_Client as VNavClient from sub_engine_client import IF as EngineClient from pyrois.RoIS_Common import Component_Status +from pyrois.RoIS_Service import Completed_Status import time from termcolor import colored @@ -226,12 +227,21 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): def analyze_vnav_status(self): while True: msg = self.component_event_queue.get() - (target_pos, robot_name), methodname = xmlrpc.client.loads(msg) + (target_pos, robot_name, status), methodname = xmlrpc.client.loads(msg) command_id = self.goal_command_table[str(target_pos)] - print("Command (id: {}) was completed by {} which reached goal {}."\ - .format(command_id, robot_name, target_pos)) - self.completed(command_id, Component_Status.READY.value) - del self.goal_command_table[str(target_pos)] + status = Component_Status(status) + if status == Component_Status.READY: + print(colored("Command (id: {}) was succeeded by {} which reached goal {}."\ + .format(command_id, robot_name, target_pos), "green")) + self.completed(command_id, Completed_Status.OK) + del self.goal_command_table[str(target_pos)] + time.sleep(3) + elif status == Component_Status.ERROR: + print(colored("Command (id: {}) was aborted by {} which couldn't reach goal {}."\ + .format(command_id, robot_name, target_pos), "red")) + self.completed(command_id, Completed_Status.ABORT) + else: + raise RuntimeError("Unhandled kind of vnav-status.") time.sleep(3) -- GitLab From f9de2a662a9b2fab0d04253c0e677f808c354106 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 15:51:55 +0900 Subject: [PATCH 11/36] Revert "Move service_apps from \(route_guidance\)_ros to \1_service" This reverts commit fdd257e0ca6e4514a290117b3d5239fe7ae821dd. --- .../scripts}/service_app_lv1.py | 0 .../scripts}/service_app_lv2.py | 0 route_guidance_service/utilities.py | 1 - 3 files changed, 1 deletion(-) rename {route_guidance_service => route_guidance_ros/scripts}/service_app_lv1.py (100%) rename {route_guidance_service => route_guidance_ros/scripts}/service_app_lv2.py (100%) delete mode 120000 route_guidance_service/utilities.py diff --git a/route_guidance_service/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py similarity index 100% rename from route_guidance_service/service_app_lv1.py rename to route_guidance_ros/scripts/service_app_lv1.py diff --git a/route_guidance_service/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py similarity index 100% rename from route_guidance_service/service_app_lv2.py rename to route_guidance_ros/scripts/service_app_lv2.py diff --git a/route_guidance_service/utilities.py b/route_guidance_service/utilities.py deleted file mode 120000 index af137a3c..00000000 --- a/route_guidance_service/utilities.py +++ /dev/null @@ -1 +0,0 @@ -../route_guidance_ros/scripts/utilities.py \ No newline at end of file -- GitLab From 85c769c49be789484da66869f81b696c253b6389 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 16:16:54 +0900 Subject: [PATCH 12/36] Fix event message type and structure --- route_guidance_ros/scripts/VirtualNavigation_client.py | 2 +- route_guidance_ros/scripts/main_engine.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py index 4451bd22..54d44369 100644 --- a/route_guidance_ros/scripts/VirtualNavigation_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -113,5 +113,5 @@ class VirtualNavigation_Client(Command, Query, Event): """poll_event """ msg = self._e_proxy.poll_event() - (params, methodname, status) = xmlrpc.client.loads(msg) + (params, methodname) = xmlrpc.client.loads(msg) self.engine_event_queue.put(msg) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index d1546338..283f41b9 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -233,13 +233,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): if status == Component_Status.READY: print(colored("Command (id: {}) was succeeded by {} which reached goal {}."\ .format(command_id, robot_name, target_pos), "green")) - self.completed(command_id, Completed_Status.OK) + self.completed(command_id, Completed_Status.OK.value) del self.goal_command_table[str(target_pos)] time.sleep(3) elif status == Component_Status.ERROR: print(colored("Command (id: {}) was aborted by {} which couldn't reach goal {}."\ .format(command_id, robot_name, target_pos), "red")) - self.completed(command_id, Completed_Status.ABORT) + self.completed(command_id, Completed_Status.ABORT.value) else: raise RuntimeError("Unhandled kind of vnav-status.") time.sleep(3) -- GitLab From a467188e4c192c33ee32b8764fa7f1e847140ddb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 16:54:30 +0900 Subject: [PATCH 13/36] Fix task management process on service --- route_guidance_ros/scripts/service_app_lv1.py | 5 +++-- route_guidance_ros/scripts/service_app_lv2.py | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index 5a220f81..56abfeff 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -2,6 +2,7 @@ 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 import logging import queue @@ -24,7 +25,7 @@ class TaskState(object): TaskKind.End: TaskKind.End } - def __init__(self, task_kind, status=Component_Status.BUSY): + def __init__(self, task_kind, status=None): self.task_kind = task_kind self.status = status @@ -44,7 +45,7 @@ class Service(Service_Application_IF): def completed(self, command_id, status): task = self.commandid_taskstate_table[command_id] - status = Component_Status(status) + status = Completed_Status(status) task.status = status next_task = task.next() if next_task == TaskKind.NaviToGuide: diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 5a220f81..56abfeff 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -2,6 +2,7 @@ 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 import logging import queue @@ -24,7 +25,7 @@ class TaskState(object): TaskKind.End: TaskKind.End } - def __init__(self, task_kind, status=Component_Status.BUSY): + def __init__(self, task_kind, status=None): self.task_kind = task_kind self.status = status @@ -44,7 +45,7 @@ class Service(Service_Application_IF): def completed(self, command_id, status): task = self.commandid_taskstate_table[command_id] - status = Component_Status(status) + status = Completed_Status(status) task.status = status next_task = task.next() if next_task == TaskKind.NaviToGuide: -- GitLab From 7e87a74d00df4d8b46555377a2c310e0809ca490 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 17:36:42 +0900 Subject: [PATCH 14/36] Modify VirtualNavigation to send goal to only active robot --- .../scripts/VirtualNavigation.py | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 0441a611..0a19ecad 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -88,7 +88,8 @@ class VirtualNavigation(Event, Command, Query): self._component.Routing_Policy = "" self.robot_goal_table = {} self.nav_state_table = \ - {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} + {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")) def position_to_coord(self, position): @@ -99,7 +100,7 @@ class VirtualNavigation(Event, Command, Query): def set_parameter(self, target_position, time_limit, routing_policy): target_coord = self.position_to_coord(target_position) - nearest_robot = self._component.get_nearest_robotname(target_coord) + nearest_robot = self._component.get_nearest_robotname(target_coord, self.nav_state_table) th = threading.Thread( target=self.send_goal_and_event, args=(target_position, nearest_robot)) @@ -146,19 +147,23 @@ class Component(object): pos = amcl_pose.pose.pose.position self.__robot_positions[robot_name] = np.array([pos.x, pos.y, pos.z]) - def send_goal_to_nearest_robot(self, dest): + def send_goal_to_nearest_robot(self, dest, state_table): dest = np.array(dest) - distance_table = \ - {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} + distance_table = {} + for name, pos in self.__robot_positions.items(): + distance_table[name] = np.linalg.norm(dest - pos) \ + if state_table[name] == Component_Status.READY else float('inf') nearest_robot = min(distance_table, key=distance_table.get) print("nearest_robot is ", nearest_robot) self.__goal_senders[nearest_robot].send_goal(dest.tolist()) return nearest_robot - def get_nearest_robotname(self, dest): + def get_nearest_robotname(self, dest, state_table): dest = np.array(dest) - distance_table = \ - {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} + distance_table = {} + for name, pos in self.__robot_positions.items(): + distance_table[name] = np.linalg.norm(dest - pos) \ + if state_table[name] == Component_Status.READY else float('inf') nearest_robot = min(distance_table, key=distance_table.get) return nearest_robot -- GitLab From 666ee6de5806d878d3c27c7c34541c4601e4b145 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 18:31:50 +0900 Subject: [PATCH 15/36] Add requirements of transitions --- route_guidance_ros/requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/route_guidance_ros/requirements.txt b/route_guidance_ros/requirements.txt index ff3e89cf..5495fb3e 100644 --- a/route_guidance_ros/requirements.txt +++ b/route_guidance_ros/requirements.txt @@ -1,3 +1,4 @@ pyrois numpy termcolor +transitions -- GitLab From 9287f807fcf2f9ff18684abfdfe8fce815034c8b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 18:32:20 +0900 Subject: [PATCH 16/36] Make Service as StateMachine with transitions mudule --- route_guidance_ros/scripts/service_app_lv2.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 56abfeff..4c0cc252 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -35,10 +35,21 @@ class TaskState(object): def next(self): return TaskState.__task_seq_table[self.task_kind] +from transitions import Machine class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) + # buid state machine + states = ['Init', 'NaviToUser', 'NaviToGuide', 'End'] + transitions = [ + {'trigger': 'StartService', 'source': 'Init', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, + {'trigger': 'NaviToUserSucceeded', 'source': 'NaviToUser', 'dest': 'NaviToGuide', 'after': 'start_navi_to_guide'}, + {'trigger': 'NaviToUserFailed', 'source': 'NaviToUser', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, + {'trigger': 'NaviToGuideSucceeded', 'source': 'NaviToGuide', 'dest': 'End'}, + ] + self.machine = Machine(self, states=states, transitions=transitions, initial='Init') + self.target_pos = "point_c" self.dest_pos = "point_a" self.commandid_taskstate_table = {} -- GitLab From b37e95925d723ebe691737b0743994caa5309a07 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 19:21:37 +0900 Subject: [PATCH 17/36] Use callback functions of state-machine --- route_guidance_ros/scripts/service_app_lv2.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 4c0cc252..9f016f36 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -35,6 +35,7 @@ class TaskState(object): def next(self): return TaskState.__task_seq_table[self.task_kind] +from termcolor import colored from transitions import Machine class Service(Service_Application_IF): @@ -46,12 +47,11 @@ class Service(Service_Application_IF): {'trigger': 'StartService', 'source': 'Init', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, {'trigger': 'NaviToUserSucceeded', 'source': 'NaviToUser', 'dest': 'NaviToGuide', 'after': 'start_navi_to_guide'}, {'trigger': 'NaviToUserFailed', 'source': 'NaviToUser', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, - {'trigger': 'NaviToGuideSucceeded', 'source': 'NaviToGuide', 'dest': 'End'}, + {'trigger': 'NaviToGuideSucceeded', 'source': 'NaviToGuide', 'dest': 'End', 'after': 'end_service' }, ] - self.machine = Machine(self, states=states, transitions=transitions, initial='Init') - self.target_pos = "point_c" self.dest_pos = "point_a" + self.machine = Machine(self, states=states, transitions=transitions, initial='Init') self.commandid_taskstate_table = {} def completed(self, command_id, status): @@ -60,9 +60,9 @@ class Service(Service_Application_IF): task.status = status next_task = task.next() if next_task == TaskKind.NaviToGuide: - self.start_navi_to_guide(self.dest_pos) + self.NaviToUserSucceeded(self.dest_pos) elif next_task == TaskKind.End: - print("Route Guide Finished !!") + self.NaviToGuideSucceeded() self.commandid_taskstate_table[command_id] = task print(self.commandid_taskstate_table) @@ -84,11 +84,13 @@ class Service(Service_Application_IF): stautus = RoIS_HRI.ReturnCode_t(return_code) self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToGuide) + def end_service(self): + 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__': -- GitLab From 77d923266186c7631e8ffb6b6c89ed4b6acf268b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 19:46:45 +0900 Subject: [PATCH 18/36] Remove old code for task management --- route_guidance_ros/scripts/service_app_lv2.py | 44 +++---------------- 1 file changed, 7 insertions(+), 37 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 9f016f36..8b9598bf 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -10,34 +10,10 @@ import threading import time import enum import xmlrpc - - -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] - from termcolor import colored from transitions import Machine + class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) @@ -52,19 +28,13 @@ class Service(Service_Application_IF): self.target_pos = "point_c" self.dest_pos = "point_a" self.machine = Machine(self, states=states, transitions=transitions, initial='Init') - self.commandid_taskstate_table = {} - 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: + def completed(self, command_id, completed_status): + completed_status = Completed_Status(completed_status) + if self.state == 'NaviToUser': self.NaviToUserSucceeded(self.dest_pos) - elif next_task == TaskKind.End: + elif self.state == 'NaviToGuide': self.NaviToGuideSucceeded() - self.commandid_taskstate_table[command_id] = task - print(self.commandid_taskstate_table) def notify_error(self, error_id, error_type): print('received error event {}({}) '.format( \ @@ -75,14 +45,14 @@ class Service(Service_Application_IF): event_id, event_type, subscribe_id, expire)) def start_navi_to_user(self, dest): + 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): + 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): print(colored("Route Guidance Completed !", 'cyan')) -- GitLab From dbecc17fec9a321bebc71934302834a6a71b2149 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 19:59:41 +0900 Subject: [PATCH 19/36] Use Task enum class --- route_guidance_ros/scripts/service_app_lv2.py | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index 8b9598bf..e38956ed 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -14,26 +14,33 @@ from termcolor import colored from transitions import Machine +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 = ['Init', 'NaviToUser', 'NaviToGuide', 'End'] + states = [Task.Init, Task.NaviToUser, Task.NaviToGuide, Task.End] transitions = [ - {'trigger': 'StartService', 'source': 'Init', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, - {'trigger': 'NaviToUserSucceeded', 'source': 'NaviToUser', 'dest': 'NaviToGuide', 'after': 'start_navi_to_guide'}, - {'trigger': 'NaviToUserFailed', 'source': 'NaviToUser', 'dest': 'NaviToUser', 'after': 'start_navi_to_user' }, - {'trigger': 'NaviToGuideSucceeded', 'source': 'NaviToGuide', 'dest': 'End', 'after': 'end_service' }, + {'trigger': 'StartService', 'source': Task.Init, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviToUserSucceeded', 'source': Task.NaviToUser, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, + {'trigger': 'NaviToUserFailed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, + {'trigger': 'NaviToGuideSucceeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, ] self.target_pos = "point_c" self.dest_pos = "point_a" - self.machine = Machine(self, states=states, transitions=transitions, initial='Init') + self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init) def completed(self, command_id, completed_status): completed_status = Completed_Status(completed_status) - if self.state == 'NaviToUser': + if self.state == Task.NaviToUser: self.NaviToUserSucceeded(self.dest_pos) - elif self.state == 'NaviToGuide': + elif self.state == Task.NaviToGuide: self.NaviToGuideSucceeded() def notify_error(self, error_id, error_type): -- GitLab From 548d3ec1ec18686155a53c4d32289349366b06da Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 20:16:47 +0900 Subject: [PATCH 20/36] Unify trigger of NaviToUser and NaviToGuidance --- route_guidance_ros/scripts/service_app_lv2.py | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv2.py b/route_guidance_ros/scripts/service_app_lv2.py index e38956ed..5926e186 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -27,21 +27,21 @@ class Service(Service_Application_IF): # 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': 'NaviToUserSucceeded', 'source': Task.NaviToUser, 'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'}, - {'trigger': 'NaviToUserFailed', 'source': Task.NaviToUser, 'dest': Task.NaviToUser, 'after': 'start_navi_to_user' }, - {'trigger': 'NaviToGuideSucceeded', 'source': Task.NaviToGuide, 'dest': Task.End, 'after': 'end_service' }, + {'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.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init) + self.machine = Machine(self, states=states, transitions=transitions, initial=Task.Init, send_event=True) def completed(self, command_id, completed_status): completed_status = Completed_Status(completed_status) - if self.state == Task.NaviToUser: - self.NaviToUserSucceeded(self.dest_pos) - elif self.state == Task.NaviToGuide: - self.NaviToGuideSucceeded() + if completed_status == Completed_Status.OK: + self.NaviSucceeded(self.dest_pos) + elif completed_status == Completed_Status.ERROR: + self.NaviFailed(self.target_pos) def notify_error(self, error_id, error_type): print('received error event {}({}) '.format( \ @@ -51,17 +51,19 @@ 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) - 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) - def end_service(self): + def end_service(self, event): print(colored("Route Guidance Completed !", 'cyan')) def run(self): -- GitLab From 3d09de2bec035f2a33d51db6ff951a06309e125c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 20:42:57 +0900 Subject: [PATCH 21/36] Fix type of navigation error code ERROR => ABORT --- 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 5926e186..f5b54273 100644 --- a/route_guidance_ros/scripts/service_app_lv2.py +++ b/route_guidance_ros/scripts/service_app_lv2.py @@ -40,7 +40,7 @@ class Service(Service_Application_IF): completed_status = Completed_Status(completed_status) if completed_status == Completed_Status.OK: self.NaviSucceeded(self.dest_pos) - elif completed_status == Completed_Status.ERROR: + elif completed_status == Completed_Status.ABORT: self.NaviFailed(self.target_pos) def notify_error(self, error_id, error_type): -- GitLab From 7038a855ae70c4195971521c16bb589f3a7fc4d9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 9 Dec 2019 23:37:35 +0900 Subject: [PATCH 22/36] Move init_pos_table from main_engine to VirtualNavigation --- route_guidance_ros/scripts/VirtualNavigation.py | 5 +++++ route_guidance_ros/scripts/main_engine.py | 5 ----- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 0a19ecad..5c7f88a8 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -86,6 +86,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 ], + 'robot2': [10.0, 0.0 ], + 'robot3': [0.0, 10.0] + } self.robot_goal_table = {} self.nav_state_table = \ {robot_name : Component_Status.READY for robot_name in robot_names} diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 283f41b9..a80f77ed 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() -- GitLab From 1a7885a88b5ae2ff704e3b70b64995a474eaffcb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 10 Dec 2019 00:17:11 +0900 Subject: [PATCH 23/36] Modify VNAV to back stucked robot to init position --- route_guidance_ros/scripts/VirtualNavigation.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 5c7f88a8..e0d6d952 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 # @@ -87,9 +88,9 @@ class VirtualNavigation(Event, Command, Query): self._component.Time_Limit = 10 self._component.Routing_Policy = "" self.init_pos_table = { - 'robot1': [0.0, 0.0 ], - 'robot2': [10.0, 0.0 ], - 'robot3': [0.0, 10.0] + '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 = \ @@ -104,6 +105,14 @@ class VirtualNavigation(Event, Command, Query): return position def set_parameter(self, target_position, time_limit, routing_policy): + # ERRORなロボットは初期位置に戻す + for robot_name, state in self.nav_state_table.items(): + if state == Component_Status.ERROR: + init_pos = self.init_pos_table[robot_name] + self._component.send_goal_to_robot(init_pos, robot_name) + self.nav_state_table[robot_name] = Component_Status.BUSY + + # 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( -- GitLab From f537231bf7aab962cd4e7fc535486161e4a797d6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 10 Dec 2019 01:06:12 +0900 Subject: [PATCH 24/36] Update robot state when back to home is finished --- .../scripts/VirtualNavigation.py | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index e0d6d952..2cd3a62f 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -106,11 +106,7 @@ class VirtualNavigation(Event, Command, Query): def set_parameter(self, target_position, time_limit, routing_policy): # ERRORなロボットは初期位置に戻す - for robot_name, state in self.nav_state_table.items(): - if state == Component_Status.ERROR: - init_pos = self.init_pos_table[robot_name] - self._component.send_goal_to_robot(init_pos, robot_name) - self.nav_state_table[robot_name] = Component_Status.BUSY + self.back_stucked_robots_to_init_pos() # READYなロボットの中でゴールに最も近い位置にいるものにNavigationさせる target_coord = self.position_to_coord(target_position) @@ -133,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) -- GitLab From 2fa2b5f674631364b36a282814fb64d53ac5f1bb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 10 Dec 2019 16:00:45 +0900 Subject: [PATCH 25/36] Add Speech_Recognition component server and client --- .../scripts/Dummy_Speech_Recognition.py | 152 ++++++++++++++++++ .../Dummy_Speech_Recognition_client.py | 115 +++++++++++++ 2 files changed, 267 insertions(+) create mode 100644 route_guidance_ros/scripts/Dummy_Speech_Recognition.py create mode 100644 route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py new file mode 100644 index 00000000..6ec83614 --- /dev/null +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -0,0 +1,152 @@ +# (Dummy) Speech_Recognition.py +# + +"""Speech_Recognition +""" + +import sys +import queue +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, c): + self._component = c + + def start(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def set_parameter(self, recognizable_languages, languages, grammer, rule): + self._component.Recognizable_Languages = recognizable_languages + self._component.Languages = languages + self._component.Grammer = grammer + self._component.Rule = rule + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, c): + self._component = c + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + + def get_parameter(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return (status, self._component.Languages, self._component.Grammer, self._component.Rule) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + def poll_event(self): + """poll_event + """ + msg = self.event_queue.get() + return msg + + def speech_recognized(self, timestamp, recognized_text): + """speech_recognition + """ + msg = xmlrpc.client.dumps((timestamp, recognized_text), 'speech_recognized') + self.event_queue.put(msg) + + def speech_input_started(self, timestamp): + msg = xmlrpc.client.dumps((timestamp), 'speech_input_started') + self.event_queue.put(msg) + + def speech_input_finished(self, timestamp): + msg = xmlrpc.client.dumps((timestamp), 'speech_input_finished') + self.event_queue.put(msg) + + +class Speech_Recognition(Event, Command, Query): + """Speech_Recognition + """ + def __init__(self, c): + super().__init__(c) + self._component = c + self._component.Recognizable_Languages = set("") + self._component.Languages = set("English") + self._component.Grammer = "" #??? + self._component.Rule = "" #??? + + +class component: + """component + """ + def __init__(self): + self._state = False + + +def event_dispatch(sr): + """event_dispatch + """ + # sr.speech_recognized(datetime.now().isoformat(), [""]) + # time.sleep(0.1) + # sr.speech_recognized(datetime.now().isoformat(), [""]) + + +def example_sr(port): + """example_sr + """ + c = component() + sr = Speech_Recognition(c) + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(sr,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(sr) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + example_sr(8042) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py new file mode 100644 index 00000000..2bc9b1fd --- /dev/null +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py @@ -0,0 +1,115 @@ +# (Dummy) Speech_Recognition_Client.py + +"""Speech_Recognition_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def start(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.start()) + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) + return status + + def set_parameter(self, languages, grammer, rule): + s = self._proxy.set_parameter(languages, grammer, rule) + status = RoIS_HRI.ReturnCode_t(s) + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def component_status(self): + (status, c_status) = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + def get_parameter(self): + (status, recognizable_languages, languages, grammer, rule) = self._proxy.get_parameter() + return (RoIS_HRI.ReturnCode_t(status), recognizable_languages, languages, grammer, rule) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, uri): + self._uri = uri + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + # if logger is not None: + # self.logger = logger + + def start_th(self): + self.th = threading.Thread(target=self.event_loop) + self.th.start() + + def event_loop(self): + """event_loop + """ + while True: + try: + self.poll_event() + except ConnectionRefusedError: + break + + def poll_event(self): + """poll_event + """ + msg = self._e_proxy.poll_event() + (params, methodname) = xmlrpc.client.loads(msg) + #self.logger.debug('poll_event: '+methodname) + print(params,methodname) + if methodname == 'speech_recognized': + self.speech_recognized(params[0], params[1]) + + def speech_recognized(self, timestamp, recognized_text): + self.events.append((timestamp,recognized_text)) + print("speech_recognized",timestamp, recognized_text) + # self.logger.debug('received completed event' + # + command_id + # + RoIS_Service.Completed_Status(status).name) + + def speech_input_started(self, timestamp): + self.events.append((timestamp)) + print("speech_input_started", timestamp) + + def speech_input_finished(self, timestamp): + self.events.append((timestamp)) + print("speech_input_started", timestamp) + + +class Speech_Recognition_Client(Command, Query, Event): + """Speech_Recognition_Client + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + self.start_th() -- GitLab From 46643112dac21c90b7aa999e7ab3c332c8cd101b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 10 Dec 2019 16:21:33 +0900 Subject: [PATCH 26/36] Enable dummy_event dispacher on Dummy_Speech_Recognition --- route_guidance_ros/scripts/Dummy_Speech_Recognition.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 6ec83614..cf0c2a26 100644 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -125,9 +125,9 @@ class component: def event_dispatch(sr): """event_dispatch """ - # sr.speech_recognized(datetime.now().isoformat(), [""]) - # time.sleep(0.1) - # sr.speech_recognized(datetime.now().isoformat(), [""]) + sr.speech_recognized(datetime.now().isoformat(), [""]) + time.sleep(0.5) + sr.speech_recognized(datetime.now().isoformat(), [""]) def example_sr(port): @@ -137,7 +137,7 @@ def example_sr(port): sr = Speech_Recognition(c) # start the timer to dispatch events - t = threading.Timer(0.1, event_dispatch, args=(sr,)) + t = threading.Timer(3, event_dispatch, args=(sr,)) t.start() # start the XML-RPC server -- GitLab From e28367f1ec09d946efdab3fad691abc0e60d39ab Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 10 Dec 2019 16:39:14 +0900 Subject: [PATCH 27/36] Add test for Speech_Recognition component --- route_guidance_ros/scripts/test_sr_component.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 route_guidance_ros/scripts/test_sr_component.py diff --git a/route_guidance_ros/scripts/test_sr_component.py b/route_guidance_ros/scripts/test_sr_component.py new file mode 100644 index 00000000..f42c3e2f --- /dev/null +++ b/route_guidance_ros/scripts/test_sr_component.py @@ -0,0 +1,13 @@ +# Test script for Speech_Recognition + +# 1. run Dummy_Speech_Recognition.py. +# 2. run this test. + + +from Dummy_Speech_Recognition_client import Speech_Recognition_Client +import time + + +uri = "http://localhost:8042" +client = Speech_Recognition_Client(uri) +time.sleep(10) -- GitLab From 68fa20349e03794fa118cfe4c07811f7882bd4c9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 10 Dec 2019 16:43:33 +0900 Subject: [PATCH 28/36] Add Speech_Recognition client to main_engine --- route_guidance_ros/scripts/main_engine.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index a80f77ed..9643ea63 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -169,6 +169,7 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) from VirtualNavigation_client import VirtualNavigation_Client as VNavClient +from Dummy_Speech_Recognition_client import Speech_Recognition_Client as SRecogClient from sub_engine_client import IF as EngineClient from pyrois.RoIS_Common import Component_Status from pyrois.RoIS_Service import Completed_Status @@ -184,7 +185,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.event_queue = queue.Queue() self.component_event_queue = queue.Queue() self.component_clients = { - 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue) + 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue), + 'Speech_Recognition': SRecogClient('http://localhost:8042') } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), -- GitLab From 30836d19e289eed9d62ce00c6cb7255506730cbf Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 10 Dec 2019 16:52:55 +0900 Subject: [PATCH 29/36] Modify utilities to launch Dummy_Speech_Recognition server --- route_guidance_ros/scripts/utilities.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 0802b5e4..e6b8b0e5 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -34,6 +34,11 @@ class SysInfoComponent(ComponentWrapper): super().__init__("System_Information.py", robot_name, port) +class SpeechRecogComponent(SubprocessWrapper): + def __init__(self): + super().__init__(["python3", "Dummy_Speech_Recognition.py"]) + + class SubEngineWrapper(SubprocessWrapper): def __init__(self, engine_port): super().__init__(["python3", "sub_engine.py", str(engine_port)]) @@ -49,6 +54,7 @@ def launch_components_and_subengines(**robot_port_table): for robot_name, engine_port in robot_port_table.items(): process.append(NavigationComponent(robot_name, engine_port+1)) process.append(SysInfoComponent(robot_name, engine_port+2)) + process.append(SpeechRecogComponent()) print("Components Constructed.") time.sleep(5) -- GitLab From 7d999fa76ce5fd6a86a78339b5059719d516ad5e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 20 Jan 2020 19:04:14 +0900 Subject: [PATCH 30/36] Add command line args to change port for each SR --- .../scripts/Dummy_Speech_Recognition.py | 16 ++++++++++++++-- route_guidance_ros/scripts/utilities.py | 6 +++--- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index cf0c2a26..89d49963 100644 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -50,7 +50,7 @@ class Command(RoIS_Common.Command): status = RoIS_HRI.ReturnCode_t.OK.value return status - def set_parameter(self, recognizable_languages, languages, grammer, rule): + def set_parameter(self, languages, grammer, rule): self._component.Recognizable_Languages = recognizable_languages self._component.Languages = languages self._component.Grammer = grammer @@ -114,6 +114,13 @@ class Speech_Recognition(Event, Command, Query): self._component.Grammer = "" #??? self._component.Rule = "" #??? + def set_parameter(self, languages, grammer, rule): + print("Speech_Recognition::set_parameter called.") + time.sleep(2) + self.speech_recognized(datetime.now(), ["WOW"]) + status = RoIS_HRI.ReturnCode_t.OK.value + return status + class component: """component @@ -149,4 +156,9 @@ def example_sr(port): if __name__ == '__main__': - example_sr(8042) + if len(sys.argv) < 2: + print("python3 ") + raise RuntimeError + + port = int(sys.argv[1]) + example_sr(port) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index e6b8b0e5..75a600dd 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -35,8 +35,8 @@ class SysInfoComponent(ComponentWrapper): class SpeechRecogComponent(SubprocessWrapper): - def __init__(self): - super().__init__(["python3", "Dummy_Speech_Recognition.py"]) + def __init__(self, port): + super().__init__(["python3", "Dummy_Speech_Recognition.py", str(port)]) class SubEngineWrapper(SubprocessWrapper): @@ -54,7 +54,7 @@ def launch_components_and_subengines(**robot_port_table): for robot_name, engine_port in robot_port_table.items(): process.append(NavigationComponent(robot_name, engine_port+1)) process.append(SysInfoComponent(robot_name, engine_port+2)) - process.append(SpeechRecogComponent()) + process.append(SpeechRecogComponent(engine_port+3)) print("Components Constructed.") time.sleep(5) -- GitLab From eeeaad544eb01284b337a40b59eb73fdddc8166d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 20 Jan 2020 19:05:15 +0900 Subject: [PATCH 31/36] Add Speech_Recognition component to sub_engine --- route_guidance_ros/scripts/sub_engine.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index e9215bf1..fe2ed45c 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -169,6 +169,7 @@ class EventIF(RoIS_HRI.EventIF): 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 pyrois.RoIS_Common import Component_Status import queue import time @@ -182,7 +183,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 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)) + 'System_Information': SysClient('http://localhost:' + str(engine_port+2)), + 'Speech_Recognition': SRecogClient('http://localhost:' + str(engine_port+3)) } self.command_id_status_table = {} @@ -205,6 +207,13 @@ 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): -- GitLab From 261a4abeff5ff350dc47f91a53cbbbffbe2f50dd Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 20 Jan 2020 19:07:37 +0900 Subject: [PATCH 32/36] Test Speech_Recognition component with test_sub_engine_with_service --- .../scripts/test_sub_engine_with_service.py | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/test_sub_engine_with_service.py b/route_guidance_ros/scripts/test_sub_engine_with_service.py index a0e9d53d..ca61e696 100644 --- a/route_guidance_ros/scripts/test_sub_engine_with_service.py +++ b/route_guidance_ros/scripts/test_sub_engine_with_service.py @@ -44,12 +44,24 @@ class Service(Service_Application_IF): print("commmand_status:\n", self.commmand_status_table) print("----------------------------------------------") + def recog_speech(self): + print("Starting recog_speech...") + time.sleep(3) + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + status, command_id = \ + self._proxy.set_parameter('Speech_Recognition', "lang", "grammer", "rule") + self.commmand_status_table[command_id] = CommandStatus(False) + print("----------------------------------------------") + print("commmand_status:\n", self.commmand_status_table) + print("----------------------------------------------") + + def run(self): destinations = ["point_e", "point_d", "point_c", "point_b"] for dest in destinations: - self.go_robot_to(dest) - while self.is_navigation_running: - time.sleep(1) + self.recog_speech() + time.sleep(3) process = setup_single_robot() -- GitLab From 6448a753b3e3bceec148cd0d0551645271b489b5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 20 Jan 2020 19:57:37 +0900 Subject: [PATCH 33/36] Randomize delay duration of speech-recongnition --- .../scripts/Dummy_Speech_Recognition.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 89d49963..5e846d4d 100644 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -102,6 +102,7 @@ class Event(RoIS_Common.Event): msg = xmlrpc.client.dumps((timestamp), 'speech_input_finished') self.event_queue.put(msg) +import random class Speech_Recognition(Event, Command, Query): """Speech_Recognition @@ -116,11 +117,24 @@ class Speech_Recognition(Event, Command, Query): def set_parameter(self, languages, grammer, rule): print("Speech_Recognition::set_parameter called.") - time.sleep(2) - self.speech_recognized(datetime.now(), ["WOW"]) + th = threading.Thread( + target=self.wait_and_send_speech_recognized_event, + daemon=True) + th.start() status = RoIS_HRI.ReturnCode_t.OK.value return status + def wait_and_send_speech_recognized_event(self): + detected_text = "point_a" + delay_duration = random.randint(3, 10) + time.sleep(delay_duration) + self.speech_recognized(datetime.now(), [detected_text]) + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + class component: """component -- GitLab From f1cf8ae0f77a3a5a3187423682e6bc99a0509f79 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 20 Jan 2020 20:18:39 +0900 Subject: [PATCH 34/36] Upgrade SR_client to send event to engine --- .../scripts/Dummy_Speech_Recognition_client.py | 12 +++++++++++- route_guidance_ros/scripts/sub_engine.py | 2 +- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py index 2bc9b1fd..d1280d88 100644 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition_client.py @@ -107,9 +107,19 @@ class Event(RoIS_Common.Event): class Speech_Recognition_Client(Command, Query, Event): """Speech_Recognition_Client """ - def __init__(self, uri): + def __init__(self, uri, engine_event_queue): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] + self.engine_event_queue = engine_event_queue self.start_th() + + def poll_event(self): + """poll_event + """ + msg = self._e_proxy.poll_event() + (params, methodname) = xmlrpc.client.loads(msg) + print("[Speech_Recognition_Client::poll_event]", params, methodname) + msg = xmlrpc.client.dumps((methodname, params), 'completed') + self.engine_event_queue.put(msg) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index fe2ed45c..28ddb009 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -184,7 +184,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 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)) + 'Speech_Recognition': SRecogClient('http://localhost:' + str(engine_port+3), self.event_queue) } self.command_id_status_table = {} -- GitLab From 63b4ce3d902781653957fdc4500100a20fe6f3bf Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 22 Jan 2020 15:55:03 +0900 Subject: [PATCH 35/36] Change event methodname from VNAV to specify event type --- route_guidance_ros/scripts/Dummy_Speech_Recognition.py | 10 ++++++---- route_guidance_ros/scripts/VirtualNavigation.py | 2 +- route_guidance_ros/scripts/main_engine.py | 6 +++--- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py index 5e846d4d..66923e74 100644 --- a/route_guidance_ros/scripts/Dummy_Speech_Recognition.py +++ b/route_guidance_ros/scripts/Dummy_Speech_Recognition.py @@ -130,10 +130,12 @@ class Speech_Recognition(Event, Command, Query): time.sleep(delay_duration) self.speech_recognized(datetime.now(), [detected_text]) - def component_status(self): - status = RoIS_HRI.ReturnCode_t.OK.value - c_status = RoIS_Common.Component_Status.UNINITIALIZED.value - return (status, c_status) + def speech_recognized(self, timestamp, recognized_text): + """speech_recognition + """ + msg = (timestamp, recognized_text), 'speech_recognized' + msg = xmlrpc.client.dumps(*msg) + self.event_queue.put(msg) class component: diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 2cd3a62f..bf7a35a8 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -126,7 +126,7 @@ 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) - msg = xmlrpclib.dumps((goal, robot_name, status.value), 'completed') + msg = xmlrpclib.dumps((goal, robot_name, status.value), 'navi_finished') self.event_queue.put(msg) def back_stucked_robots_to_init_pos(self): diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 9643ea63..bab490e9 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -186,7 +186,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.component_event_queue = queue.Queue() self.component_clients = { 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue), - 'Speech_Recognition': SRecogClient('http://localhost:8042') + # 'Speech_Recognition': SRecogClient('http://localhost:8042') } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), @@ -197,7 +197,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): for engine_client in self.engine_clients.values(): engine_client.connect() self.command_id_result_table = {} - th = threading.Thread(target=self.analyze_vnav_status, daemon=True) + th = threading.Thread(target=self.analyze_c_status, daemon=True) th.start() def set_parameter(self, component_ref, *parameters): @@ -221,7 +221,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): status = RoIS_HRI.ReturnCode_t.OK.value return status, self.command_id_result_table[command_id] - def analyze_vnav_status(self): + def analyze_c_status(self): while True: msg = self.component_event_queue.get() (target_pos, robot_name, status), methodname = xmlrpc.client.loads(msg) -- GitLab From 31c169b3e66938054e570238ef1a2888216f82ae Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 22 Jan 2020 16:00:48 +0900 Subject: [PATCH 36/36] Enable SpeechRecognition component on main-engine --- route_guidance_ros/scripts/main_engine.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index bab490e9..eb2f3f9d 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -186,7 +186,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.component_event_queue = queue.Queue() self.component_clients = { 'Navigation': VNavClient('http://localhost:8041', self.component_event_queue), - # 'Speech_Recognition': SRecogClient('http://localhost:8042') + 'Speech_Recognition': SRecogClient('http://localhost:8042', self.component_event_queue) } self.engine_clients = { 'robot1': EngineClient('http://localhost:8010'), @@ -215,6 +215,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): status, results = engine_client.get_command_result(command_id, "") self.command_id_result_table[str(self.command_id)].append(results) status = RoIS_HRI.ReturnCode_t.OK.value + elif component_ref == 'Speech_Recognition': + pass + return (status, str(self.command_id)) def get_command_result(self, command_id, condition): -- GitLab