diff --git a/goal_sender/scripts/goal_sender_node.py b/goal_sender/scripts/Navigation.py similarity index 62% rename from goal_sender/scripts/goal_sender_node.py rename to goal_sender/scripts/Navigation.py index 07c545cff408b092989dd37c5b25edbf28a3acbb..879b2bdf8c7a11ec795d972c0b18518041934406 100755 --- a/goal_sender/scripts/goal_sender_node.py +++ b/goal_sender/scripts/Navigation.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -# goal_sender_node.py +# Navigation.py # # Copyright 2018 Eiichi Inohira # This software may be modified and distributed under the terms @@ -38,8 +38,6 @@ class Command(RoIS_Common.Command): self._component = c def start(self): - print("Sending goal...") - self._component.send_goal() status = RoIS_HRI.ReturnCode_t.OK.value return status @@ -71,46 +69,61 @@ class Event(RoIS_Common.Event): self._component = c self.event_queue = queue.Queue() - def poll_event(self): - msg = self.event_queue.get() - return msg + # def poll_event(self): + # msg = self.event_queue.get() + # return msg - # def person_detected(self, timestamp, number): - # msg = xmlrpc.client.dumps((timestamp, number), 'person_detected') - # self.event_queue.put(msg) +from goal_sender_ros import GoalSenderROS +class Navigation(Event, Command, Query): + def __init__(self, c): + super(Navigation, self).__init__(c) + self._component = c + self._goal_sender = GoalSenderROS() + self._component.Target_Position = [""] + self._component.Time_Limit = 10 + self._component.Routing_Policy = "" -class GoalSender(Event, Command, Query): - pass + def set_parameter(self, target_position, time_limit, routing_policy): + self._goal_sender.send_goal(target_position) + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = self._goal_sender.get_goalsender_state().value + return (status, c_status) -def event_dispatch(gs): - # gs.person_detected(datetime.now().isoformat(), 1) +def event_dispatch(n): + # n.person_detected(datetime.now().isoformat(), 1) time.sleep(0.1) - # gs.person_detected(datetime.now().isoformat(), 1) + # n.person_detected(datetime.now().isoformat(), 1) +class Component(object): + pass + import rospy -from goal_sender_ros import GoalSenderROS -def example_gs(port): +def example_n(port): print("Starting node...") - component = GoalSenderROS() - gs = GoalSender(component) - print("Goalsender constructed") + component = Component() + n = Navigation(component) + print("Navigation constructed") # start the timer to dispatch events - t = threading.Timer(0.1, event_dispatch, args=(gs,)) + t = threading.Timer(0.1, event_dispatch, args=(n,)) t.start() # start the XML-RPC server server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) - server.register_instance(gs) + server.register_instance(n) server.register_introspection_functions() server.register_multicall_functions() server.serve_forever() if __name__ == '__main__': - rospy.init_node('goal_sender_node') - example_gs(8000) + rospy.init_node('navigation_component') + example_n(8001) diff --git a/goal_sender/scripts/goal_sender_client.py b/goal_sender/scripts/Navigation_client.py similarity index 53% rename from goal_sender/scripts/goal_sender_client.py rename to goal_sender/scripts/Navigation_client.py index 239def8481da7048da376865b5010a5cf3048dbf..fc616a489b251b996f217c27261883fc8672c39c 100644 --- a/goal_sender/scripts/goal_sender_client.py +++ b/goal_sender/scripts/Navigation_client.py @@ -1,4 +1,4 @@ -# goal_sender_client.py +# Navigation_Client.py # # Copyright 2018 Eiichi Inohira # This software may be modified and distributed under the terms @@ -7,7 +7,7 @@ # For python3 # For HRI Engine -"""goal_sender_client +"""Navigation_Client """ import threading @@ -40,6 +40,11 @@ class Command(RoIS_Common.Command): status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) return status + def set_parameter(self, target_position, time_limit, routing_policy): + status = self._proxy.set_parameter(target_position, time_limit, routing_policy) + status = RoIS_HRI.ReturnCode_t(status) + return status + class Query(RoIS_Common.Query): """Query @@ -49,8 +54,12 @@ class Query(RoIS_Common.Query): self._proxy = xmlrpc.client.ServerProxy(self._uri) def component_status(self): - (status, c_status) = self._proxy.component_status() + status, c_status = self._proxy.component_status() return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + def get_parameter(self): + (status, target_position, time_limit, routing_policy) = self._proxy.get_parameter() + return (RoIS_HRI.ReturnCode_t(status), target_position, time_limit, routing_policy) class Event(RoIS_Common.Event): @@ -59,46 +68,40 @@ class Event(RoIS_Common.Event): def __init__(self, uri): self._uri = uri self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] # if logger is not None: # self.logger = logger - - def start_th(self): - self.th = threading.Thread(target=self.event_loop) - self.th.start() - - def event_loop(self): - """event_loop - """ - while True: - try: - self.poll_event() - except ConnectionRefusedError: - break - - def poll_event(self): - """poll_event - """ - msg = self._e_proxy.poll_event() - (params, methodname) = xmlrpc.client.loads(msg) - #self.logger.debug('poll_event: '+methodname) - # print(params,methodname) - if methodname == 'person_detected': - self.person_detected(params[0], params[1]) - - def person_detected(self, timestamp, number): - self.events.append((timestamp,number)) - print("person_detected",timestamp,number) - # self.logger.debug('received completed event' - # + command_id - # + RoIS_Service.Completed_Status(status).name) - - -class GoalSenderClient(Command, Query, Event): - """GoalSenderClient + + # def start_th(self): + # self.th = threading.Thread(target=self.event_loop) + # self.th.start() + + # def event_loop(self): + # """event_loop + # """ + # while True: + # try: + # self.poll_event() + # except ConnectionRefusedError: + # break + + # def poll_event(self): + # """poll_event + # """ + # msg = self._e_proxy.poll_event() + # (params, methodname) = xmlrpc.client.loads(msg) + # #self.logger.debug('poll_event: '+methodname) + # print(params,methodname) + # if methodname == 'speech_recognized': + # self.speech_recognized(params[0], params[1]) + + +class Navigation_Client(Command, Query, Event): + """Navigation_Client """ def __init__(self, uri): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] - self.start_th() + # self.start_th() diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py deleted file mode 100644 index 014bce3a81b62566d45ceec767e3d4d98c3c206a..0000000000000000000000000000000000000000 --- a/goal_sender/scripts/goal_sender_ros.py +++ /dev/null @@ -1,41 +0,0 @@ -#/usr/bin/env python - -import rospy -import actionlib -from actionlib_msgs.msg import GoalStatus - -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped - -def pose_to_goal(pose): - goal = MoveBaseGoal() - goal.target_pose.header.frame_id = 'map' - goal.target_pose.pose.position.x = pose[0][0] - goal.target_pose.pose.position.y = pose[0][1] - goal.target_pose.pose.position.z = pose[0][2] - goal.target_pose.pose.orientation.x = pose[1][0] - goal.target_pose.pose.orientation.y = pose[1][1] - goal.target_pose.pose.orientation.z = pose[1][2] - goal.target_pose.pose.orientation.w = pose[1][3] - - return goal - - -class GoalSenderROS(object): - def __init__(self): - self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) - # self.action_client.wait_for_server() - - def send_goal(self): - pose = [(10.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)] - goal = pose_to_goal(pose) - state = self.action_client.send_goal_and_wait(goal) - if state == GoalStatus.SUCCEEDED: - rospy.loginfo("state: SUCCEEDED!!") - elif state == GoalStatus.ABORTED: - rospy.loginfo("state: ABORTED...") - else: - rospy.loginfo("state: {}".format(state)) - -# If you remove this comment out, it does not work. -# rospy.init_node('goal_sender') diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py new file mode 100644 index 0000000000000000000000000000000000000000..e3237a2401fef09da24da67a45a735cc5dbb7794 --- /dev/null +++ b/goal_sender/scripts/route_guidance_engine.py @@ -0,0 +1,226 @@ +# route_guidance_engine.py +# +# Copyright 2017 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python 3 +# For HRI Engine + +"""route_guidance_engine +""" + +from __future__ import print_function + +import os +import sys +import threading + +from pyrois import RoIS_HRI + +if sys.version_info.major == 2: + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass +else: + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + + +class SystemIF(RoIS_HRI.SystemIF): + """SystemIF + """ + def __init__(self, Engine): + self._engine = Engine + + def connect(self): + # print("connect") + if self._engine.state is False: + self._engine.state = True + status = RoIS_HRI.ReturnCode_t.OK.value + else: + status = RoIS_HRI.ReturnCode_t.ERROR.value + # time.sleep(30) + return status + + def disconnect(self): + if self._engine.state is True: + self._engine.state = False + status = RoIS_HRI.ReturnCode_t.OK.value + else: + status = RoIS_HRI.ReturnCode_t.ERROR.value + #status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def get_profile(self, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + # RoIS_HRI_Profile + profile = "Unsupported" + return (status, profile) + + def get_error_detail(self, error_id, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + # ResultLists + results = "None" + return (status, results) + + +class CommandIF(RoIS_HRI.CommandIF): + """CommandIF + """ + def __init__(self, Engine): + self._engine = Engine + + def search(self, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + # List< RoIS_Identifier> + component_ref_list = ["None"] + return (status, component_ref_list) + + def bind(self, component_ref): + # print("state:", self._engine.state) + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def bind_any(self, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + component_ref_list = ["None"] + return (status, component_ref_list) + + def release(self, component_ref): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def get_parameter(self, component_ref): + status = RoIS_HRI.ReturnCode_t.OK.value + parameter_list = ["None"] + return (status, parameter_list) + + def set_parameter(self, component_ref, parameters): + status = RoIS_HRI.ReturnCode_t.OK.value + command_id = "0" + return (status, command_id) + + def execute(self, command_unit_list): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def get_command_result(self, command_id, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + results = ["None"] + return (status, results) + + +class QueryIF(RoIS_HRI.QueryIF): + """ + class QueryIF(object): + """ + + def __init__(self, Engine): + self._engine = Engine + + def query(self, query_type, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + results = ["None"] + return (status, results) + + +class EventIF(RoIS_HRI.EventIF): + """ + class QueryIF(object): + """ + + def __init__(self, Engine): + self._engine = Engine + + def subscribe(self, event_type, condition): + """ + subscribe(self, event_type, condition) -> (status,subscribe_id) + """ + status = RoIS_HRI.ReturnCode_t.OK.value + subscribe_id = "0" + return (status, subscribe_id) + + def unsubscribe(self, subscribe_id): + """ + unsubscribe(self,subscribe_id) -> status + """ + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def get_event_detail(self, event_id, condition): + """ + get_event_detail(self,event_id,condition) -> (status,results) + """ + status = RoIS_HRI.ReturnCode_t.OK.value + results = ["None"] + return (status, results) + + +from Navigation_client import Navigation_Client as NavClient + +class IF(SystemIF, CommandIF, QueryIF, EventIF): + """IF + """ + def __init__(self, Engine): + super().__init__(Engine) + self.compoent_clients = { + 'Navigation': NavClient('http://localhost:8001') + } + + def set_parameter(self, component_ref, parameters): + status = None + if not component_ref in self.compoent_clients: + status = RoIS_HRI.ReturnCode_t.ERROR.value + else: + target_component_client = self.compoent_clients[component_ref] + status = target_component_client.set_parameter(*parameters).value + return (status, "2") + + ### Remove ### + def get_navi_status(self): + status, c_status = self.compoent_clients['Navigation'].component_status() + return (status.value, c_status.value) + ############## + +class IF_server: + """IF_Server + """ + def __init__(self, port): + self._addr = os.getenv("HRIENGINE") + self._server = ThreadingXMLRPCServer( + ("0.0.0.0", port), logRequests=False) + + def run(self, _IF): + """IF_Server + """ + self._server.register_instance(_IF) + self._server.register_introspection_functions() + # print("server running") + self._server.serve_forever() + + +class RouteGuidanceEngine: + """IF_Server + """ + def __init__(self): + self.state = False + + +def test_engine(port): + """test_engine + """ + IF_server(port).run(IF(RouteGuidanceEngine())) + + +if __name__ == "__main__": + test_engine(8000) diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/route_guidance_engine_client.py new file mode 100644 index 0000000000000000000000000000000000000000..73786572ec680498cf7c81cb1a05b2dfe0ff5fc9 --- /dev/null +++ b/goal_sender/scripts/route_guidance_engine_client.py @@ -0,0 +1,142 @@ +# HRI_Engine_client.py +# +# Copyright 2017 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python 3 +# For Service Application + +"""HRI_Engine_client +""" +import time +import xmlrpc.client +# from pyrois import Service_Application_Base_example + +from pyrois import RoIS_HRI, RoIS_Common +# import Service_Application_IF_test + +class SystemIF(RoIS_HRI.SystemIF): + """SystemIF + """ + def __init__(self, proxy_obj): + self._proxy = proxy_obj + + def connect(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.connect()) + return status + + def disconnect(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.disconnect()) + return status + + def get_profile(self, condition): + (s, profile) = self._proxy.get_profile(condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, profile) + + def get_error_detail(self, error_id, condition): + (s, results) = self._proxy.get_error_detail(error_id, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, results) + + +class CommandIF(RoIS_HRI.CommandIF): + """CommandIF + """ + def __init__(self, proxy_obj): + self._proxy = proxy_obj + + def search(self, condition): + (s, component_ref_list) = self._proxy.search(condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, component_ref_list) + + def bind(self, component_ref): + status = RoIS_HRI.ReturnCode_t(self._proxy.bind(component_ref)) + return status + + def bind_any(self, condition): + (s, component_ref_list) = self._proxy.search(condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, component_ref_list) + + def release(self, component_ref): + status = RoIS_HRI.ReturnCode_t(self._proxy.release(component_ref)) + return status + + def get_parameter(self, component_ref): + (s, parameter_list) = self._proxy.get_parameter(component_ref) + 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) + status = RoIS_HRI.ReturnCode_t(status) + return (status, command_id) + + def execute(self, command_unit_list): + s = self._proxy.execute(command_unit_list) + status = RoIS_HRI.ReturnCode_t(s) + return status + + def get_command_result(self, command_id, condition): + (s, results) = self._proxy.get_command_result(command_id, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, results) + + +class QueryIF(RoIS_HRI.QueryIF): + """QueryIF + """ + def __init__(self, proxy_obj): + self._proxy = proxy_obj + + def query(self, query_type, condition): + (s, results) = self._proxy.query(query_type, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, results) + + +class EventIF(RoIS_HRI.EventIF): + """EventIF + """ + def __init__(self, proxy_obj): + self._proxy = proxy_obj + + def subscribe(self, event_type, condition): + (s, subscribe_id) = self._proxy.subscribe(event_type, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, subscribe_id) + + def unsubscribe(self, subscribe_id): + s = self._proxy.unsubscribe(subscribe_id) + status = RoIS_HRI.ReturnCode_t(s) + return status + + def get_event_detail(self, event_id, condition): + (s, results) = self._proxy.get_event_detail(event_id, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, results) + + +# class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF_test.Service_Application_IF): +class IF(SystemIF, CommandIF, QueryIF, EventIF): + """IF + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + # self.a = Service_Application_Base_example.Service_Application_Base() + # Service_Application_IF_test.Service_Application_IF.__init__(self,uri) + + def analysis_c_status(self): + s = self._proxy.analysis_c_status() + status = RoIS_HRI.ReturnCode_t(s) + return status + + ### Remove ### + 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) + ############## diff --git a/goal_sender/scripts/test.py b/goal_sender/scripts/test.py deleted file mode 100644 index 71b907528691e7b08dda3381c1212c3641a7e6ba..0000000000000000000000000000000000000000 --- a/goal_sender/scripts/test.py +++ /dev/null @@ -1,8 +0,0 @@ -import time -from goal_sender_client import GoalSenderClient - -uri = "http://localhost:8000" -client = GoalSenderClient(uri) - -time.sleep(3) -print(client.start()) diff --git a/goal_sender/scripts/test_component.py b/goal_sender/scripts/test_component.py new file mode 100644 index 0000000000000000000000000000000000000000..73fdab2689dd6c5dc9e0f693649818ba57619401 --- /dev/null +++ b/goal_sender/scripts/test_component.py @@ -0,0 +1,9 @@ +import time +from Navigation_client import Navigation_Client + +uri = "http://localhost:8001" +client = Navigation_Client(uri) + +print(client.start()) +time.sleep(3) +print(client.set_parameter("ahi", "chuni", "dwa")) diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py new file mode 100644 index 0000000000000000000000000000000000000000..006b76c90de2c4dc538cb73e9ae699af283ca7ee --- /dev/null +++ b/goal_sender/scripts/test_engine.py @@ -0,0 +1,30 @@ +# 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:8000' +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")