From 6371874d8cd37c0d864319ac1e830d20c904c46d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 31 Oct 2019 13:41:02 +0900 Subject: [PATCH 01/13] Upgade GoalSender to subscribe amcl_pose --- goal_sender/scripts/goal_sender_ros.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index 3b69730d..8b5a36bd 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -4,7 +4,7 @@ import rospy import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped from actionlib_msgs.msg import GoalStatus as g_state from pyrois.RoIS_Common import Component_Status as c_state @@ -40,6 +40,7 @@ class GoalSenderROS(object): def __init__(self, robot_name): self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction) + rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) self.action_client.wait_for_server() def send_goal(self, pose_xy): @@ -52,5 +53,8 @@ class GoalSenderROS(object): state = self.action_client.get_state() return GoalSenderROS.GoalSenderState[state] + def amcl_pose_callback(self, amcl_pose): + print(amcl_pose.pose.pose.position) + # If you remove this comment out, it does not work. # rospy.init_node('goal_sender') -- GitLab From e2cffb4deccc90fbc3a706259ad2e3c2c64478f7 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 00:35:41 +0900 Subject: [PATCH 02/13] Add function to get robot's current position --- goal_sender/scripts/goal_sender_ros.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index 8b5a36bd..801530ab 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -40,6 +40,8 @@ class GoalSenderROS(object): def __init__(self, robot_name): self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction) + self.position = None + rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) self.action_client.wait_for_server() @@ -54,7 +56,11 @@ class GoalSenderROS(object): return GoalSenderROS.GoalSenderState[state] def amcl_pose_callback(self, amcl_pose): - print(amcl_pose.pose.pose.position) + pos = amcl_pose.pose.pose.position + self.position = [pos.x, pos.y] + + def get_current_position(self): + return self.position # If you remove this comment out, it does not work. # rospy.init_node('goal_sender') -- GitLab From 5903eaec3ebd3e765d26e827f1cc0e1964166315 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 01:32:29 +0900 Subject: [PATCH 03/13] Add System_Information compoent files --- goal_sender/scripts/System_Information.py | 113 ++++++++++++++++++ .../scripts/System_Information_client.py | 41 +++++++ 2 files changed, 154 insertions(+) create mode 100755 goal_sender/scripts/System_Information.py create mode 100644 goal_sender/scripts/System_Information_client.py diff --git a/goal_sender/scripts/System_Information.py b/goal_sender/scripts/System_Information.py new file mode 100755 index 00000000..e3676c82 --- /dev/null +++ b/goal_sender/scripts/System_Information.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python + +# System_Information.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +"""System_Information +""" + +import sys +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import Queue as queue + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import queue + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, c): + super(Query, self).__init__() + self._component = c + + def component_status(self): + pass + + def robot_position(self): + pass + + def engine_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + e_status = RoIS_Common.Component_Status.READY.value + operable_time = ["2100-01-01T00:00:01+09:00","2100-01-01T00:00:01+09:00"] + return (status, e_status, operable_time) + + +class System_Information(Query): + """System_Information + """ + def __init__(self, c): + super(System_Information, self).__init__(c) + 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 + position_data = ["0", "0"] + return(status, timestamp, robot_ref, position_data) + + +import rospy + +class component: + """component + """ + def __init__(self, robot_name): + self._state = False + + +def event_dispatch(pd): + """event_dispatch + """ + +def example_si(port, robot_name): + """example_si + """ + c = component(robot_name) + si = System_Information(c) + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(si,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(si) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + if len(sys.argv) < 3: + print("rosrun System_Information.py ") + raise RuntimeError + + port, robot_name = int(sys.argv[1]), sys.argv[2] + rospy.init_node('sysinfo_component', anonymous=True) + example_si(port, robot_name) diff --git a/goal_sender/scripts/System_Information_client.py b/goal_sender/scripts/System_Information_client.py new file mode 100644 index 00000000..7285134d --- /dev/null +++ b/goal_sender/scripts/System_Information_client.py @@ -0,0 +1,41 @@ +# System_Information_Client.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Engine + +"""System_Information_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def robot_position(self): + (status, timestamp, robot_ref, position_data) = self._proxy.component_status() + return(RoIS_HRI.ReturnCode_t(status), timestamp, robot_ref, position_data) + + def engine_status(self): + (status, e_status, operable_time) = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(e_status), operable_time) + + +class System_Information_Client(Query): + """System_Information_Client + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) \ No newline at end of file -- GitLab From 9c0fc3a24fd0260680a5a05ec2b7719168225e7a Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 02:11:53 +0900 Subject: [PATCH 04/13] Upgrade SysInfor component with amcl_pose subscriber --- goal_sender/scripts/System_Information.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/System_Information.py b/goal_sender/scripts/System_Information.py index e3676c82..908384b1 100755 --- a/goal_sender/scripts/System_Information.py +++ b/goal_sender/scripts/System_Information.py @@ -68,18 +68,27 @@ class System_Information(Query): status = RoIS_HRI.ReturnCode_t.OK.value timestamp = str(datetime.now()) robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::robot1"] # codebook, version - position_data = ["0", "0"] + position_data = self._component.get_position() return(status, timestamp, robot_ref, position_data) import rospy +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped class component: """component """ def __init__(self, robot_name): self._state = False + self.position = None + rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) + def amcl_pose_callback(self, amcl_pose): + pos = amcl_pose.pose.pose.position + self.position = [pos.x, pos.y] + + def get_position(self): + return self.position def event_dispatch(pd): """event_dispatch -- GitLab From aaaaf80cab69e12d6bfb3c91de07c476e03aae3d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 02:13:18 +0900 Subject: [PATCH 05/13] Remove amcl_pose subscriber from goal_sender --- goal_sender/scripts/goal_sender_ros.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index 801530ab..3b69730d 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -4,7 +4,7 @@ import rospy import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped +from geometry_msgs.msg import PoseStamped from actionlib_msgs.msg import GoalStatus as g_state from pyrois.RoIS_Common import Component_Status as c_state @@ -40,9 +40,6 @@ class GoalSenderROS(object): def __init__(self, robot_name): self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction) - self.position = None - - rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) self.action_client.wait_for_server() def send_goal(self, pose_xy): @@ -55,12 +52,5 @@ class GoalSenderROS(object): state = self.action_client.get_state() return GoalSenderROS.GoalSenderState[state] - def amcl_pose_callback(self, amcl_pose): - pos = amcl_pose.pose.pose.position - self.position = [pos.x, pos.y] - - def get_current_position(self): - return self.position - # If you remove this comment out, it does not work. # rospy.init_node('goal_sender') -- GitLab From 2b69c1ac32ff872ca6a3b2f9aa6eb790a857bae6 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 02:32:30 +0900 Subject: [PATCH 06/13] Include System_Information client into sub_engine --- .../scripts/sub_route_guidance_engine.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/goal_sender/scripts/sub_route_guidance_engine.py b/goal_sender/scripts/sub_route_guidance_engine.py index 53995591..e0533a84 100644 --- a/goal_sender/scripts/sub_route_guidance_engine.py +++ b/goal_sender/scripts/sub_route_guidance_engine.py @@ -167,15 +167,17 @@ class EventIF(RoIS_HRI.EventIF): from Navigation_client import Navigation_Client as NavClient +from System_Information_client import System_Information_Client as SysClient class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): """IF """ - def __init__(self, Engine, nav_port): + def __init__(self, Engine, engine_port): super().__init__(Engine) self.command_id = 0 self.compoent_clients = { - 'Navigation': NavClient('http://localhost:' + nav_port) + 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), + 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) } def set_parameter(self, component_ref, parameters): @@ -218,16 +220,16 @@ class RouteGuidanceEngine: self.state = False -def test_engine(port, nav_port): +def test_engine(port): """test_engine """ - IF_server(port).run(IF(RouteGuidanceEngine(), nav_port)) + IF_server(port).run(IF(RouteGuidanceEngine(), port)) if __name__ == "__main__": - if len(sys.argv) < 3: - print("py sub_route_guidance_engine.py ") + if len(sys.argv) < 2: + print("py sub_route_guidance_engine.py ") raise RuntimeError - port, nav_port = int(sys.argv[1]), sys.argv[2] - test_engine(port, nav_port) + port = int(sys.argv[1]) + test_engine(port) -- GitLab From d2916579084000cabbfd293fef68cbd540c3b3eb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 13:19:34 +0900 Subject: [PATCH 07/13] Add test for System_Information component --- .../{test_component.py => test_nav_component.py} | 4 ++-- goal_sender/scripts/test_sys_component.py | 13 +++++++++++++ 2 files changed, 15 insertions(+), 2 deletions(-) rename goal_sender/scripts/{test_component.py => test_nav_component.py} (70%) create mode 100644 goal_sender/scripts/test_sys_component.py diff --git a/goal_sender/scripts/test_component.py b/goal_sender/scripts/test_nav_component.py similarity index 70% rename from goal_sender/scripts/test_component.py rename to goal_sender/scripts/test_nav_component.py index 25e4a77e..66fb8949 100644 --- a/goal_sender/scripts/test_component.py +++ b/goal_sender/scripts/test_nav_component.py @@ -1,11 +1,11 @@ # 1. launch navigation. -# 2. execute "rosrun (this_package) Navigation.py" +# 2. execute "rosrun (this_package) Navigation.py 8011 robot1" # 3. run this test import time from Navigation_client import Navigation_Client -uri = "http://localhost:8001" +uri = "http://localhost:8011" client = Navigation_Client(uri) print(client.start()) diff --git a/goal_sender/scripts/test_sys_component.py b/goal_sender/scripts/test_sys_component.py new file mode 100644 index 00000000..223bdb63 --- /dev/null +++ b/goal_sender/scripts/test_sys_component.py @@ -0,0 +1,13 @@ +# 1. launch navigation. +# 2. execute "rosrun (this_package) System_Information.py 8012 robot1" +# 3. run this test + +import time +from System_Information_client import System_Information_Client + +uri = "http://localhost:8012" +client = System_Information_Client(uri) + +while True: + time.sleep(1) + print(client.robot_position()) -- GitLab From 9f996e020f24e4c4d41be7533c41706509cf4cb3 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 13:42:45 +0900 Subject: [PATCH 08/13] Fix bug on System_Information_client --- goal_sender/scripts/System_Information.py | 2 +- goal_sender/scripts/System_Information_client.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/goal_sender/scripts/System_Information.py b/goal_sender/scripts/System_Information.py index 908384b1..a2493cc1 100755 --- a/goal_sender/scripts/System_Information.py +++ b/goal_sender/scripts/System_Information.py @@ -69,7 +69,7 @@ class System_Information(Query): timestamp = str(datetime.now()) robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::robot1"] # codebook, version position_data = self._component.get_position() - return(status, timestamp, robot_ref, position_data) + return (status, timestamp, robot_ref, position_data) import rospy diff --git a/goal_sender/scripts/System_Information_client.py b/goal_sender/scripts/System_Information_client.py index 7285134d..d44ddc10 100644 --- a/goal_sender/scripts/System_Information_client.py +++ b/goal_sender/scripts/System_Information_client.py @@ -25,8 +25,8 @@ class Query(RoIS_Common.Query): self._proxy = xmlrpc.client.ServerProxy(self._uri) def robot_position(self): - (status, timestamp, robot_ref, position_data) = self._proxy.component_status() - return(RoIS_HRI.ReturnCode_t(status), timestamp, robot_ref, position_data) + (status, timestamp, robot_ref, position_data) = self._proxy.robot_position() + return (RoIS_HRI.ReturnCode_t(status), timestamp, robot_ref, position_data) def engine_status(self): (status, e_status, operable_time) = self._proxy.component_status() @@ -38,4 +38,4 @@ class System_Information_Client(Query): """ def __init__(self, uri): self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) \ No newline at end of file + self._proxy = xmlrpc.client.ServerProxy(self._uri) -- GitLab From 8864752a2f3ed3b4e55ad4260c4b0499f2711ad8 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 1 Nov 2019 18:16:25 +0900 Subject: [PATCH 09/13] Inherit Service_Application_IF for HRI engine client --- goal_sender/scripts/sub_route_guidance_engine_client.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/sub_route_guidance_engine_client.py b/goal_sender/scripts/sub_route_guidance_engine_client.py index 6abe2d97..6e9267ff 100644 --- a/goal_sender/scripts/sub_route_guidance_engine_client.py +++ b/goal_sender/scripts/sub_route_guidance_engine_client.py @@ -14,6 +14,7 @@ import xmlrpc.client # from pyrois import Service_Application_Base_example from pyrois import RoIS_HRI, RoIS_Common, RoIS_Service +from pyrois.Service_Application_IF import Service_Application_IF # import Service_Application_IF_test class SystemIF(RoIS_HRI.SystemIF): @@ -120,7 +121,7 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) -class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): +class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF): """IF """ def __init__(self, uri): -- GitLab From d693fbdeaab8602c5649cf01fde336639185c021 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Sun, 3 Nov 2019 15:10:43 +0900 Subject: [PATCH 10/13] Refactor engine test --- goal_sender/scripts/test_sub_engine.py | 20 ++++++++------------ goal_sender/scripts/test_toplevel_engine.py | 11 +++++------ 2 files changed, 13 insertions(+), 18 deletions(-) diff --git a/goal_sender/scripts/test_sub_engine.py b/goal_sender/scripts/test_sub_engine.py index bce26dbc..a884cfbc 100644 --- a/goal_sender/scripts/test_sub_engine.py +++ b/goal_sender/scripts/test_sub_engine.py @@ -4,27 +4,23 @@ # 4. run this test. -from sub_route_guidance_engine_client import IF +from sub_route_guidance_engine_client import Engine import time from pyrois.RoIS_Common import Component_Status -s = 'http://127.0.0.1:8010' -i = IF(s) -i.connect() -i.search("engine_profile_test") -print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis')) -#i.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis') +engine = Engine('http://127.0.0.1:8010') +engine.connect() +engine.search("engine_profile_test") destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] for dest in destinations: - print(i.set_parameter('Navigation',[dest, "", ""])) + print(engine.set_parameter('Navigation',[dest, "", ""])) navi_status = Component_Status.BUSY - print("TEST: status = {}".format(navi_status)) while navi_status != Component_Status.READY: - server_status, navi_status = i.get_navi_status() + server_status, navi_status = engine.get_navi_status() print("TEST: status = {}".format(navi_status)) time.sleep(1) time.sleep(3) -#print(i.analysis_c_status()) -i.disconnect() +#print(engine.analysis_c_status()) +engine.disconnect() print("finish") diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index 407a75d2..3d7291ac 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -5,16 +5,15 @@ # 5. run this test. -from toplevel_route_guidance_engine_client import IF +from toplevel_route_guidance_engine_client import Engine import time from pyrois.RoIS_Common import Component_Status -s = 'http://127.0.0.1:8000' -i = IF(s) -i.connect() +engine = IF('http://127.0.0.1:8000') +engine.connect() time.sleep(3) -status = i.set_parameter('Navigation',[[25, 20], "", ""]) +status = engine.set_parameter('Navigation',[[25, 20], "", ""]) print(status) -i.disconnect() +engine.disconnect() print("finish") -- GitLab From dca06fffe81026377ad8afd313ae009f15efcbec Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 4 Nov 2019 22:01:44 +0900 Subject: [PATCH 11/13] Add function to get position on sub-engine --- goal_sender/scripts/sub_route_guidance_engine.py | 5 +++++ goal_sender/scripts/sub_route_guidance_engine_client.py | 5 +++++ goal_sender/scripts/test_sub_engine.py | 4 +++- 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/sub_route_guidance_engine.py b/goal_sender/scripts/sub_route_guidance_engine.py index e0533a84..33cf9ca9 100644 --- a/goal_sender/scripts/sub_route_guidance_engine.py +++ b/goal_sender/scripts/sub_route_guidance_engine.py @@ -194,6 +194,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application def get_navi_status(self): status, c_status = self.compoent_clients['Navigation'].component_status() return (status.value, c_status.value) + + def get_position(self): + si_compoent = self.compoent_clients['System_Information'] + status, timestamp, robot_ref, position_data = si_compoent.robot_position() + return (status.value, timestamp, robot_ref, position_data) ############## class IF_server: diff --git a/goal_sender/scripts/sub_route_guidance_engine_client.py b/goal_sender/scripts/sub_route_guidance_engine_client.py index 6e9267ff..0fc6e7ec 100644 --- a/goal_sender/scripts/sub_route_guidance_engine_client.py +++ b/goal_sender/scripts/sub_route_guidance_engine_client.py @@ -139,4 +139,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF): def get_navi_status(self): status, c_status = self._proxy.get_navi_status() return RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status) + + def get_position(self): + position = self._proxy.get_position() + position[0] = RoIS_HRI.ReturnCode_t(position[0]) + return position ############## diff --git a/goal_sender/scripts/test_sub_engine.py b/goal_sender/scripts/test_sub_engine.py index a884cfbc..7932218e 100644 --- a/goal_sender/scripts/test_sub_engine.py +++ b/goal_sender/scripts/test_sub_engine.py @@ -4,7 +4,7 @@ # 4. run this test. -from sub_route_guidance_engine_client import Engine +from sub_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status @@ -19,6 +19,8 @@ for dest in destinations: server_status, navi_status = engine.get_navi_status() print("TEST: status = {}".format(navi_status)) time.sleep(1) + position = engine.get_position() + print("TEST: position = {}".format(position)) time.sleep(3) #print(engine.analysis_c_status()) -- GitLab From 102471e29eeea5787a8fbe43dde26282fcb30352 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 00:46:28 +0900 Subject: [PATCH 12/13] Upgrade top-level engine and test for efficient navi --- goal_sender/scripts/test_toplevel_engine.py | 11 ++++++----- .../scripts/toplevel_route_guidance_engine.py | 15 +++++++++++++-- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index 3d7291ac..f9589007 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -5,15 +5,16 @@ # 5. run this test. -from toplevel_route_guidance_engine_client import Engine +from toplevel_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status -engine = IF('http://127.0.0.1:8000') +engine = Engine('http://127.0.0.1:8000') engine.connect() -time.sleep(3) -status = engine.set_parameter('Navigation',[[25, 20], "", ""]) -print(status) +for dest in [[25, 0], [0, 20], [5, 0]]: + status = engine.set_parameter('Navigation', [dest, "", ""]) + print(status) + time.sleep(5) engine.disconnect() print("finish") diff --git a/goal_sender/scripts/toplevel_route_guidance_engine.py b/goal_sender/scripts/toplevel_route_guidance_engine.py index 95e4aa98..aa5aa1e7 100644 --- a/goal_sender/scripts/toplevel_route_guidance_engine.py +++ b/goal_sender/scripts/toplevel_route_guidance_engine.py @@ -14,6 +14,7 @@ from __future__ import print_function import os import sys +import numpy as np from pyrois import RoIS_HRI @@ -181,8 +182,18 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): engine_client.connect() def set_parameter(self, command_ref, parameters): - for engine_client in self.engine_clients.values(): - engine_client.set_parameter(command_ref, parameters) + dest = np.array(parameters[0]) + robot_distance_table = {} + for name, engine in self.engine_clients.items(): + pos = engine.get_position()[3] + print("OK1") + pos = np.array(pos) + robot_distance_table[name] = np.linalg.norm(dest - pos) + nearest_robot = min(robot_distance_table, key=robot_distance_table.get) + print("Nearest robot is '{}'".format(nearest_robot)) + + target_engine = self.engine_clients[nearest_robot] + target_engine.set_parameter(command_ref, parameters) status = RoIS_HRI.ReturnCode_t.OK.value self.command_id += 1 return (status, str(self.command_id)) -- GitLab From d22fca035b261658389a441d93cc3a3648132e14 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 5 Nov 2019 01:50:55 +0900 Subject: [PATCH 13/13] Upgrade test script to avoid to create many windows --- goal_sender/scripts/test_toplevel_engine.py | 20 ++++++++++++++++++++ goal_sender/scripts/utilities.py | 19 +++++++++++++++++++ 2 files changed, 39 insertions(+) create mode 100644 goal_sender/scripts/utilities.py diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index f9589007..a963bcf9 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -8,6 +8,26 @@ from toplevel_route_guidance_engine_client import IF as Engine import time from pyrois.RoIS_Common import Component_Status +from utilities import NavigationComponent, SysInfoComponent +from subprocess import Popen + +robot_port_table = {'robot1': 8010, 'robot2': 8020, 'robot3': 8030} +components = [] +for robot_name, engine_port in robot_port_table.items(): + components.append(NavigationComponent(robot_name, engine_port+1)) + components.append(SysInfoComponent(robot_name, engine_port+2)) +print("Components Constructed.") +time.sleep(5) + +engines = [] +for engine_port in robot_port_table.values(): + engines.append(Popen(["python3", "sub_route_guidance_engine.py", str(engine_port)])) +print("Sub Engine Constructed.") +time.sleep(5) + +toplevel_engine_p = Popen(["python3", "toplevel_route_guidance_engine.py"]) +print("Toplevel Engine Constructed.") +time.sleep(5) engine = Engine('http://127.0.0.1:8000') engine.connect() diff --git a/goal_sender/scripts/utilities.py b/goal_sender/scripts/utilities.py new file mode 100644 index 00000000..3bcaf8ff --- /dev/null +++ b/goal_sender/scripts/utilities.py @@ -0,0 +1,19 @@ +from subprocess import Popen + +class ComponentWrapper(object): + def __init__(self, component_name, robot_name, port): + command = ["rosrun", "goal_sender", component_name, str(port), robot_name] + self.process = Popen(command) + + def __del__(self): + self.process.kill() + + +class NavigationComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("Navigation.py", robot_name, port) + + +class SysInfoComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("System_Information.py", robot_name, port) -- GitLab