From 2123ec772537c03c20871f443af12cef766a7198 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 26 Nov 2019 14:46:04 +0900 Subject: [PATCH 01/30] Modify vnav status subscriber --- route_guidance_ros/scripts/main_engine.py | 23 ++++++++--------------- 1 file changed, 8 insertions(+), 15 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index f76f9e9d..5f02faac 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -197,34 +197,27 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.goal_command_table = {} for engine_client in self.engine_clients.values(): engine_client.connect() + th = threading.Thread(target=self.analyze_vnav_status, daemon=True) + th.start() def set_parameter(self, component_ref, parameters): target_component = self.component_clients[component_ref] self.command_id += 1 status = target_component.set_parameter(*parameters).value - th = threading.Thread( - target=self.analyze_c_status, - daemon=True, - args=(self.component_clients[component_ref], str(self.command_id))) - th.start() goal = parameters[0] goal.append(0) self.goal_command_table[str(goal)] = str(self.command_id) return (status, str(self.command_id)) - def analyze_c_status(self, component, command_id): + def analyze_vnav_status(self): while True: - # Is component_event_queue thread-safe ? msg = self.component_event_queue.get() (target_pos, robot_name), methodname = xmlrpc.client.loads(msg) - if self.goal_command_table[str(target_pos)] == command_id: - 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)] - return - else: - self.component_event_queue.put(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)] time.sleep(3) -- GitLab From 4fcb71c6b83968039fd13181ab28de7a2f8b9ba0 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 27 Nov 2019 14:46:33 +0900 Subject: [PATCH 02/30] Modify shape of args of set_parameter of engines --- route_guidance_ros/scripts/main_engine.py | 2 +- route_guidance_ros/scripts/main_engine_client.py | 4 ++-- route_guidance_ros/scripts/sub_engine.py | 2 +- route_guidance_ros/scripts/sub_engine_client.py | 4 ++-- route_guidance_ros/scripts/test_main_engine.py | 2 +- route_guidance_ros/scripts/test_main_engine_with_service.py | 2 +- route_guidance_ros/scripts/test_sub_engine.py | 2 +- route_guidance_ros/scripts/test_sub_engine_with_service.py | 2 +- 8 files changed, 10 insertions(+), 10 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 5f02faac..2afd9674 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -200,7 +200,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): th = threading.Thread(target=self.analyze_vnav_status, daemon=True) th.start() - def set_parameter(self, component_ref, parameters): + def set_parameter(self, component_ref, *parameters): target_component = self.component_clients[component_ref] self.command_id += 1 status = target_component.set_parameter(*parameters).value diff --git a/route_guidance_ros/scripts/main_engine_client.py b/route_guidance_ros/scripts/main_engine_client.py index 1e7e3a18..3d982279 100644 --- a/route_guidance_ros/scripts/main_engine_client.py +++ b/route_guidance_ros/scripts/main_engine_client.py @@ -69,8 +69,8 @@ class CommandIF(RoIS_HRI.CommandIF): status = RoIS_HRI.ReturnCode_t(s) return (status, parameter_list) - def set_parameter(self, component_ref, parameters): - (s, command_id) = self._proxy.set_parameter(component_ref, parameters) + def set_parameter(self, component_ref, *parameters): + (s, command_id) = self._proxy.set_parameter(component_ref, *parameters) status = RoIS_HRI.ReturnCode_t(s) return (status, command_id) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index 6a90c252..fedc0f32 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -185,7 +185,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) } - def set_parameter(self, component_ref, parameters): + def set_parameter(self, component_ref, *parameters): status = None self.command_id += 1 if not component_ref in self.compoent_clients: diff --git a/route_guidance_ros/scripts/sub_engine_client.py b/route_guidance_ros/scripts/sub_engine_client.py index 0fc6e7ec..04f0b12a 100644 --- a/route_guidance_ros/scripts/sub_engine_client.py +++ b/route_guidance_ros/scripts/sub_engine_client.py @@ -71,8 +71,8 @@ class CommandIF(RoIS_HRI.CommandIF): status = RoIS_HRI.ReturnCode_t(s) return (status, parameter_list) - def set_parameter(self, component_ref, parameters): - (status, command_id) = self._proxy.set_parameter(component_ref, parameters) + def set_parameter(self, component_ref, *parameters): + (status, command_id) = self._proxy.set_parameter(component_ref, *parameters) status = RoIS_HRI.ReturnCode_t(status) return (status, command_id) diff --git a/route_guidance_ros/scripts/test_main_engine.py b/route_guidance_ros/scripts/test_main_engine.py index c00e159b..53750282 100644 --- a/route_guidance_ros/scripts/test_main_engine.py +++ b/route_guidance_ros/scripts/test_main_engine.py @@ -14,7 +14,7 @@ print("Engine was established.") time.sleep(10) engine.connect() for dest in [[25, 0], [0, 20], [5, 0]]: - status = engine.set_parameter('Navigation', [dest, "", ""]) + status = engine.set_parameter('Navigation', dest, "", "") print(status) time.sleep(5) # engine.all_finished() diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index 60718ad2..5a7b1f80 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -24,7 +24,7 @@ class Service(Service_Application_IF): def go_robot_to(self, dest): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) - (return_code, command_id) = self._proxy.set_parameter('Navigation', [dest, "", ""]) + (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") print("go_robot_to {} called with command_id {}".format(dest, command_id)) self.is_running = True diff --git a/route_guidance_ros/scripts/test_sub_engine.py b/route_guidance_ros/scripts/test_sub_engine.py index c6b6aad0..668dd648 100644 --- a/route_guidance_ros/scripts/test_sub_engine.py +++ b/route_guidance_ros/scripts/test_sub_engine.py @@ -14,7 +14,7 @@ engine.connect() engine.search("engine_profile_test") destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] for dest in destinations: - print(engine.set_parameter('Navigation',[dest, "", ""])) + print(engine.set_parameter('Navigation', dest, "", "")) navi_status = Component_Status.BUSY while navi_status != Component_Status.READY: # server_status, navi_status = engine.get_navi_status() 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 01f0dc39..85d9de48 100644 --- a/route_guidance_ros/scripts/test_sub_engine_with_service.py +++ b/route_guidance_ros/scripts/test_sub_engine_with_service.py @@ -38,7 +38,7 @@ class Service(Service_Application_IF): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) status, command_id = \ - self._proxy.set_parameter('Navigation',[dest, "", ""]) + self._proxy.set_parameter('Navigation', dest, "", "") self.commmand_status_table[command_id] = CommandStatus(False) print("----------------------------------------------") print("commmand_status:\n", self.commmand_status_table) -- GitLab From bc20e162e6b2294d043f160a22bc8eaafafd7e4e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 27 Nov 2019 15:24:27 +0900 Subject: [PATCH 03/30] Upgrade SubprocessWrapper with registering func to atexit --- route_guidance_ros/scripts/utilities.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py index 8ff7edbd..0802b5e4 100644 --- a/route_guidance_ros/scripts/utilities.py +++ b/route_guidance_ros/scripts/utilities.py @@ -1,13 +1,22 @@ import time +import subprocess from subprocess import Popen +import atexit +def register_process_killer(process): + def kill_subprocess(pid): + subprocess.run(["kill", "-SIGKILL", str(pid)]) + print("process {} was killed.".format(pid)) + atexit.register(kill_subprocess, process.pid) class SubprocessWrapper(object): def __init__(self, command): self.process = Popen(command) + register_process_killer(self.process) - def __del__(self): - self.process.kill() + # It is not guaranteed that __del__() methods are called when the interpreter exits. + # def __del__(self): + # self.process.kill() class ComponentWrapper(SubprocessWrapper): -- GitLab From 01efec93d95da438b9ff12d31d235e8e44cf1c69 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 27 Nov 2019 15:50:57 +0900 Subject: [PATCH 04/30] Add function to update and show robots position --- .../scripts/test_main_engine_with_service.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index 5a7b1f80..5074e7f5 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -15,6 +15,7 @@ class Service(Service_Application_IF): super().__init__(uri) self.current_dest_index = 0 self.is_running = False + self.robot_positions = [] def completed(self, command_id, status): print("Service command_id {} was completed.".format(command_id)) @@ -28,6 +29,12 @@ class Service(Service_Application_IF): print("go_robot_to {} called with command_id {}".format(dest, command_id)) self.is_running = True + def update_robot_positions(self): + pass + + def get_robot_positions(self): + return self.robot_positions + def run(self): destinations = [ [25, 10], [0, 20], [5, 0], [10, 0], [25, 20], [0, 5] @@ -35,6 +42,9 @@ class Service(Service_Application_IF): for dest in destinations: self.go_robot_to(dest) while self.is_running: + robot_positions = self.get_robot_positions() + print("robot_positions:") + [print(f"\t{i}: {pos}") for i, pos in enumerate(robot_positions)] time.sleep(3) -- GitLab From 5dd39d43289e374031c294d9b10ef60b2f7aecbe Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 27 Nov 2019 17:05:02 +0900 Subject: [PATCH 05/30] Create task managing system on test-service --- .../scripts/test_main_engine_with_service.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index 5074e7f5..85d71809 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -16,21 +16,30 @@ class Service(Service_Application_IF): self.current_dest_index = 0 self.is_running = False self.robot_positions = [] + self.commandid_task_table = {} def completed(self, command_id, status): print("Service command_id {} was completed.".format(command_id)) - self.current_dest_index += 1 - self.is_running = False + if self.commandid_task_table[command_id] == 'Navigation': + self.current_dest_index += 1 + self.is_running = False + else: # task 'RobotPosition' + status, results = self._proxy.get_command_result(command_id, None) + self.robot_positions = results def go_robot_to(self, dest): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") + self.commandid_task_table[command_id] = 'Navigation' print("go_robot_to {} called with command_id {}".format(dest, command_id)) self.is_running = True def update_robot_positions(self): - pass + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + (return_code, command_id) = self._proxy.set_parameter('RobotPosition') + self.commandid_task_table[command_id] = 'RobotPosition' def get_robot_positions(self): return self.robot_positions @@ -42,6 +51,7 @@ class Service(Service_Application_IF): for dest in destinations: self.go_robot_to(dest) while self.is_running: + self.update_robot_positions() robot_positions = self.get_robot_positions() print("robot_positions:") [print(f"\t{i}: {pos}") for i, pos in enumerate(robot_positions)] -- GitLab From 1a2429ed840c32b97c6d34d4efe16cbe16905cde Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 28 Nov 2019 02:46:46 +0900 Subject: [PATCH 06/30] Upgrade impl of Engine::set_parameter for RobotPosition --- route_guidance_ros/scripts/main_engine.py | 15 ++++++++++----- route_guidance_ros/scripts/sub_engine.py | 16 ++++++++++++---- 2 files changed, 22 insertions(+), 9 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 2afd9674..4d2e1dcb 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -201,12 +201,17 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): th.start() def set_parameter(self, component_ref, *parameters): - target_component = self.component_clients[component_ref] self.command_id += 1 - status = target_component.set_parameter(*parameters).value - goal = parameters[0] - goal.append(0) - self.goal_command_table[str(goal)] = str(self.command_id) + if component_ref == 'Navigation': + target_component = self.component_clients[component_ref] + status = target_component.set_parameter(*parameters).value + goal = parameters[0] + goal.append(0) + self.goal_command_table[str(goal)] = str(self.command_id) + elif component_ref == 'RobotPosition': + for engine_client in self.engine_clients.values(): + status, command_id = engine_client.set_parameter('System_Information') + status = RoIS_HRI.ReturnCode_t.OK.value return (status, str(self.command_id)) def analyze_vnav_status(self): diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index fedc0f32..be119145 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -191,11 +191,19 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): if not component_ref in self.compoent_clients: status = RoIS_HRI.ReturnCode_t.ERROR.value else: - print("[set_parameter] component_ref started") target_component_client = self.compoent_clients[component_ref] - 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() + if component_ref == 'Navigation': + 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() + elif component_ref == 'System_Information': + status = RoIS_HRI.ReturnCode_t.OK.value + print("robot_position called") + result = target_component_client.robot_position() + print(result) return (status, str(self.command_id)) ### Remove ### -- GitLab From 5c9bd6bc9a3d35c81633db047b4ea02e8437beef Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 28 Nov 2019 03:19:06 +0900 Subject: [PATCH 07/30] Upgrade sub-engine to get robot-position as command result --- route_guidance_ros/scripts/sub_engine.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index be119145..acc597db 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -184,6 +184,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) } + self.command_id_status_table = {} def set_parameter(self, component_ref, *parameters): status = None @@ -201,11 +202,15 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): th.start() elif component_ref == 'System_Information': status = RoIS_HRI.ReturnCode_t.OK.value - print("robot_position called") result = target_component_client.robot_position() - print(result) + self.command_id_status_table[str(self.command_id)] = results return (status, str(self.command_id)) + def get_command_result(self, command_id, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + results = self.command_id_status_table[command_id] + return (status, results) + ### Remove ### def get_navi_status(self): status, c_status = self.compoent_clients['Navigation'].component_status() -- GitLab From bdf1b904ac2fa9c5e58899eb62f8815e8251ab22 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 28 Nov 2019 13:01:39 +0900 Subject: [PATCH 08/30] Modify engines to send robot_positions from sub to main --- route_guidance_ros/scripts/main_engine.py | 6 ++++++ route_guidance_ros/scripts/sub_engine.py | 5 ++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 4d2e1dcb..6a3cccdc 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -197,6 +197,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): self.goal_command_table = {} 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.start() @@ -209,8 +210,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): goal.append(0) self.goal_command_table[str(goal)] = str(self.command_id) elif component_ref == 'RobotPosition': + self.command_id_result_table[str(self.command_id)] = [] for engine_client in self.engine_clients.values(): status, command_id = engine_client.set_parameter('System_Information') + time.sleep(3) + status, results = engine_client.get_command_result(command_id, "") + self.command_id_result_table[str(self.command_id)].append(results) + print("\tRobotPosition results: ", results) status = RoIS_HRI.ReturnCode_t.OK.value return (status, str(self.command_id)) diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index acc597db..b71fcc13 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -203,7 +203,10 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): elif component_ref == 'System_Information': status = RoIS_HRI.ReturnCode_t.OK.value result = target_component_client.robot_position() - self.command_id_status_table[str(self.command_id)] = results + result = (result[0].value, result[1], result[2], result[3]) # FIXME + self.command_id_status_table[str(self.command_id)] = result + print("End of System_Information") + print(status, result) return (status, str(self.command_id)) def get_command_result(self, command_id, condition): -- GitLab From 79231cff82f289a7af88bc7570e543aa4d6c61c5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 28 Nov 2019 13:34:41 +0900 Subject: [PATCH 09/30] Upgrade main-engine and service to get robot_position result --- route_guidance_ros/scripts/main_engine.py | 5 ++++- route_guidance_ros/scripts/sub_engine.py | 2 -- route_guidance_ros/scripts/test_main_engine_with_service.py | 3 +++ 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 6a3cccdc..702500a8 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -216,10 +216,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): time.sleep(3) status, results = engine_client.get_command_result(command_id, "") self.command_id_result_table[str(self.command_id)].append(results) - print("\tRobotPosition results: ", results) status = RoIS_HRI.ReturnCode_t.OK.value return (status, str(self.command_id)) + def get_command_result(self, command_id, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + return status, self.command_id_result_table[command_id] + def analyze_vnav_status(self): while True: msg = self.component_event_queue.get() diff --git a/route_guidance_ros/scripts/sub_engine.py b/route_guidance_ros/scripts/sub_engine.py index b71fcc13..e9215bf1 100644 --- a/route_guidance_ros/scripts/sub_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -205,8 +205,6 @@ 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 - print("End of System_Information") - print(status, result) return (status, str(self.command_id)) def get_command_result(self, command_id, condition): diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index 85d71809..a218e41a 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -40,6 +40,9 @@ class Service(Service_Application_IF): time.sleep(3) (return_code, command_id) = self._proxy.set_parameter('RobotPosition') self.commandid_task_table[command_id] = 'RobotPosition' + time.sleep(2) + status, results = self._proxy.get_command_result(command_id, "") + self.robot_positions = results def get_robot_positions(self): return self.robot_positions -- GitLab From 5fd11774216563c06a93b0e1f6a6a0c3bad3beff Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 28 Nov 2019 13:39:08 +0900 Subject: [PATCH 10/30] Modify system_info to specify robot_ref with robot_name --- route_guidance_ros/scripts/System_Information.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/route_guidance_ros/scripts/System_Information.py b/route_guidance_ros/scripts/System_Information.py index a2493cc1..b7370ad2 100755 --- a/route_guidance_ros/scripts/System_Information.py +++ b/route_guidance_ros/scripts/System_Information.py @@ -60,14 +60,15 @@ class Query(RoIS_Common.Query): class System_Information(Query): """System_Information """ - def __init__(self, c): + def __init__(self, c, robot_name): super(System_Information, self).__init__(c) + self._robot_name = robot_name self._component = c def robot_position(self): status = RoIS_HRI.ReturnCode_t.OK.value timestamp = str(datetime.now()) - robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::robot1"] # codebook, version + robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::{}".format(self._robot_name)] position_data = self._component.get_position() return (status, timestamp, robot_ref, position_data) @@ -98,7 +99,7 @@ def example_si(port, robot_name): """example_si """ c = component(robot_name) - si = System_Information(c) + si = System_Information(c, robot_name) # start the timer to dispatch events t = threading.Timer(0.1, event_dispatch, args=(si,)) -- GitLab From 8c25836a9e759223994074679eba3244f04c72e5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 1 Dec 2019 17:57:23 +0900 Subject: [PATCH 11/30] Add file for field point name --- route_guidance_ros/scripts/field_points.yaml | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 route_guidance_ros/scripts/field_points.yaml diff --git a/route_guidance_ros/scripts/field_points.yaml b/route_guidance_ros/scripts/field_points.yaml new file mode 100644 index 00000000..37087258 --- /dev/null +++ b/route_guidance_ros/scripts/field_points.yaml @@ -0,0 +1,10 @@ +point_a: [ 25, 20 ] +point_b: [ 25, 10 ] +point_c: [ 25, 0 ] +point_d: [ 15, 0 ] +point_e: [ 10, 0 ] +point_f: [ 5, 20 ] +point_g: [ 0, 0 ] +point_h: [ 0, 10 ] +point_i: [ 0, 20 ] +point_j: [ 0, 20 ] -- GitLab From 7e13ae45a2259dc57289959f5fd85b7af6e0356d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 1 Dec 2019 19:01:52 +0900 Subject: [PATCH 12/30] Modify component's set_parameter to use named-point --- route_guidance_ros/scripts/Navigation.py | 6 ++++-- route_guidance_ros/scripts/VirtualNavigation.py | 4 ++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/route_guidance_ros/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py index 593f1fc5..45312515 100755 --- a/route_guidance_ros/scripts/Navigation.py +++ b/route_guidance_ros/scripts/Navigation.py @@ -72,7 +72,7 @@ class Event(RoIS_Common.Event): from goal_sender_ros import GoalSenderROS import xmlrpclib - +import yaml from pyrois.RoIS_Common import Component_Status class Navigation(Event, Command, Query): @@ -84,11 +84,13 @@ class Navigation(Event, Command, Query): self._component.Time_Limit = 10 self._component.Routing_Policy = "" self.latest_nav_state = Component_Status.UNINITIALIZED + self.pointname_coord_table = yaml.load(open("field_points.yaml")) def set_parameter(self, target_position, time_limit, routing_policy): # Put event self.latest_nav_state = Component_Status.BUSY - + if isinstance(target_position, str) and target_position in self.pointname_coord_table: + target_position = self.pointname_coord_table[target_position] self._goal_sender.send_goal(target_position) status = RoIS_HRI.ReturnCode_t.OK.value return status diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 0abc14df..3ed752df 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -75,6 +75,7 @@ class Event(RoIS_Common.Event): from goal_sender_ros import GoalSenderROS from pyrois.RoIS_Common import Component_Status +import yaml import xmlrpclib @@ -88,9 +89,12 @@ class VirtualNavigation(Event, Command, Query): self.robot_goal_table = {} self.nav_state_table = \ {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names} + self.pointname_coord_table = yaml.load(open("field_points.yaml")) def set_parameter(self, target_position, time_limit, routing_policy): nearest_robot = self._component.get_nearest_robotname(target_position) + if isinstance(target_position, str) and target_position in self.pointname_coord_table: + target_position = self.pointname_coord_table[target_position] th = threading.Thread( target=self.send_goal_and_event, args=(target_position, nearest_robot)) -- GitLab From 09dfcd5212006d7ee34eebf3290747fd95f81f8a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 01:57:14 +0900 Subject: [PATCH 13/30] Use named-points for navigation service --- route_guidance_ros/scripts/VirtualNavigation.py | 14 ++++++++++---- route_guidance_ros/scripts/main_engine.py | 3 ++- .../scripts/test_main_engine_with_service.py | 10 +++++----- .../scripts/test_sub_engine_with_service.py | 2 +- 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 3ed752df..1609ccff 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -91,10 +91,15 @@ class VirtualNavigation(Event, Command, Query): {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): + if isinstance(position, str) and position in self.pointname_coord_table: + return self.pointname_coord_table[position] + else: + return position + def set_parameter(self, target_position, time_limit, routing_policy): - nearest_robot = self._component.get_nearest_robotname(target_position) - if isinstance(target_position, str) and target_position in self.pointname_coord_table: - target_position = self.pointname_coord_table[target_position] + target_coord = self.position_to_coord(target_position) + nearest_robot = self._component.get_nearest_robotname(target_coord) th = threading.Thread( target=self.send_goal_and_event, args=(target_position, nearest_robot)) @@ -104,7 +109,8 @@ class VirtualNavigation(Event, Command, Query): return status def send_goal_and_event(self, goal, robot_name): - status = self._component.send_goal_to_robot_async(goal, robot_name) + goal_coord = self.position_to_coord(goal) + status = self._component.send_goal_to_robot_async(goal_coord, robot_name) 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 702500a8..65814e1a 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -207,7 +207,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): target_component = self.component_clients[component_ref] status = target_component.set_parameter(*parameters).value goal = parameters[0] - goal.append(0) + if not isinstance(goal, str): + goal.append(0) self.goal_command_table[str(goal)] = str(self.command_id) elif component_ref == 'RobotPosition': self.command_id_result_table[str(self.command_id)] = [] diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index a218e41a..3abd6aa3 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -49,15 +49,15 @@ class Service(Service_Application_IF): def run(self): destinations = [ - [25, 10], [0, 20], [5, 0], [10, 0], [25, 20], [0, 5] + "point_b", "point_h", [5, 0], "point_e", "point_a", [0, 5] ] for dest in destinations: self.go_robot_to(dest) while self.is_running: - self.update_robot_positions() - robot_positions = self.get_robot_positions() - print("robot_positions:") - [print(f"\t{i}: {pos}") for i, pos in enumerate(robot_positions)] + # self.update_robot_positions() + # robot_positions = self.get_robot_positions() + # print("robot_positions:") + # [print(f"\t{i}: {pos}") for i, pos in enumerate(robot_positions)] time.sleep(3) 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 85d9de48..f437db9c 100644 --- a/route_guidance_ros/scripts/test_sub_engine_with_service.py +++ b/route_guidance_ros/scripts/test_sub_engine_with_service.py @@ -45,7 +45,7 @@ class Service(Service_Application_IF): print("----------------------------------------------") def run(self): - destinations = [[5, 0], [10, 0], [25, 0], [25, 20]] + destinations = ["point_e", "point_d", "point_c", "point_b"] for dest in destinations: self.go_robot_to(dest) while self.is_navigation_running: -- GitLab From 731dbeb05254651eb914dcac9bfa8f2ea7d0d5da Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 02:16:50 +0900 Subject: [PATCH 14/30] Change shape of coord 2D => 3D --- .../scripts/VirtualNavigation.py | 2 +- route_guidance_ros/scripts/field_points.yaml | 20 +++++++++---------- route_guidance_ros/scripts/goal_sender_ros.py | 2 -- route_guidance_ros/scripts/main_engine.py | 2 -- route_guidance_ros/scripts/test_app.py | 2 +- .../scripts/test_main_engine.py | 2 +- .../scripts/test_main_engine_with_service.py | 2 +- .../scripts/test_nav_component.py | 2 +- route_guidance_ros/scripts/test_sub_engine.py | 2 +- 9 files changed, 16 insertions(+), 20 deletions(-) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py index 1609ccff..3d7d651b 100755 --- a/route_guidance_ros/scripts/VirtualNavigation.py +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -140,7 +140,7 @@ class Component(object): def update_position(self, robot_name, amcl_pose): pos = amcl_pose.pose.pose.position - self.__robot_positions[robot_name] = np.array([pos.x, pos.y]) + self.__robot_positions[robot_name] = np.array([pos.x, pos.y, pos.z]) def send_goal_to_nearest_robot(self, dest): dest = np.array(dest) diff --git a/route_guidance_ros/scripts/field_points.yaml b/route_guidance_ros/scripts/field_points.yaml index 37087258..4f527735 100644 --- a/route_guidance_ros/scripts/field_points.yaml +++ b/route_guidance_ros/scripts/field_points.yaml @@ -1,10 +1,10 @@ -point_a: [ 25, 20 ] -point_b: [ 25, 10 ] -point_c: [ 25, 0 ] -point_d: [ 15, 0 ] -point_e: [ 10, 0 ] -point_f: [ 5, 20 ] -point_g: [ 0, 0 ] -point_h: [ 0, 10 ] -point_i: [ 0, 20 ] -point_j: [ 0, 20 ] +point_a: [ 25, 20, 0 ] +point_b: [ 25, 10, 0 ] +point_c: [ 25, 0, 0 ] +point_d: [ 15, 0, 0 ] +point_e: [ 10, 0, 0 ] +point_f: [ 5, 20, 0 ] +point_g: [ 0, 0, 0 ] +point_h: [ 0, 10, 0 ] +point_i: [ 0, 20, 0 ] +point_j: [ 0, 20, 0 ] diff --git a/route_guidance_ros/scripts/goal_sender_ros.py b/route_guidance_ros/scripts/goal_sender_ros.py index d7f6f355..13bec919 100644 --- a/route_guidance_ros/scripts/goal_sender_ros.py +++ b/route_guidance_ros/scripts/goal_sender_ros.py @@ -43,13 +43,11 @@ class GoalSenderROS(object): self.action_client.wait_for_server() def send_goal(self, pose_xy): - pose_xy.append(0) pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)] goal = pose_to_goal(pose) self.action_client.send_goal(goal) def send_goal_and_wait(self, pose_xy): - pose_xy.append(0) 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) diff --git a/route_guidance_ros/scripts/main_engine.py b/route_guidance_ros/scripts/main_engine.py index 65814e1a..63fbcee9 100644 --- a/route_guidance_ros/scripts/main_engine.py +++ b/route_guidance_ros/scripts/main_engine.py @@ -207,8 +207,6 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base): target_component = self.component_clients[component_ref] status = target_component.set_parameter(*parameters).value goal = parameters[0] - if not isinstance(goal, str): - goal.append(0) self.goal_command_table[str(goal)] = str(self.command_id) elif component_ref == 'RobotPosition': self.command_id_result_table[str(self.command_id)] = [] diff --git a/route_guidance_ros/scripts/test_app.py b/route_guidance_ros/scripts/test_app.py index e16f7de0..4e33277f 100644 --- a/route_guidance_ros/scripts/test_app.py +++ b/route_guidance_ros/scripts/test_app.py @@ -13,7 +13,7 @@ time.sleep(10) print("launching Service...") service = Service('http://localhost:8000') print("Service was established.") -for dest in [[25, 0], [0, 20], [5, 0]]: +for dest in [[25, 0, 0], [0, 20, 0], [5, 0, 0]]: status = service.go_robot_to(dest) print(status) time.sleep(5) diff --git a/route_guidance_ros/scripts/test_main_engine.py b/route_guidance_ros/scripts/test_main_engine.py index 53750282..d2919b85 100644 --- a/route_guidance_ros/scripts/test_main_engine.py +++ b/route_guidance_ros/scripts/test_main_engine.py @@ -13,7 +13,7 @@ engine = Engine('http://127.0.0.1:8000') print("Engine was established.") time.sleep(10) engine.connect() -for dest in [[25, 0], [0, 20], [5, 0]]: +for dest in [[25, 0, 0], [0, 20, 0], [5, 0, 0]]: status = engine.set_parameter('Navigation', dest, "", "") print(status) time.sleep(5) diff --git a/route_guidance_ros/scripts/test_main_engine_with_service.py b/route_guidance_ros/scripts/test_main_engine_with_service.py index 3abd6aa3..73e97a1c 100644 --- a/route_guidance_ros/scripts/test_main_engine_with_service.py +++ b/route_guidance_ros/scripts/test_main_engine_with_service.py @@ -49,7 +49,7 @@ class Service(Service_Application_IF): def run(self): destinations = [ - "point_b", "point_h", [5, 0], "point_e", "point_a", [0, 5] + "point_b", "point_h", [5, 0, 0], "point_e", "point_a", [0, 5, 0] ] for dest in destinations: self.go_robot_to(dest) diff --git a/route_guidance_ros/scripts/test_nav_component.py b/route_guidance_ros/scripts/test_nav_component.py index 8724364d..a01f387e 100644 --- a/route_guidance_ros/scripts/test_nav_component.py +++ b/route_guidance_ros/scripts/test_nav_component.py @@ -13,4 +13,4 @@ client = Navigation_Client(uri) print(client.start()) time.sleep(3) -print(client.set_parameter([10, 0], "", "")) +print(client.set_parameter([10, 0, 0], "", "")) diff --git a/route_guidance_ros/scripts/test_sub_engine.py b/route_guidance_ros/scripts/test_sub_engine.py index 668dd648..9b72c42a 100644 --- a/route_guidance_ros/scripts/test_sub_engine.py +++ b/route_guidance_ros/scripts/test_sub_engine.py @@ -12,7 +12,7 @@ process = setup_single_robot() engine = Engine('http://127.0.0.1:8010') engine.connect() engine.search("engine_profile_test") -destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] +destinations = [[5, 0, 0], [10, 0, 0], [25, 0, 0], [25, 10, 0]] for dest in destinations: print(engine.set_parameter('Navigation', dest, "", "")) navi_status = Component_Status.BUSY -- GitLab From fc0cca3bca02e302e8576d61680545670aebf1b5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 15:57:34 +0900 Subject: [PATCH 15/30] Add base of human urdf model --- rois_description/robot/human1.urdf.xacro | 185 +++++++++++++++++++++++ 1 file changed, 185 insertions(+) create mode 100644 rois_description/robot/human1.urdf.xacro diff --git a/rois_description/robot/human1.urdf.xacro b/rois_description/robot/human1.urdf.xacro new file mode 100644 index 00000000..b45fb452 --- /dev/null +++ b/rois_description/robot/human1.urdf.xacro @@ -0,0 +1,185 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + + + + robot1 + + + + + true + Gazebo/White + + + + + + true + Gazebo/Green + + + + + + true + Gazebo/Red + + + + + + true + Gazebo/Black + + + + + -- GitLab From 609e038f3343a98d745278c5942ff8d924531aed Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 16:02:51 +0900 Subject: [PATCH 16/30] Add controller file of human --- rois_gazebo/config/human1_controller.yaml | 46 +++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 rois_gazebo/config/human1_controller.yaml diff --git a/rois_gazebo/config/human1_controller.yaml b/rois_gazebo/config/human1_controller.yaml new file mode 100644 index 00000000..ddf31bf4 --- /dev/null +++ b/rois_gazebo/config/human1_controller.yaml @@ -0,0 +1,46 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +diff_drive_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel_joint' + right_wheel : 'right_wheel_joint' + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.3 + wheel_radius : 0.1 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 1.0 + + # Base frame_id + base_frame_id: human1/base_footprint #default: base_link + odom_frame_id: human1/odom + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 2.0 # m/s + min_velocity : -2.0 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2 + min_acceleration : -2.0 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + min_acceleration : -1.0 -- GitLab From 7ca178c59f6e26f3412d8a8d10a0d2ae4bdadcb0 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 16:11:33 +0900 Subject: [PATCH 17/30] Rename robot => model on model-spawn-launchers --- rois_gazebo/launch/setup_gazebo.launch | 12 ++++++------ .../{spawn_robot.launch => spawn_model.launch} | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) rename rois_gazebo/launch/{spawn_robot.launch => spawn_model.launch} (72%) diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index 95e9dfdb..99fb925a 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -15,9 +15,9 @@ value="$(arg world_name)" /> - + - + - + + default="$(find rois_description)/robot/$(arg model_name).urdf.xacro" /> - + - + - + + args="-urdf -model $(arg model_name) -param robot_description $(arg init_pos)" /> -- GitLab From 2aafaf5911044ced24f4cce13522390edae62b6b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 17:09:23 +0900 Subject: [PATCH 18/30] Add launch file to spawn human model --- rois_gazebo/launch/spawn_human.launch | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 rois_gazebo/launch/spawn_human.launch diff --git a/rois_gazebo/launch/spawn_human.launch b/rois_gazebo/launch/spawn_human.launch new file mode 100644 index 00000000..ab4dbdb7 --- /dev/null +++ b/rois_gazebo/launch/spawn_human.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + -- GitLab From 6ce0e0cb0c9d7f2e6dd8e4e01d828b30897b3e6c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 17:09:39 +0900 Subject: [PATCH 19/30] Fix params on human1.urdf.xacro --- rois_description/robot/human1.urdf.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rois_description/robot/human1.urdf.xacro b/rois_description/robot/human1.urdf.xacro index b45fb452..b10258d5 100644 --- a/rois_description/robot/human1.urdf.xacro +++ b/rois_description/robot/human1.urdf.xacro @@ -1,5 +1,5 @@ - + @@ -150,7 +150,7 @@ - robot1 + human1 -- GitLab From 3d39c87a354f1e6686ef55ebaeba9f4e5b2bc54f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 17:57:28 +0900 Subject: [PATCH 20/30] Change default speed of human model --- rois_gazebo/launch/spawn_human.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/rois_gazebo/launch/spawn_human.launch b/rois_gazebo/launch/spawn_human.launch index ab4dbdb7..4b6a51e9 100644 --- a/rois_gazebo/launch/spawn_human.launch +++ b/rois_gazebo/launch/spawn_human.launch @@ -7,5 +7,6 @@ + -- GitLab From 49caf86a03294abcad0eb0e89ac323e6d1a85ac9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 18:06:31 +0900 Subject: [PATCH 21/30] Change visual of human model --- rois_description/robot/human1.urdf.xacro | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rois_description/robot/human1.urdf.xacro b/rois_description/robot/human1.urdf.xacro index b10258d5..d367df1e 100644 --- a/rois_description/robot/human1.urdf.xacro +++ b/rois_description/robot/human1.urdf.xacro @@ -16,7 +16,7 @@ - + @@ -156,21 +156,21 @@ true - Gazebo/White + Gazebo/Red true - Gazebo/Green + Gazebo/White true - Gazebo/Red + Gazebo/White -- GitLab From a5d7c4725e2be00b21e997b6234566f820de5f7c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 2 Dec 2019 19:05:23 +0900 Subject: [PATCH 22/30] Use command-line argument for spawn_human launcher --- rois_gazebo/launch/spawn_human.launch | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rois_gazebo/launch/spawn_human.launch b/rois_gazebo/launch/spawn_human.launch index 4b6a51e9..98863fe8 100644 --- a/rois_gazebo/launch/spawn_human.launch +++ b/rois_gazebo/launch/spawn_human.launch @@ -1,7 +1,10 @@ + + + - - + + -- GitLab From c9b7af293b4326e003e1618566ecfbbbe061573b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 02:40:38 +0900 Subject: [PATCH 23/30] Remove dependency between service_app and sub-tester --- route_guidance_ros/scripts/test_sub_engine_with_service.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f437db9c..a0e9d53d 100644 --- a/route_guidance_ros/scripts/test_sub_engine_with_service.py +++ b/route_guidance_ros/scripts/test_sub_engine_with_service.py @@ -7,7 +7,7 @@ import time from pyrois.RoIS_Common import Component_Status from utilities import setup_single_robot from collections import namedtuple -from service_app import Service_Application_IF +from pyrois.Service_Application_IF import Service_Application_IF import xmlrpc -- GitLab From 0b9cd763777af75fb5d8f6ffb0d1905b87b30edb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 02:50:18 +0900 Subject: [PATCH 24/30] Remove unnecessary functions on service_app --- route_guidance_ros/scripts/service_app.py | 54 +---------------------- 1 file changed, 1 insertion(+), 53 deletions(-) diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app.py index fc0a6146..e5a6b86e 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app.py @@ -15,39 +15,8 @@ class Service_Application_IF(pyrois_Service_Application_IF): """Service_Application_IF """ def __init__(self, uri, logger=None): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) + super().__init__(uri) self.command_id_table = {} - if logger is not None: - self.logger = logger - self.th = threading.Thread(target=self.event_poll_loop) - self.th.start() - - def event_poll_loop(self): - print("poll loop started") - while True: - # try: - # self.poll_event() - self.poll_event() - # except: - # print("Error ") - # continue - - def poll_event(self): - """poll_event - """ - msg = self._proxy.poll_event() - if msg is None: - return - (params, methodname) = xmlrpc.client.loads(msg) - #self.logger.debug('poll_event: '+methodname) - print("params: ", params, ", methodname: ", methodname) - if methodname == 'completed': - self.completed(params[0], params[1]) - elif methodname == 'notify_error': - self.notify_error(params[0], params[1]) - elif methodname == 'notify_event': - self.notify_event(params[0], params[1], params[2], params[3]) def completed(self, command_id, status): self.command_id_table[command_id] = True @@ -72,24 +41,3 @@ class Service_Application_IF(pyrois_Service_Application_IF): stautus = RoIS_HRI.ReturnCode_t(return_code) self.command_id_table[command_id] = False print(self.command_id_table) - - -def example_sa_IF(url, q): - try: - logger = logging.getLogger('Service_Application_IF') - logger.setLevel(logging.DEBUG) - ch = logging.handlers.QueueHandler(q) - # ch = logging.StreamHandler() - ch.setLevel(logging.DEBUG) - formatter = logging.Formatter( - '%(asctime)s - %(name)s - %(levelname)s - %(message)s') - ch.setFormatter(formatter) - logger.addHandler(ch) - a = Service_Application_IF(url, logger=logger) - except KeyboardInterrupt: - print("Interrupted") - - -if __name__ == '__main__': - q = queue.Queue() - example_sa_IF("http://localhost:8000", q) -- GitLab From d9812ab462bf09255cc471199ea5bfcec51eec76 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 02:55:12 +0900 Subject: [PATCH 25/30] Rename service_app.py => service_app_lv1.py --- .../scripts/{service_app.py => service_app_lv1.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename route_guidance_ros/scripts/{service_app.py => service_app_lv1.py} (95%) diff --git a/route_guidance_ros/scripts/service_app.py b/route_guidance_ros/scripts/service_app_lv1.py similarity index 95% rename from route_guidance_ros/scripts/service_app.py rename to route_guidance_ros/scripts/service_app_lv1.py index e5a6b86e..786ed248 100644 --- a/route_guidance_ros/scripts/service_app.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -11,7 +11,7 @@ import socketserver import xmlrpc.server -class Service_Application_IF(pyrois_Service_Application_IF): +class Service(pyrois_Service_Application_IF): """Service_Application_IF """ def __init__(self, uri, logger=None): -- GitLab From 74350e62f0b93dd1a90a1afaea14c129d2a8b41c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 19:05:41 +0900 Subject: [PATCH 26/30] Make base of task manage system --- route_guidance_ros/scripts/service_app_lv1.py | 48 ++++++++++++++++--- 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index 786ed248..a00e0757 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -1,26 +1,48 @@ from main_engine_client import IF as RoIS_IF from pyrois.Service_Application_IF import Service_Application_IF as pyrois_Service_Application_IF from pyrois import RoIS_Service, RoIS_HRI +from utilities import setup_multi_robot import logging import queue import threading import time +import enum import socketserver import xmlrpc.server +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, is_finished=False): + self.task_kind = task_kind + self.is_finished = is_finished + + @staticmethod + def next(current_task): + return TaskState.__task_seq_table[current_task] + class Service(pyrois_Service_Application_IF): """Service_Application_IF """ - def __init__(self, uri, logger=None): + def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) - self.command_id_table = {} + self.commandid_taskstate_table = {} def completed(self, command_id, status): - self.command_id_table[command_id] = True - print(self.command_id_table) + self.commandid_taskstate_table[command_id] = True + print(self.commandid_taskstate_table) def notify_error(self, error_id, error_type): print('received error event {}({}) ', @@ -37,7 +59,19 @@ class Service(pyrois_Service_Application_IF): def go_robot_to(self, dest): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) - (return_code, command_id) = self._proxy.set_parameter('Navigation', [dest, "", ""]) + (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") stautus = RoIS_HRI.ReturnCode_t(return_code) - self.command_id_table[command_id] = False - print(self.command_id_table) + self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToUser) + print(self.commandid_taskstate_table) + + def run(self): + target_point = "point_c" + self.go_robot_to(target_point) + +if __name__ == '__main__': + process = setup_multi_robot() + time.sleep(5) + service = Service() + print("Starting service..") + service.run() + print("Finish.") -- GitLab From 66a81b52fae9828e80bddb7c6b97aa04309ec8b6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 20:58:16 +0900 Subject: [PATCH 27/30] Complete route-guidance level1 --- route_guidance_ros/scripts/service_app_lv1.py | 31 ++++++++++++++----- 1 file changed, 23 insertions(+), 8 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index a00e0757..41ef2b4b 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -28,21 +28,28 @@ class TaskState(object): self.task_kind = task_kind self.is_finished = is_finished - @staticmethod - def next(current_task): - return TaskState.__task_seq_table[current_task] + def __repr__(self): + return "task_kind: {}, is_finished: {}".format(self.task_kind, self.is_finished) + + def next(self): + return TaskState.__task_seq_table[self.task_kind] class Service(pyrois_Service_Application_IF): - """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): - self.commandid_taskstate_table[command_id] = True + task = self.commandid_taskstate_table[command_id] print(self.commandid_taskstate_table) + 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 !!") def notify_error(self, error_id, error_type): print('received error event {}({}) ', @@ -56,7 +63,7 @@ class Service(pyrois_Service_Application_IF): subscribe_id, expire) - def go_robot_to(self, dest): + def start_navi_to_user(self, dest): self._proxy = xmlrpc.client.ServerProxy(self._uri) time.sleep(3) (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "") @@ -64,9 +71,17 @@ class Service(pyrois_Service_Application_IF): self.commandid_taskstate_table[command_id] = TaskState(TaskKind.NaviToUser) print(self.commandid_taskstate_table) + def start_navi_to_guide(self, dest): + self._proxy = xmlrpc.client.ServerProxy(self._uri) + time.sleep(3) + (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) + print(self.commandid_taskstate_table) + def run(self): target_point = "point_c" - self.go_robot_to(target_point) + self.start_navi_to_user(target_point) if __name__ == '__main__': process = setup_multi_robot() -- GitLab From 31de1351987ad84fed2dd403df40270a9e642c02 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 21:30:32 +0900 Subject: [PATCH 28/30] Refactor service_app_lv1 --- route_guidance_ros/scripts/service_app_lv1.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index 41ef2b4b..b0050bf5 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -2,6 +2,7 @@ from main_engine_client import IF as RoIS_IF from pyrois.Service_Application_IF import Service_Application_IF as pyrois_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 @@ -29,7 +30,7 @@ class TaskState(object): self.is_finished = is_finished def __repr__(self): - return "task_kind: {}, is_finished: {}".format(self.task_kind, self.is_finished) + return "".format(self.task_kind, self.is_finished) def next(self): return TaskState.__task_seq_table[self.task_kind] @@ -44,8 +45,8 @@ class Service(pyrois_Service_Application_IF): def completed(self, command_id, status): task = self.commandid_taskstate_table[command_id] - print(self.commandid_taskstate_table) next_task = task.next() + print("status is ", status) if next_task == TaskKind.NaviToGuide: self.start_navi_to_guide(self.dest_pos) elif next_task == TaskKind.End: @@ -64,25 +65,24 @@ class Service(pyrois_Service_Application_IF): expire) def start_navi_to_user(self, dest): - self._proxy = xmlrpc.client.ServerProxy(self._uri) - time.sleep(3) (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) print(self.commandid_taskstate_table) def start_navi_to_guide(self, dest): - self._proxy = xmlrpc.client.ServerProxy(self._uri) - time.sleep(3) (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) print(self.commandid_taskstate_table) 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) -- GitLab From 80f89477c9eee159a79e319250b2f799d8776fb5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 5 Dec 2019 22:06:43 +0900 Subject: [PATCH 29/30] Use Component_Status to manage task --- route_guidance_ros/scripts/service_app_lv1.py | 28 +++++++++---------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index b0050bf5..a532d8ea 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -2,7 +2,7 @@ from main_engine_client import IF as RoIS_IF from pyrois.Service_Application_IF import Service_Application_IF as pyrois_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_Common import Component_Status import logging import queue @@ -18,6 +18,7 @@ class TaskKind(enum.Enum): NaviToGuide = 2 End = 3 + class TaskState(object): __task_seq_table = { TaskKind.NaviToUser: TaskKind.NaviToGuide, @@ -25,12 +26,12 @@ class TaskState(object): TaskKind.End: TaskKind.End } - def __init__(self, task_kind, is_finished=False): + def __init__(self, task_kind, status=Component_Status.BUSY): self.task_kind = task_kind - self.is_finished = is_finished + self.status = status def __repr__(self): - return "".format(self.task_kind, self.is_finished) + return "".format(self.task_kind, self.status) def next(self): return TaskState.__task_seq_table[self.task_kind] @@ -45,36 +46,33 @@ class Service(pyrois_Service_Application_IF): def completed(self, command_id, status): task = self.commandid_taskstate_table[command_id] + status = Component_Status(status) + task.status = status next_task = task.next() - print("status is ", status) 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 {}({}) ', - error_id, - RoIS_Service.ErrorType(error_type).name) + 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 {} {} {} {}', - 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) - print(self.commandid_taskstate_table) 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) - print(self.commandid_taskstate_table) def run(self): self._proxy = xmlrpc.client.ServerProxy(self._uri) -- GitLab From 9e5301c00ffe10aca695f451281ac0ee2048d14e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 6 Dec 2019 15:38:08 +0900 Subject: [PATCH 30/30] Refactor importing mudules on service_app_lv1 --- route_guidance_ros/scripts/service_app_lv1.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/route_guidance_ros/scripts/service_app_lv1.py b/route_guidance_ros/scripts/service_app_lv1.py index a532d8ea..5a220f81 100644 --- a/route_guidance_ros/scripts/service_app_lv1.py +++ b/route_guidance_ros/scripts/service_app_lv1.py @@ -1,5 +1,4 @@ -from main_engine_client import IF as RoIS_IF -from pyrois.Service_Application_IF import Service_Application_IF as pyrois_Service_Application_IF +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 @@ -9,9 +8,8 @@ import queue import threading import time import enum +import xmlrpc -import socketserver -import xmlrpc.server class TaskKind(enum.Enum): NaviToUser = 1 @@ -37,7 +35,7 @@ class TaskState(object): return TaskState.__task_seq_table[self.task_kind] -class Service(pyrois_Service_Application_IF): +class Service(Service_Application_IF): def __init__(self, uri="http://localhost:8000", logger=None): super().__init__(uri) self.target_pos = "point_c" -- GitLab