diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py index 879b2bdf8c7a11ec795d972c0b18518041934406..fb38f75cab2922e85ac0d5314353937ab202bb93 100755 --- a/goal_sender/scripts/Navigation.py +++ b/goal_sender/scripts/Navigation.py @@ -76,10 +76,10 @@ class Event(RoIS_Common.Event): from goal_sender_ros import GoalSenderROS class Navigation(Event, Command, Query): - def __init__(self, c): + def __init__(self, c, robot_name): super(Navigation, self).__init__(c) self._component = c - self._goal_sender = GoalSenderROS() + self._goal_sender = GoalSenderROS(robot_name) self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" @@ -106,10 +106,10 @@ class Component(object): import rospy -def example_n(port): +def example_n(port, robot_name): print("Starting node...") component = Component() - n = Navigation(component) + n = Navigation(component, robot_name) print("Navigation constructed") # start the timer to dispatch events @@ -125,5 +125,10 @@ def example_n(port): if __name__ == '__main__': - rospy.init_node('navigation_component') - example_n(8001) + if len(sys.argv) < 3: + print("rosrun Navigation.py ") + raise RuntimeError + + port, robot_name = int(sys.argv[1]), sys.argv[2] + rospy.init_node('navigation_component', anonymous=True) + example_n(port=port, robot_name=robot_name) diff --git a/goal_sender/scripts/System_Information.py b/goal_sender/scripts/System_Information.py new file mode 100755 index 0000000000000000000000000000000000000000..a2493cc13731ed606f8a1355b792b24ee4a2bf68 --- /dev/null +++ b/goal_sender/scripts/System_Information.py @@ -0,0 +1,122 @@ +#!/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 = 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 + """ + +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 0000000000000000000000000000000000000000..d44ddc1028366997ef9ce47ae55562d875fe757d --- /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.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() + 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) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index c6058e48db97d7733ca4d534cd5fa807cad86d65..3b69730de70dcd80a734b54481cd6971c7dbb20c 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -38,8 +38,8 @@ class GoalSenderROS(object): g_state.LOST: c_state.ERROR } - def __init__(self): - self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) + def __init__(self, robot_name): + self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction) self.action_client.wait_for_server() def send_goal(self, pose_xy): diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/sub_route_guidance_engine.py similarity index 87% rename from goal_sender/scripts/route_guidance_engine.py rename to goal_sender/scripts/sub_route_guidance_engine.py index ab821d0a8125d4213eeee5dd753b8e0d2c35dc7b..33cf9ca91b0c36374cbc69d9e29d617349f252e5 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/sub_route_guidance_engine.py @@ -1,4 +1,4 @@ -# route_guidance_engine.py +# sub_route_guidance_engine.py # # Copyright 2017 Eiichi Inohira # This software may be modified and distributed under the terms @@ -7,7 +7,7 @@ # For python 3 # For HRI Engine -"""route_guidance_engine +"""sub_route_guidance_engine """ from __future__ import print_function @@ -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): + def __init__(self, Engine, engine_port): super().__init__(Engine) self.command_id = 0 self.compoent_clients = { - 'Navigation': NavClient('http://localhost:8001') + 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), + 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) } def set_parameter(self, component_ref, parameters): @@ -192,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: @@ -221,8 +228,13 @@ class RouteGuidanceEngine: def test_engine(port): """test_engine """ - IF_server(port).run(IF(RouteGuidanceEngine())) + IF_server(port).run(IF(RouteGuidanceEngine(), port)) if __name__ == "__main__": - test_engine(8010) + if len(sys.argv) < 2: + print("py sub_route_guidance_engine.py ") + raise RuntimeError + + port = int(sys.argv[1]) + test_engine(port) diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/sub_route_guidance_engine_client.py similarity index 93% rename from goal_sender/scripts/route_guidance_engine_client.py rename to goal_sender/scripts/sub_route_guidance_engine_client.py index 6abe2d974ee306c75e95e5758a6bb5afe95077d9..0fc6e7ece7b008f0f58d450135182e38db52cb4a 100644 --- a/goal_sender/scripts/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): @@ -138,4 +139,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application 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_engine.py b/goal_sender/scripts/test_engine.py deleted file mode 100644 index a0e913f9237fb323e48609cb1d092a051bbbca01..0000000000000000000000000000000000000000 --- a/goal_sender/scripts/test_engine.py +++ /dev/null @@ -1,30 +0,0 @@ -# 1. launch navigation. -# 2. execute "rosrun (this_package) Navigation.py". -# 3. run route_guidance_engine server. -# 4. run this test. - - -from route_guidance_engine_client import IF -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') -destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] -for dest in destinations: - print(i.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() - print("TEST: status = {}".format(navi_status)) - time.sleep(1) - time.sleep(3) - -#print(i.analysis_c_status()) -i.disconnect() -print("finish") 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 25e4a77eb7901f1c7622631ba0595213d9536534..66fb89490e85a5737c01767d096eb10ade3bf67e 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_sub_engine.py b/goal_sender/scripts/test_sub_engine.py new file mode 100644 index 0000000000000000000000000000000000000000..7932218ebc8a156795908d22bcf49d26162da2e3 --- /dev/null +++ b/goal_sender/scripts/test_sub_engine.py @@ -0,0 +1,28 @@ +# 1. launch navigation. +# 2. execute "rosrun (this_package) Navigation.py 8001 robot1". +# 3. run sub_route_guidance_engine server with server and component port. +# 4. run this test. + + +from sub_route_guidance_engine_client import IF as Engine +import time +from pyrois.RoIS_Common import Component_Status + +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(engine.set_parameter('Navigation',[dest, "", ""])) + navi_status = Component_Status.BUSY + while navi_status != Component_Status.READY: + 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()) +engine.disconnect() +print("finish") diff --git a/goal_sender/scripts/test_sys_component.py b/goal_sender/scripts/test_sys_component.py new file mode 100644 index 0000000000000000000000000000000000000000..223bdb633dab74a204447e5f1bff6dcaa272b828 --- /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()) diff --git a/goal_sender/scripts/test_toplevel_engine.py b/goal_sender/scripts/test_toplevel_engine.py index e57fb1bab5c153b348231a9c5f39998857118ed2..a963bcf9a3da78db79870c4635fb0b36eca21edd 100644 --- a/goal_sender/scripts/test_toplevel_engine.py +++ b/goal_sender/scripts/test_toplevel_engine.py @@ -1,20 +1,40 @@ # 1. launch navigation. # 2. execute "rosrun (this_package) Navigation.py". -# 3. run route_guidance_engine server. +# 3. run sub_route_guidance_engine server. # 4. run toplevel_route_guidance_engine server. # 5. run this test. -from toplevel_route_guidance_engine_client import IF +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 -s = 'http://127.0.0.1:8000' -i = IF(s) -i.connect() -time.sleep(3) -status = i.set_parameter('Navigation',[[25, 20], "", ""]) -print(status) +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) -i.disconnect() +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() +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 db0043b316b4a8e1fe419c13ae2876a300bf59ef..aa5aa1e737f49f78f0f95ac7aea84a394bbfbe55 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 @@ -164,7 +165,7 @@ class EventIF(RoIS_HRI.EventIF): results = ["None"] return (status, results) -from route_guidance_engine_client import IF as EngineClient +from sub_route_guidance_engine_client import IF as EngineClient class IF(SystemIF, CommandIF, QueryIF, EventIF): """IF @@ -173,14 +174,26 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): super().__init__(Engine) self.command_id = 0 self.engine_clients = { - 'robot1': EngineClient('http://localhost:8010') + 'robot1': EngineClient('http://localhost:8010'), + 'robot2': EngineClient('http://localhost:8020'), + 'robot3': EngineClient('http://localhost:8030') } for engine_client in self.engine_clients.values(): 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)) diff --git a/goal_sender/scripts/utilities.py b/goal_sender/scripts/utilities.py new file mode 100644 index 0000000000000000000000000000000000000000..3bcaf8ff5d8019b40adc9033a548a3e22ad5c285 --- /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)